Line data Source code
1 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
2 :
3 : #include "AP_GSOF_config.h"
4 :
5 : #if AP_GSOF_ENABLED
6 :
7 : #include <AP_Logger/AP_Logger.h>
8 : #include <AP_HAL/utility/sparse-endian.h>
9 : #include <GCS_MAVLink/GCS.h>
10 : #include <AP_GSOF/AP_GSOF.h>
11 :
12 : #define gsof_DEBUGGING 0
13 : #if gsof_DEBUGGING
14 : extern const AP_HAL::HAL& hal;
15 : # define Debug(fmt, args ...) \
16 : do { \
17 : hal.console->printf("%s:%d: " fmt "\n", \
18 : __FUNCTION__, __LINE__, \
19 : ## args); \
20 : hal.scheduler->delay(1); \
21 : } while(0)
22 : #else
23 : # define Debug(fmt, args ...)
24 : #endif
25 :
26 : int
27 7759 : AP_GSOF::parse(const uint8_t temp, MsgTypes& parsed_msgs)
28 : {
29 : // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html
30 7759 : switch (msg.state) {
31 501 : default:
32 : case Msg_Parser::State::STARTTX:
33 501 : if (temp == STX) {
34 66 : msg.state = Msg_Parser::State::STATUS;
35 66 : msg.read = 0;
36 66 : msg.checksumcalc = 0;
37 : }
38 501 : break;
39 66 : case Msg_Parser::State::STATUS:
40 66 : msg.status = temp;
41 66 : msg.state = Msg_Parser::State::PACKETTYPE;
42 66 : msg.checksumcalc += temp;
43 66 : break;
44 66 : case Msg_Parser::State::PACKETTYPE:
45 66 : msg.packettype = temp;
46 66 : msg.state = Msg_Parser::State::LENGTH;
47 66 : msg.checksumcalc += temp;
48 66 : break;
49 66 : case Msg_Parser::State::LENGTH:
50 66 : msg.length = temp;
51 66 : msg.state = Msg_Parser::State::DATA;
52 66 : msg.checksumcalc += temp;
53 66 : break;
54 6928 : case Msg_Parser::State::DATA:
55 6928 : msg.data[msg.read] = temp;
56 6928 : msg.read++;
57 6928 : msg.checksumcalc += temp;
58 6928 : if (msg.read >= msg.length) {
59 66 : msg.state = Msg_Parser::State::CHECKSUM;
60 : }
61 6928 : break;
62 66 : case Msg_Parser::State::CHECKSUM:
63 66 : msg.checksum = temp;
64 66 : msg.state = Msg_Parser::State::ENDTX;
65 66 : if (msg.checksum == msg.checksumcalc) {
66 66 : if (!process_message(parsed_msgs)) {
67 0 : return NO_GSOF_DATA;
68 : }
69 66 : return PARSED_GSOF_DATA;
70 : }
71 0 : break;
72 66 : case Msg_Parser::State::ENDTX:
73 66 : msg.endtx = temp;
74 66 : msg.state = Msg_Parser::State::STARTTX;
75 66 : break;
76 : }
77 :
78 7693 : return NO_GSOF_DATA;
79 : }
80 :
81 : bool
82 66 : AP_GSOF::process_message(MsgTypes& parsed_msgs)
83 : {
84 66 : if (msg.packettype == 0x40) { // GSOF
85 : // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
86 :
87 372 : for (uint32_t a = 3; a < msg.length; a++) {
88 306 : const uint8_t output_type = msg.data[a];
89 :
90 306 : if (output_type >= parsed_msgs.size()) {
91 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
92 0 : AP_HAL::panic("Invalid output type.");
93 : #else
94 0 : return false;
95 : #endif
96 : }
97 :
98 306 : parsed_msgs.set(output_type);
99 306 : a++;
100 306 : const uint8_t output_length = msg.data[a];
101 306 : a++;
102 : // TODO handle corruption on output_length causing buffer overrun?
103 :
104 306 : switch (output_type) {
105 62 : case POS_TIME:
106 62 : parse_pos_time(a);
107 62 : break;
108 61 : case POS:
109 61 : parse_pos(a);
110 61 : break;
111 64 : case VEL:
112 64 : parse_vel(a);
113 64 : break;
114 60 : case DOP:
115 60 : parse_dop(a);
116 60 : break;
117 59 : case POS_SIGMA:
118 59 : parse_pos_sigma(a);
119 59 : break;
120 0 : case INS_FULL_NAV:
121 0 : parse_ins_full_nav(a);
122 0 : break;
123 0 : case INS_RMS:
124 0 : parse_ins_rms(a);
125 0 : break;
126 0 : case LLH_MSL:
127 0 : parse_llh_msl(a);
128 0 : break;
129 0 : default:
130 : // TODO log warning unparsed packet.
131 0 : break;
132 : }
133 :
134 306 : a += output_length - 1u;
135 : }
136 :
137 66 : return true;
138 : }
139 :
140 : // No GSOF packets.
141 0 : return false;
142 : }
143 :
144 62 : void AP_GSOF::parse_pos_time(uint32_t a)
145 : {
146 : // https://receiverhelp.trimble.com/oem-gnss/gsof-messages-time.html
147 62 : pos_time.time_week_ms = be32toh_ptr(msg.data + a);
148 62 : pos_time.time_week = be32toh_ptr(msg.data + a + 4);
149 62 : pos_time.num_sats = msg.data[a + 6];
150 62 : pos_time.pos_flags1 = msg.data[a + 7];
151 62 : pos_time.pos_flags2 = msg.data[a + 8];
152 : #if HAL_LOGGING_ENABLED
153 62 : log_pos_time();
154 : #endif // HAL_LOGGING_ENABLED
155 62 : }
156 :
157 61 : void AP_GSOF::parse_pos(uint32_t a)
158 : {
159 : // https://receiverhelp.trimble.com/oem-gnss/gsof-messages-llh.html
160 61 : position.latitude_rad = be64todouble_ptr(msg.data, a);
161 61 : position.longitude_rad = be64todouble_ptr(msg.data, a + 8);
162 : // Altitude is "Height from WGS-84 datum" -> Likely ellipsoid
163 61 : position.altitude = be64todouble_ptr(msg.data, a + 16);
164 61 : }
165 :
166 64 : void AP_GSOF::parse_vel(uint32_t a)
167 : {
168 : // https://receiverhelp.trimble.com/oem-gnss/gsof-messages-velocity.html
169 :
170 64 : vel.velocity_flags = msg.data[a];
171 64 : constexpr uint8_t BIT_VELOCITY_VALID = 0;
172 64 : if (BIT_IS_SET(vel.velocity_flags, BIT_VELOCITY_VALID)) {
173 62 : vel.horizontal_velocity = be32tofloat_ptr(msg.data, a + 1);
174 62 : vel.vertical_velocity = be32tofloat_ptr(msg.data, a + 9);
175 : }
176 :
177 64 : constexpr uint8_t BIT_HEADING_VALID = 2;
178 64 : if (BIT_IS_SET(vel.velocity_flags, BIT_HEADING_VALID)) {
179 62 : vel.heading = be32tofloat_ptr(msg.data, a + 5);
180 : }
181 64 : }
182 :
183 60 : void AP_GSOF::parse_dop(uint32_t a)
184 : {
185 : // https://receiverhelp.trimble.com/oem-gnss/gsof-messages-pdop.html
186 : // Skip pdop.
187 60 : dop.hdop = be32tofloat_ptr(msg.data, a + 4);
188 60 : }
189 :
190 59 : void AP_GSOF::parse_pos_sigma(uint32_t a)
191 : {
192 : // https://receiverhelp.trimble.com/oem-gnss/gsof-messages-sigma.html
193 : // Skip pos_rms
194 59 : pos_sigma.sigma_east = be32tofloat_ptr(msg.data, a + 4);
195 59 : pos_sigma.sigma_north = be32tofloat_ptr(msg.data, a + 8);
196 59 : pos_sigma.sigma_up = be32tofloat_ptr(msg.data, a + 16);
197 59 : }
198 :
199 0 : void AP_GSOF::parse_ins_full_nav(uint32_t a)
200 : {
201 : // https://receiverhelp.trimble.com/oem-gnss/gsof-messages-ins-full-nav.html
202 0 : ins_full_nav.gps_week = be16toh_ptr(msg.data + a);
203 0 : ins_full_nav.gps_time_ms = be32toh_ptr(msg.data + a + 2);
204 0 : ins_full_nav.imu_alignment_status = ImuAlignmentStatus(msg.data[a + 6]);
205 0 : ins_full_nav.gnss_status = GnssStatus(msg.data[a + 7]);
206 0 : ins_full_nav.latitude = be64todouble_ptr(msg.data, a + 8);
207 0 : ins_full_nav.longitude = be64todouble_ptr(msg.data, a + 16);
208 0 : ins_full_nav.altitude = be64todouble_ptr(msg.data, a + 24);
209 0 : ins_full_nav.vel_n = be32tofloat_ptr(msg.data, a + 32);
210 0 : ins_full_nav.vel_e = be32tofloat_ptr(msg.data, a + 36);
211 0 : ins_full_nav.vel_d = be32tofloat_ptr(msg.data, a + 40);
212 0 : ins_full_nav.speed = be32tofloat_ptr(msg.data, a + 44);
213 0 : ins_full_nav.roll_deg = be64todouble_ptr(msg.data, a + 48);
214 0 : ins_full_nav.pitch_deg = be64todouble_ptr(msg.data, a + 56);
215 0 : ins_full_nav.heading_deg = be64todouble_ptr(msg.data, a + 64);
216 0 : ins_full_nav.track_angle_deg = be64todouble_ptr(msg.data, a + 72);
217 0 : ins_full_nav.ang_rate_x = be32tofloat_ptr(msg.data, a + 80);
218 0 : ins_full_nav.ang_rate_y = be32tofloat_ptr(msg.data, a + 84);
219 0 : ins_full_nav.ang_rate_z = be32tofloat_ptr(msg.data, a + 88);
220 0 : ins_full_nav.acc_x = be32tofloat_ptr(msg.data, a + 92);
221 0 : ins_full_nav.acc_y = be32tofloat_ptr(msg.data, a + 96);
222 0 : ins_full_nav.acc_z = be32tofloat_ptr(msg.data, a + 100);
223 :
224 : #if HAL_LOGGING_ENABLED
225 0 : log_ins_full_nav();
226 : #endif // HAL_LOGGING_ENABLED
227 0 : }
228 :
229 0 : void AP_GSOF::parse_ins_rms(uint32_t a)
230 : {
231 : // https://receiverhelp.trimble.com/oem-gnss/gsof-messages-ins-rms.html
232 0 : ins_rms.gps_week = be16toh_ptr(msg.data + a);
233 0 : ins_rms.gps_time_ms = be32toh_ptr(msg.data + a + 2);
234 0 : ins_rms.imu_alignment_status = ImuAlignmentStatus(msg.data[a + 6]);
235 0 : ins_rms.gnss_status = GnssStatus(msg.data[a + 7]);
236 0 : ins_rms.pos_rms_n = be32tofloat_ptr(msg.data, a + 8);
237 0 : ins_rms.pos_rms_e = be32tofloat_ptr(msg.data, a + 12);
238 0 : ins_rms.pos_rms_d = be32tofloat_ptr(msg.data, a + 16);
239 0 : ins_rms.vel_rms_n = be32tofloat_ptr(msg.data, a + 20);
240 0 : ins_rms.vel_rms_e = be32tofloat_ptr(msg.data, a + 24);
241 0 : ins_rms.vel_rms_d = be32tofloat_ptr(msg.data, a + 28);
242 0 : ins_rms.roll_rms_deg = be32tofloat_ptr(msg.data, a + 32);
243 0 : ins_rms.pitch_rms_deg = be32tofloat_ptr(msg.data, a + 36);
244 0 : ins_rms.yaw_rms_deg = be32tofloat_ptr(msg.data, a + 40);
245 :
246 : #if HAL_LOGGING_ENABLED
247 0 : log_ins_rms();
248 : #endif // HAL_LOGGING_ENABLED
249 0 : }
250 :
251 0 : void AP_GSOF::parse_llh_msl(uint32_t a)
252 : {
253 : // https://receiverhelp.trimble.com/oem-gnss/gsof-messages-llmsl.html
254 0 : llh_msl.latitude = RAD_TO_DEG_DOUBLE * be64todouble_ptr(msg.data, a);
255 0 : llh_msl.longitude = RAD_TO_DEG_DOUBLE * be64todouble_ptr(msg.data, a + 8);
256 0 : llh_msl.altitude_msl = be64todouble_ptr(msg.data, a + 16);
257 : // Assume the model is EGM96 in ArduPilot, but log for BIN analysis.
258 0 : memcpy(llh_msl.model, msg.data + a + 24, sizeof(llh_msl.model));
259 0 : llh_msl.model[sizeof(llh_msl.model) - 1] = '\0'; // ensure null-termination
260 :
261 : #if HAL_LOGGING_ENABLED
262 0 : log_llh_msl();
263 : #endif // HAL_LOGGING_ENABLED
264 0 : }
265 :
266 : #if HAL_LOGGING_ENABLED
267 62 : void AP_GSOF::log_pos_time() const
268 : {
269 :
270 : // @LoggerMessage: GSPT
271 : // @Description: GSOF1 GPS Position Time Metadata
272 : // @Field: TimeUS: GPS Time since system startup [us]
273 : // @Field: TOWms: GPS Time of week [ms]
274 : // @Field: Week: GPS week number
275 : // @Field: Sats: Number of satellites
276 : // @Field: Flags1: Positioning flags byte 1
277 : // @Field: Flags2: Positioning flags byte 2
278 : // @Field: InitNum: Initialization count
279 124 : AP::logger().WriteStreaming(
280 : "GSPT",
281 : "TimeUS,TOWms,Week,Sats,Flags1,Flags2,InitNum",
282 : "s" "s" "-" "S" "-" "-" "-",
283 : "F" "C" "0" "0" "0" "0" "0",
284 : "Q" "I" "H" "B" "B" "B" "B",
285 : AP_HAL::micros64(),
286 62 : pos_time.time_week_ms,
287 62 : pos_time.time_week,
288 62 : pos_time.num_sats,
289 62 : pos_time.pos_flags1,
290 62 : pos_time.pos_flags2,
291 62 : pos_time.initialized_number
292 : );
293 62 : }
294 :
295 0 : void AP_GSOF::log_ins_full_nav() const
296 : {
297 : // @LoggerMessage: GSN1
298 : // @Description: GSOF49 Full INS Navigation Solution 1
299 : // @Field: TimeUS: Time since system startup [us]
300 : // @Field: GpsWk: GPS week number since Jan 1980
301 : // @Field: GpsTs: GPS time of week [ms]
302 : // @Field: ImuStat: IMU alignment status (enum)
303 : // @Field: GStat: GNSS status (enum)
304 : // @Field: Lat: Latitude [degrees]
305 : // @Field: Lng: Longitude [degrees]
306 : // @Field: Alt: Altitude in ITRF 2020 [meters]
307 : // @Field: VN: Velocity North [m/s]
308 : // @Field: VE: Velocity East [m/s]
309 : // @Field: VD: Velocity Down [m/s]
310 : // @Field: Spd: 3D Speed [m/s]
311 0 : AP::logger().WriteStreaming(
312 : "GSN1",
313 : "TimeUS,GpsWk,GpsTs,ImuStat,GStat,Lat,Lng,Alt,VN," "VE," "VD," "Spd",
314 : "s" "-" "s" "-" "-" "D" "U" "m" "n" "n" "n" "n",
315 : "F" "0" "C" "0" "0" "0" "0" "0" "0" "0" "0" "0",
316 : "Q" "H" "I" "B" "B" "d" "d" "d" "f" "f" "f" "f",
317 : AP_HAL::micros64(),
318 0 : ins_full_nav.gps_week,
319 0 : ins_full_nav.gps_time_ms,
320 0 : static_cast<uint8_t>(ins_full_nav.imu_alignment_status),
321 0 : static_cast<uint8_t>(ins_full_nav.gnss_status),
322 0 : ins_full_nav.latitude,
323 0 : ins_full_nav.longitude,
324 0 : ins_full_nav.altitude,
325 0 : ins_full_nav.vel_n,
326 0 : ins_full_nav.vel_e,
327 0 : ins_full_nav.vel_d,
328 0 : ins_full_nav.speed
329 : );
330 :
331 : // @LoggerMessage: GSN2
332 : // @Description: GSOF49 Full INS Navigation Solution 2
333 : // @Field: TimeUS: Time since system startup [us]
334 : // @Field: Roll: Roll [degrees]
335 : // @Field: Pitch: Pitch [degrees]
336 : // @Field: Heading: Heading [degrees]
337 : // @Field: Track: Track angle [degrees]
338 : // @Field: RateX: Angular rate X [deg/s]
339 : // @Field: RateY: Angular rate Y [deg/s]
340 : // @Field: RateZ: Angular rate Z [m/s]
341 : // @Field: AccX: Acceleration X [m/s^2]
342 : // @Field: AccY: Acceleration Y [m/s^2]
343 : // @Field: AccZ: Acceleration Z [m/s^2]
344 0 : AP::logger().WriteStreaming(
345 : "GSN2",
346 : "TimeUS,Roll,Pitch,Heading,Track,RateX,RateY,RateZ,AccX,AccY,AccZ",
347 : "s" "d" "d" "h" "d" "k" "k" "E" "o" "o" "o",
348 : "F" "0" "0" "0" "0" "0" "0" "0" "0" "0" "0",
349 : "Q" "d" "d" "d" "d" "f" "f" "f" "f" "f" "f",
350 : AP_HAL::micros64(),
351 0 : ins_full_nav.roll_deg,
352 0 : ins_full_nav.pitch_deg,
353 0 : ins_full_nav.heading_deg,
354 0 : ins_full_nav.track_angle_deg,
355 0 : ins_full_nav.ang_rate_x,
356 0 : ins_full_nav.ang_rate_y,
357 0 : ins_full_nav.ang_rate_z,
358 0 : ins_full_nav.acc_x,
359 0 : ins_full_nav.acc_y,
360 0 : ins_full_nav.acc_z
361 : );
362 0 : }
363 :
364 0 : void AP_GSOF::log_ins_rms() const
365 : {
366 : // @LoggerMessage: GSR1
367 : // @Description: GSOF50 INS Solution RMS Values 1
368 : // @Field: TUS: Time since system startup [us]
369 : // @Field: GpsTMs: GPS time of week [ms]
370 : // @Field: PUN: North Position uncertainty RMS [m]
371 : // @Field: PUE: East Position uncertainty RMS [m]
372 : // @Field: PUD: Down Position uncertainty RMS [m]
373 : // @Field: VUN: North Velocity uncertainty RMS [m/s]
374 : // @Field: VUE: East Velocity uncertainty RMS [m/s]
375 : // @Field: VUD: Down Velocity uncertainty RMS [m/s]
376 : // @Field: RollU: Roll RMS uncertainty [deg]
377 : // @Field: PitchU: Pitch RMS uncertainty[deg]
378 : // @Field: YawU: Yaw RMS uncertainty[deg]
379 0 : AP::logger().WriteStreaming(
380 : "GSR1",
381 : "TUS,GpsTMs,PUN,PUE,PUD,VUN,VUE,VUD,RollU,PitchU,YawU",
382 : "s" "s" "m" "m" "m" "n" "n" "n" "d" "d" "d",
383 : "F" "C" "0" "0" "0" "0" "0" "0" "0" "0" "0",
384 : "Q" "I" "f" "f" "f" "f" "f" "f" "f" "f" "f",
385 : AP_HAL::micros64(),
386 0 : ins_rms.gps_time_ms,
387 0 : ins_rms.pos_rms_n,
388 0 : ins_rms.pos_rms_e,
389 0 : ins_rms.pos_rms_d,
390 0 : ins_rms.vel_rms_n,
391 0 : ins_rms.vel_rms_e,
392 0 : ins_rms.vel_rms_d,
393 0 : ins_rms.roll_rms_deg,
394 0 : ins_rms.pitch_rms_deg,
395 0 : ins_rms.yaw_rms_deg
396 : );
397 :
398 0 : }
399 0 : void AP_GSOF::log_llh_msl() const
400 : {
401 : // @LoggerMessage: GSLH
402 : // @Description: GSOF70 LLH with MSL Altitude
403 : // @Field: TimeUS: Time since system startup [µs]
404 : // @Field: Lat: Latitude [deg]
405 : // @Field: Lng: Longitude [deg]
406 : // @Field: AltMSL: Altitude above mean sea level [m]
407 : // @Field: Model: Geoid model used (null-terminated string)
408 0 : AP::logger().WriteStreaming(
409 : "GSLH",
410 : "TimeUS,Lat,Lng,AltMSL,Model",
411 : "s" "D" "U" "m" "-",
412 : "F" "0" "0" "0" "-",
413 : "Q" "d" "d" "d" "Z",
414 : AP_HAL::micros64(),
415 0 : llh_msl.latitude,
416 0 : llh_msl.longitude,
417 0 : llh_msl.altitude_msl,
418 0 : llh_msl.model
419 : );
420 0 : }
421 : #endif // HAL_LOGGING_ENABLED
422 :
423 : #endif // AP_GSOF_ENABLED
424 :
|
