Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include <cassert>
17 : #include <string.h> // Required for memchr, memmove
18 : #include "AP_ExternalAHRS_SensAItion_Parser.h"
19 : #include <AP_GPS/GPS_Backend.h>
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <AP_HAL/utility/sparse-endian.h>
22 : #include <GCS_MAVLink/GCS.h>
23 :
24 : // Constructor
25 38 : AP_ExternalAHRS_SensAItion_Parser::AP_ExternalAHRS_SensAItion_Parser(ConfigMode mode) :
26 38 : config_mode(mode)
27 : {
28 38 : reset_parser();
29 38 : }
30 :
31 60775 : size_t AP_ExternalAHRS_SensAItion_Parser::parse_stream(const uint8_t* data, size_t data_size, Measurement& meas)
32 : {
33 2338789 : for (size_t i = 0; i < data_size; i++) {
34 2338541 : if (parse_single_byte(data[i])) {
35 60527 : decode_packet(meas);
36 60527 : reset_parser(); // Ready for more packets in same buffer
37 60527 : return i + 1; // No of bytes parsed
38 : }
39 : }
40 :
41 248 : meas = Measurement(); // Uninitialized
42 248 : return data_size;
43 : }
44 :
45 60569 : void AP_ExternalAHRS_SensAItion_Parser::reset_parser()
46 : {
47 60569 : parse_state = ParseState::WAITING_HEADER;
48 60569 : packet_buffer_len = 0;
49 60569 : target_payload_len = 0;
50 60569 : current_packet_id = PacketID::UNKNOWN;
51 60569 : }
52 :
53 : // Error Handler
54 6 : void AP_ExternalAHRS_SensAItion_Parser::handle_invalid_packet()
55 : {
56 : // Fallback if called with empty buffer, which should never happen
57 6 : if (packet_buffer_len == 0) {
58 0 : return;
59 : }
60 :
61 : // Look for a new header byte starting from index 1 to resync
62 6 : uint8_t *p = (uint8_t *)memchr(&packet_buffer[1], HEADER_BYTE, packet_buffer_len - 1);
63 :
64 6 : if (p == nullptr) {
65 : // No header found, reset completely
66 4 : reset_parser();
67 4 : return;
68 : }
69 :
70 2 : const size_t bytes_to_discard = p - packet_buffer;
71 2 : const size_t bytes_to_keep = packet_buffer_len - bytes_to_discard;
72 :
73 2 : memmove(&packet_buffer[0], p, bytes_to_keep);
74 2 : packet_buffer_len = bytes_to_keep;
75 :
76 : // Determine State based on Mode and remaining data
77 2 : if (config_mode == ConfigMode::INTERLEAVED_INS) {
78 0 : if (packet_buffer_len >= 2) {
79 : // We have Header + Potential ID. Process it immediately.
80 0 : uint8_t id = packet_buffer[1];
81 0 : switch (static_cast<PacketID>(id)) {
82 0 : case PacketID::IMU:
83 0 : target_payload_len = PAYLOAD_SIZE_IMU;
84 0 : parse_state = ParseState::COLLECTING_PAYLOAD;
85 0 : current_packet_id = PacketID::IMU;
86 0 : break;
87 0 : case PacketID::AHRS:
88 0 : target_payload_len = PAYLOAD_SIZE_QUAT;
89 0 : parse_state = ParseState::COLLECTING_PAYLOAD;
90 0 : current_packet_id = PacketID::AHRS;
91 0 : break;
92 0 : case PacketID::INS:
93 0 : target_payload_len = PAYLOAD_SIZE_INS;
94 0 : parse_state = ParseState::COLLECTING_PAYLOAD;
95 0 : current_packet_id = PacketID::INS;
96 0 : break;
97 0 : default:
98 : // The "New" ID is also bad. Drop header and retry.
99 0 : reset_parser();
100 0 : return;
101 : }
102 : } else {
103 0 : parse_state = ParseState::WAITING_ID;
104 : }
105 : } else {
106 : // Legacy Mode
107 2 : target_payload_len = PAYLOAD_SIZE_IMU;
108 2 : parse_state = ParseState::COLLECTING_PAYLOAD;
109 : }
110 : }
111 :
112 : // Checksum Validator & Deep Debugger
113 60531 : bool AP_ExternalAHRS_SensAItion_Parser::buffer_contains_valid_packet() const
114 : {
115 60531 : if (packet_buffer_len < 2 || packet_buffer[0] != HEADER_BYTE) {
116 0 : return false;
117 : }
118 :
119 60531 : const uint8_t calculated = crc_xor_of_bytes(&packet_buffer[1], packet_buffer_len - 2);
120 60531 : const uint8_t received = packet_buffer[packet_buffer_len - 1];
121 :
122 60531 : if (calculated != received) {
123 4 : return false;
124 : }
125 :
126 60527 : return true;
127 : }
128 :
129 : // The Core State Machine
130 2338541 : bool AP_ExternalAHRS_SensAItion_Parser::parse_single_byte(uint8_t byte)
131 : {
132 : // Safety: Prevent buffer overflow
133 2338541 : if (packet_buffer_len >= MAX_PACKET_SIZE) {
134 0 : handle_invalid_packet();
135 0 : if (packet_buffer_len >= MAX_PACKET_SIZE) {
136 0 : reset_parser();
137 : }
138 : }
139 :
140 2338541 : switch (parse_state) {
141 60577 : case ParseState::WAITING_HEADER:
142 60577 : if (byte == HEADER_BYTE) {
143 60533 : packet_buffer_len = 0;
144 60533 : packet_buffer[packet_buffer_len++] = byte;
145 :
146 60533 : if (config_mode == ConfigMode::INTERLEAVED_INS) {
147 23695 : parse_state = ParseState::WAITING_ID;
148 : } else {
149 36838 : target_payload_len = PAYLOAD_SIZE_IMU;
150 36838 : parse_state = ParseState::COLLECTING_PAYLOAD;
151 : }
152 : }
153 60577 : break;
154 :
155 23695 : case ParseState::WAITING_ID:
156 23695 : packet_buffer[packet_buffer_len++] = byte;
157 :
158 : switch (static_cast<PacketID>(byte)) {
159 21520 : case PacketID::IMU:
160 21520 : target_payload_len = PAYLOAD_SIZE_IMU;
161 21520 : current_packet_id = PacketID::IMU;
162 21520 : parse_state = ParseState::COLLECTING_PAYLOAD;
163 21520 : break;
164 :
165 1077 : case PacketID::AHRS:
166 1077 : target_payload_len = PAYLOAD_SIZE_QUAT;
167 1077 : current_packet_id = PacketID::AHRS;
168 1077 : parse_state = ParseState::COLLECTING_PAYLOAD;
169 1077 : break;
170 :
171 1096 : case PacketID::INS:
172 1096 : target_payload_len = PAYLOAD_SIZE_INS;
173 1096 : current_packet_id = PacketID::INS;
174 1096 : parse_state = ParseState::COLLECTING_PAYLOAD;
175 1096 : break;
176 :
177 2 : default:
178 2 : parse_errors++;
179 2 : handle_invalid_packet();
180 2 : break;
181 : }
182 23695 : break;
183 :
184 2254269 : case ParseState::COLLECTING_PAYLOAD:
185 2254269 : packet_buffer[packet_buffer_len++] = byte;
186 :
187 : // Calculate Expected Total Length
188 : // Legacy: Header(1) + Payload(N) + CRC(1)
189 : // Interleaved: Header(1) + ID(1) + Payload(N) + CRC(1)
190 2254269 : const size_t overhead = (config_mode == ConfigMode::INTERLEAVED_INS) ? 3 : 2;
191 2254269 : const size_t expected_total_len = target_payload_len + overhead;
192 :
193 2254269 : if (packet_buffer_len >= expected_total_len) {
194 60531 : if (buffer_contains_valid_packet()) {
195 60527 : valid_packets++;
196 60527 : return true;
197 : }
198 :
199 4 : parse_errors++;
200 4 : handle_invalid_packet();
201 4 : return false;
202 : }
203 2193738 : break;
204 : }
205 :
206 2278010 : return false;
207 : }
208 :
209 0 : bool AP_ExternalAHRS_SensAItion_Parser::validate_checksum() const
210 : {
211 0 : return buffer_contains_valid_packet();
212 : }
213 :
214 : // Router
215 60527 : void AP_ExternalAHRS_SensAItion_Parser::decode_packet(Measurement& measurement)
216 : {
217 60527 : const uint8_t* payload = nullptr;
218 :
219 60527 : if (config_mode == ConfigMode::INTERLEAVED_INS) {
220 23693 : payload = &packet_buffer[2]; // Skip Header + ID
221 23693 : switch (current_packet_id) {
222 21520 : case PacketID::IMU:
223 21520 : decode_imu(payload, measurement);
224 21520 : break;
225 1077 : case PacketID::AHRS:
226 1077 : decode_ahrs(payload, measurement);
227 1077 : break;
228 1096 : case PacketID::INS:
229 1096 : decode_ins(payload, measurement);
230 1096 : break;
231 0 : case PacketID::UNKNOWN:
232 0 : assert(false && "decode_packet() should only be called with a valid packet ID!");
233 : break;
234 : }
235 : } else {
236 36834 : payload = &packet_buffer[1]; // Skip Header
237 36834 : decode_imu(payload, measurement);
238 : }
239 60527 : }
240 :
241 : // IMU Decoder (Packet 0)
242 58354 : void AP_ExternalAHRS_SensAItion_Parser::decode_imu(const uint8_t* payload, Measurement& measurement)
243 : {
244 : // ... [Code omitted: No changes to decoders, use your existing implementation] ...
245 : // Accel (Bytes 0-11): 3 x Int32 (ug)
246 58354 : const int32_t accel_x_ug = be32toh_ptr(&payload[0]);
247 58354 : const int32_t accel_y_ug = be32toh_ptr(&payload[4]);
248 58354 : const int32_t accel_z_ug = be32toh_ptr(&payload[8]);
249 :
250 : // Gyro (Bytes 12-23): 3 x Int32 (udeg/s)
251 58354 : const int32_t gyro_x_udegs = be32toh_ptr(&payload[12]);
252 58354 : const int32_t gyro_y_udegs = be32toh_ptr(&payload[16]);
253 58354 : const int32_t gyro_z_udegs = be32toh_ptr(&payload[20]);
254 :
255 : // Temp (Bytes 24-25): 1 x Int16 (Scaled)
256 58354 : const int16_t temp_raw = be16toh_ptr(&payload[24]);
257 :
258 : // Mag (Bytes 26-31): 3 x Int16 (mGauss)
259 58354 : const int16_t mag_x_mgauss = be16toh_ptr(&payload[26]);
260 58354 : const int16_t mag_y_mgauss = be16toh_ptr(&payload[28]);
261 58354 : const int16_t mag_z_mgauss = be16toh_ptr(&payload[30]);
262 :
263 : // Baro (Bytes 32-35): 1 x Int32 (0.1 Pa)
264 58354 : const int32_t baro_raw = be32toh_ptr(&payload[32]);
265 :
266 : // Accel: ug -> m/s^2 (Note: 1,000,000 ug = 9.81 m/s^2 roughly)
267 58354 : const float ug_to_mss = 1.0e-6f * GRAVITY_MSS;
268 58354 : measurement.acceleration_mss = Vector3f(accel_x_ug, accel_y_ug, accel_z_ug) * ug_to_mss;
269 :
270 : // Gyro: udeg/s -> rad/s (Note: 1,000,000 udeg/s = 1 deg/s = 0.017 rad/s)
271 58354 : const float udeg_to_rad = 1.0e-6f * DEG_TO_RAD;
272 58354 : measurement.angular_velocity_rads = Vector3f(gyro_x_udegs, gyro_y_udegs, gyro_z_udegs) * udeg_to_rad;
273 :
274 : // Temp: (Raw * 0.008) + 20
275 58354 : measurement.temperature_degc = ((float)temp_raw * 0.008f) + 20.0f;
276 :
277 : // Mag: mGauss (Pass-through, ArduPilot expects mGauss)
278 58354 : measurement.magnetic_field_mgauss = Vector3f(mag_x_mgauss, mag_y_mgauss, mag_z_mgauss);
279 :
280 : // Baro: 0.1 Pa -> Pascal
281 58354 : measurement.air_pressure_p = (float)baro_raw * 0.1f;
282 :
283 : // Metadata
284 58354 : measurement.type = MeasurementType::IMU;
285 58354 : measurement.timestamp_us = AP_HAL::micros64();
286 58354 : }
287 :
288 1077 : void AP_ExternalAHRS_SensAItion_Parser::decode_ahrs(const uint8_t* payload, Measurement& measurement)
289 : {
290 : // Payload layout: W (0-3), X (4-7), Y (8-11), Z (12-15)
291 1077 : const int32_t quat_w_raw = be32toh_ptr(&payload[0]);
292 1077 : const int32_t quat_x_raw = be32toh_ptr(&payload[4]);
293 1077 : const int32_t quat_y_raw = be32toh_ptr(&payload[8]);
294 1077 : const int32_t quat_z_raw = be32toh_ptr(&payload[12]);
295 :
296 1077 : const float scale_factor = 1.0e-6f;
297 :
298 2154 : measurement.orientation = Quaternion(
299 1077 : (float)quat_w_raw * scale_factor,
300 1077 : (float)quat_x_raw * scale_factor,
301 1077 : (float)quat_y_raw * scale_factor,
302 1077 : (float)quat_z_raw * scale_factor
303 : );
304 :
305 1077 : measurement.type = MeasurementType::AHRS;
306 1077 : measurement.timestamp_us = AP_HAL::micros64();
307 1077 : }
308 :
309 1096 : void AP_ExternalAHRS_SensAItion_Parser::decode_ins(const uint8_t* payload, Measurement& measurement)
310 : {
311 : // --- 1. PARSE RAW BYTES (Big-Endian) ---
312 :
313 : // 0-3: Num Sats (G2, G1)
314 1096 : measurement.num_sats_gnss2 = payload[1];
315 1096 : measurement.num_sats_gnss1 = payload[3];
316 :
317 : // 4-7: Error Flags
318 1096 : measurement.error_flags = be32toh_ptr(&payload[4]);
319 :
320 : // 8: Sensor Valid
321 1096 : measurement.sensor_valid = payload[8];
322 :
323 : // 9-16: Lat/Lon (1e-7 deg)
324 1096 : const int32_t lat_raw = be32toh_ptr(&payload[9]);
325 1096 : const int32_t lon_raw = be32toh_ptr(&payload[13]);
326 :
327 : // 17-28: Velocity N, E, D (mm/s)
328 1096 : const int32_t vel_n_mm = be32toh_ptr(&payload[17]);
329 1096 : const int32_t vel_e_mm = be32toh_ptr(&payload[21]);
330 1096 : const int32_t vel_d_mm = be32toh_ptr(&payload[25]);
331 :
332 : // 29-32: Altitude relative to WGS 84 ellipsoid (mm)
333 1096 : const int32_t alt_raw_mm = be32toh_ptr(&payload[29]);
334 :
335 : // 33: Alignment Status
336 1096 : measurement.alignment_status = payload[33];
337 :
338 : // 34-37: Time of week (ms)
339 1096 : measurement.time_itow_ms = be32toh_ptr(&payload[34]);
340 :
341 : // 38-39: GNSS Fix
342 1096 : measurement.gnss2_fix = payload[38];
343 1096 : measurement.gnss1_fix = payload[39];
344 :
345 : // 40-44: UTC Date/Time
346 1096 : const uint16_t year = (uint16_t)((payload[40]<<8) | payload[41]);
347 1096 : const uint16_t month = (uint16_t)((payload[42]<<8) | payload[43]);
348 1096 : const uint8_t day = payload[44];
349 :
350 : // 45-68: Accuracy Metrics (mm or mm/s)
351 1096 : const int32_t acc_lat_mm = be32toh_ptr(&payload[45]);
352 1096 : const int32_t acc_lon_mm = be32toh_ptr(&payload[49]);
353 1096 : const int32_t acc_vn_mm = be32toh_ptr(&payload[53]);
354 1096 : const int32_t acc_ve_mm = be32toh_ptr(&payload[57]);
355 1096 : const int32_t acc_vd_mm = be32toh_ptr(&payload[61]);
356 1096 : const int32_t acc_vd_pos_mm = be32toh_ptr(&payload[65]);
357 :
358 : // --- 2. POPULATE & CONVERT ---
359 1096 : const float mm_to_cm = 0.1f;
360 1096 : measurement.location = Location(lat_raw, lon_raw, alt_raw_mm * mm_to_cm, Location::AltFrame::ABSOLUTE);
361 :
362 1096 : const float mms_to_ms = 0.001f;
363 1096 : const float mm_to_m = 0.001f;
364 :
365 1096 : measurement.velocity_ned.x = (float)vel_n_mm * mms_to_ms;
366 1096 : measurement.velocity_ned.y = (float)vel_e_mm * mms_to_ms;
367 1096 : measurement.velocity_ned.z = (float)vel_d_mm * mms_to_ms;
368 :
369 : // Accuracy
370 1096 : measurement.pos_accuracy.x = (float)acc_lat_mm * mm_to_m; // Horizontal Latitude
371 1096 : measurement.pos_accuracy.y = (float)acc_lon_mm * mm_to_m; // Horizontal Longitude
372 1096 : measurement.pos_accuracy.z = (float)acc_vd_pos_mm * mm_to_m; // Vertical
373 :
374 1096 : measurement.vel_accuracy.x = (float)acc_vn_mm * mms_to_ms;
375 1096 : measurement.vel_accuracy.y = (float)acc_ve_mm * mms_to_ms;
376 1096 : measurement.vel_accuracy.z = (float)acc_vd_mm * mms_to_ms;
377 :
378 : // GPS Week Calculation (Gate check)
379 1096 : if (measurement.gnss1_fix >= 2) {
380 1094 : measurement.gps_week = calculate_gps_week(year, month, day);
381 : } else {
382 2 : measurement.gps_week = 0;
383 : }
384 :
385 1096 : measurement.type = MeasurementType::INS;
386 1096 : measurement.timestamp_us = AP_HAL::micros64();
387 :
388 1096 : }
389 :
390 1094 : uint16_t AP_ExternalAHRS_SensAItion_Parser::calculate_gps_week(uint16_t year, uint8_t month, uint8_t day)
391 : {
392 : // Convert to BCD form (DDMMYY) - the GPS_Backend function assumes year is 20YY
393 1094 : const uint32_t bcd_date = year % 100U + month * 100U + day * 10000U;
394 :
395 1094 : const uint32_t bcd_time_ms = 0U;
396 : uint16_t gps_week;
397 : uint32_t gps_time_ms;
398 1094 : AP_GPS_Backend::BCD_to_gps_time(bcd_date, bcd_time_ms, gps_week, gps_time_ms);
399 :
400 1094 : return gps_week;
401 : }
|
