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 "AP_GPS_config.h"
17 :
18 : #if AP_GPS_ENABLED
19 :
20 : #include "AP_GPS.h"
21 : #include "GPS_Backend.h"
22 : #include <AP_Logger/AP_Logger.h>
23 : #include <time.h>
24 : #include <AP_Common/time.h>
25 : #include <AP_InternalError/AP_InternalError.h>
26 : #include <AP_AHRS/AP_AHRS.h>
27 :
28 : #define GPS_BACKEND_DEBUGGING 0
29 :
30 : #if GPS_BACKEND_DEBUGGING
31 : # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
32 : #else
33 : # define Debug(fmt, args ...)
34 : #endif
35 :
36 : #include <GCS_MAVLink/GCS.h>
37 :
38 : #if AP_GPS_DEBUG_LOGGING_ENABLED
39 : #include <AP_Filesystem/AP_Filesystem.h>
40 : #endif
41 :
42 : extern const AP_HAL::HAL& hal;
43 :
44 3190 : AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
45 3190 : port(_port),
46 3190 : gps(_gps),
47 3190 : state(_state),
48 3190 : params(_params)
49 : {
50 3190 : state.have_speed_accuracy = false;
51 3190 : state.have_horizontal_accuracy = false;
52 3190 : state.have_vertical_accuracy = false;
53 3190 : }
54 :
55 : /*
56 : get the last time of week in ms
57 : */
58 8053612 : uint32_t AP_GPS_Backend::get_last_itow_ms(void) const
59 : {
60 8053612 : if (!_have_itow) {
61 3831 : return state.time_week_ms;
62 : }
63 8049781 : return (_pseudo_itow_delta_ms == 0)?(_last_itow_ms):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms);
64 : }
65 :
66 1497 : void AP_GPS_Backend::BCD_to_gps_time(uint32_t bcd_date, uint32_t bcd_time_ms, uint16_t& gps_week, uint32_t& gps_time_ms)
67 : {
68 1497 : struct tm tm {};
69 :
70 1497 : tm.tm_year = 100U + bcd_date % 100U;
71 1497 : tm.tm_mon = ((bcd_date / 100U) % 100U)-1;
72 1497 : tm.tm_mday = bcd_date / 10000U;
73 :
74 1497 : uint32_t v = bcd_time_ms;
75 1497 : uint16_t msec = v % 1000U; v /= 1000U;
76 1497 : tm.tm_sec = v % 100U; v /= 100U;
77 1497 : tm.tm_min = v % 100U; v /= 100U;
78 1497 : tm.tm_hour = v % 100U;
79 :
80 : // convert from time structure to unix time
81 1497 : time_t unix_time = ap_mktime(&tm);
82 :
83 : // convert to time since GPS epoch
84 1497 : const uint32_t unix_to_GPS_secs = 315964800UL;
85 1497 : const uint16_t leap_seconds_unix = GPS_LEAPSECONDS_MILLIS/1000U;
86 1497 : uint32_t ret = unix_time + leap_seconds_unix - unix_to_GPS_secs;
87 :
88 : // get GPS week and time
89 1497 : gps_week = ret / AP_SEC_PER_WEEK;
90 1497 : gps_time_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC;
91 1497 : gps_time_ms += msec;
92 1497 : }
93 :
94 : /*
95 : fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
96 : */
97 869 : void AP_GPS_Backend::fill_3d_velocity(void)
98 : {
99 869 : float gps_heading = radians(state.ground_course);
100 :
101 869 : state.velocity.x = state.ground_speed * cosf(gps_heading);
102 869 : state.velocity.y = state.ground_speed * sinf(gps_heading);
103 869 : state.velocity.z = 0;
104 869 : state.have_vertical_velocity = false;
105 869 : }
106 :
107 : /*
108 : fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
109 : */
110 17455 : void AP_GPS_Backend::velocity_to_speed_course(AP_GPS::GPS_State &s)
111 : {
112 17455 : s.ground_course = wrap_360(degrees(atan2f(s.velocity.y, s.velocity.x)));
113 17455 : s.ground_speed = s.velocity.xy().length();
114 17455 : }
115 :
116 : void
117 0 : AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len)
118 : {
119 : // not all backends have valid ports
120 0 : if (port != nullptr) {
121 0 : if (port->txspace() > len) {
122 0 : port->write(data, len);
123 : } else {
124 : Debug("GPS %d: Not enough TXSPACE", state.instance + 1);
125 : }
126 : }
127 0 : }
128 :
129 2943 : void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const
130 : {
131 2943 : const uint8_t instance = state.instance;
132 2943 : const struct AP_GPS::detect_state dstate = gps.detect_state[instance];
133 :
134 2943 : if (dstate.auto_detected_baud) {
135 5628 : hal.util->snprintf(buffer, buflen,
136 : "GPS %d: probing for %s at %d baud",
137 2814 : instance + 1,
138 2814 : name(),
139 2814 : int(dstate.probe_baud));
140 : } else {
141 258 : hal.util->snprintf(buffer, buflen,
142 : "GPS %d: specified as %s",
143 129 : instance + 1,
144 129 : name());
145 : }
146 2943 : }
147 :
148 :
149 1098 : void AP_GPS_Backend::broadcast_gps_type() const
150 : {
151 : char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
152 1098 : _detection_message(buffer, sizeof(buffer));
153 1098 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", buffer);
154 1098 : }
155 :
156 : #if HAL_LOGGING_ENABLED
157 1845 : void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const
158 : {
159 : char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
160 1845 : _detection_message(buffer, sizeof(buffer));
161 1845 : AP::logger().Write_Message(buffer);
162 1845 : }
163 :
164 1657 : bool AP_GPS_Backend::should_log() const
165 : {
166 1657 : return gps.should_log();
167 : }
168 : #endif
169 :
170 :
171 : #if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED
172 129 : void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
173 : {
174 129 : const uint8_t instance = state.instance;
175 : // send status
176 129 : switch (instance) {
177 129 : case 0:
178 129 : mavlink_msg_gps_rtk_send(chan,
179 : 0, // Not implemented yet
180 : 0, // Not implemented yet
181 129 : state.rtk_week_number,
182 129 : state.rtk_time_week_ms,
183 : 0, // Not implemented yet
184 : 0, // Not implemented yet
185 129 : state.rtk_num_sats,
186 129 : state.rtk_baseline_coords_type,
187 129 : state.rtk_baseline_x_mm,
188 129 : state.rtk_baseline_y_mm,
189 129 : state.rtk_baseline_z_mm,
190 129 : state.rtk_accuracy,
191 129 : state.rtk_iar_num_hypotheses);
192 129 : break;
193 0 : case 1:
194 0 : mavlink_msg_gps2_rtk_send(chan,
195 : 0, // Not implemented yet
196 : 0, // Not implemented yet
197 0 : state.rtk_week_number,
198 0 : state.rtk_time_week_ms,
199 : 0, // Not implemented yet
200 : 0, // Not implemented yet
201 0 : state.rtk_num_sats,
202 0 : state.rtk_baseline_coords_type,
203 0 : state.rtk_baseline_x_mm,
204 0 : state.rtk_baseline_y_mm,
205 0 : state.rtk_baseline_z_mm,
206 0 : state.rtk_accuracy,
207 0 : state.rtk_iar_num_hypotheses);
208 0 : break;
209 : }
210 129 : }
211 : #endif // AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED
212 :
213 :
214 :
215 : /*
216 : set a timestamp based on arrival time on uart at current byte,
217 : assuming the message started nbytes ago
218 : */
219 407 : void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
220 : {
221 407 : if (port) {
222 407 : state.last_corrected_gps_time_us = port->receive_time_constraint_us(nbytes);
223 407 : state.corrected_timestamp_updated = true;
224 : }
225 407 : }
226 :
227 :
228 3495892 : void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
229 : {
230 3495892 : if (itow != _last_itow_ms) {
231 710144 : _last_itow_ms = itow;
232 710144 : _have_itow = true;
233 :
234 : /*
235 : we need to calculate a pseudo-itow, which copes with the
236 : iTow from the GPS changing in unexpected ways. We assume
237 : that timestamps from the GPS are always in multiples of
238 : 50ms. That means we can't handle a GPS with an update rate
239 : of more than 20Hz. We could do more, but we'd need the GPS
240 : poll time to be higher
241 : */
242 710144 : const uint32_t gps_min_period_ms = 50;
243 :
244 : // get the time the packet arrived on the UART
245 : uint64_t uart_us;
246 710144 : if (_last_pps_time_us != 0 && (state.status >= AP_GPS_FixType::FIX_2D)) {
247 : // pps is only reliable when we have some sort of GPS fix
248 0 : uart_us = _last_pps_time_us;
249 0 : _last_pps_time_us = 0;
250 710144 : } else if (port) {
251 695548 : uart_us = port->receive_time_constraint_us(msg_length);
252 : } else {
253 14596 : uart_us = AP_HAL::micros64();
254 : }
255 :
256 710144 : uint32_t now = AP_HAL::millis();
257 710144 : uint32_t dt_ms = now - _last_ms;
258 710144 : _last_ms = now;
259 :
260 : // round to nearest 50ms period
261 710144 : dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms;
262 :
263 : // work out an actual message rate. If we get 5 messages in a
264 : // row with a new rate we switch rate
265 710144 : if (_last_rate_ms == dt_ms) {
266 704416 : if (_rate_counter < 5) {
267 9523 : _rate_counter++;
268 694893 : } else if (_rate_ms != dt_ms) {
269 248 : _rate_ms = dt_ms;
270 : }
271 : } else {
272 5728 : _rate_counter = 0;
273 5728 : _last_rate_ms = dt_ms;
274 5728 : if (_rate_ms != 0) {
275 4642 : set_pps_desired_freq(1000/_rate_ms);
276 : }
277 : }
278 710144 : if (_rate_ms == 0) {
279 : // only allow 5Hz to 20Hz in user config
280 1297 : _rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200);
281 : }
282 :
283 : // round to calculated message rate
284 710144 : dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms;
285 :
286 : // calculate pseudo-itow
287 710144 : _pseudo_itow += dt_ms * 1000U;
288 :
289 : // use msg arrival time, and correct for jitter
290 710144 : uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
291 710144 : state.last_corrected_gps_time_us = local_us;
292 710144 : state.corrected_timestamp_updated = true;
293 :
294 : #ifndef HAL_BUILD_AP_PERIPH
295 : // look for lagged data from the GPS. This is meant to detect
296 : // the case that the GPS is trying to push more data into the
297 : // UART than can fit (eg. with GPS_RAW_DATA at 115200).
298 : // This is disabled on AP_Periph as it is better to catch missed packet rate at the flight
299 : // controller level
300 : float expected_lag;
301 710144 : if (gps.get_lag(state.instance, expected_lag)) {
302 709138 : float lag_s = (now - (state.last_corrected_gps_time_us/1000U)) * 0.001;
303 709138 : if (lag_s > expected_lag+0.05) {
304 : // more than 50ms over expected lag, increment lag counter
305 1630 : state.lagged_sample_count++;
306 : } else {
307 707508 : state.lagged_sample_count = 0;
308 : }
309 : }
310 : #endif // HAL_BUILD_AP_PERIPH
311 :
312 710144 : if (state.status >= AP_GPS_FixType::FIX_2D) {
313 : // we must have a decent fix to calculate difference between itow and pseudo-itow
314 701624 : _pseudo_itow_delta_ms = itow - (_pseudo_itow/1000ULL);
315 : }
316 : }
317 3495892 : }
318 :
319 : #if GPS_MOVING_BASELINE
320 2318 : bool AP_GPS_Backend::calculate_moving_base_yaw(float reported_heading_deg, const float reported_distance, const float reported_D) {
321 2318 : return calculate_moving_base_yaw(state, reported_heading_deg, reported_distance, reported_D);
322 : }
323 :
324 2318 : bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D) {
325 2318 : constexpr float minimum_antenna_separation = AP_GPS_MB_MIN_ANTENNA_SEPARATION_M;
326 2318 : constexpr float permitted_error_length_pct = 0.2; // percentage
327 : #if HAL_LOGGING_ENABLED || AP_AHRS_ENABLED
328 2318 : float min_D = 0.0f;
329 2318 : float max_D = 0.0f;
330 : #endif
331 2318 : bool selectedOffset = false;
332 2318 : Vector3f offset;
333 2318 : switch (MovingBase::Type(gps.params[interim_state.instance].mb_params.type)) {
334 2318 : case MovingBase::Type::RelativeToAlternateInstance:
335 2318 : offset = gps.params[interim_state.instance^1].antenna_offset.get() - gps.params[interim_state.instance].antenna_offset.get();
336 2318 : selectedOffset = true;
337 2318 : break;
338 0 : case MovingBase::Type::RelativeToCustomBase:
339 0 : offset = gps.params[interim_state.instance].mb_params.base_offset.get();
340 0 : selectedOffset = true;
341 0 : break;
342 : }
343 :
344 2318 : if (!selectedOffset) {
345 : // invalid type, let's throw up a flag
346 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
347 0 : goto bad_yaw;
348 : }
349 :
350 : {
351 2318 : const float offset_dist = offset.length();
352 2318 : const float min_dist = MIN(offset_dist, reported_distance);
353 :
354 2318 : if (offset_dist < minimum_antenna_separation) {
355 : // offsets have to be sufficiently large to get a meaningful angle off of them
356 : Debug("Insufficent antenna offset (%f, %f, %f)", (double)offset.x, (double)offset.y, (double)offset.z);
357 58 : goto bad_yaw;
358 : }
359 :
360 2318 : if (reported_distance < minimum_antenna_separation) {
361 : // if the reported distance is less then the minimum separation it's not sufficiently robust
362 : Debug("Reported baseline distance (%f) was less then the minimum antenna separation (%f)",
363 : (double)reported_distance, (double)minimum_antenna_separation);
364 0 : goto bad_yaw;
365 : }
366 :
367 :
368 2318 : if (fabsf(offset_dist - reported_distance) > (min_dist * permitted_error_length_pct)) {
369 : // the magnitude of the vector is much further then we were expecting
370 : Debug("Offset=%.2f vs reported-distance=%.2f (max-delta=%.2f)",
371 : offset_dist, reported_distance, (double)(min_dist * permitted_error_length_pct));
372 1 : goto bad_yaw;
373 : }
374 :
375 : {
376 : // the reported heading is the bearing of the baseline in the horizontal
377 : // plane, so its noise scales inversely with the horizontal separation of
378 : // the antennas. A baseline close to vertical carries no usable yaw
379 : // information regardless of the antenna separation, whether because of
380 : // a vertical antenna installation or because vehicle attitude has
381 : // rotated the baseline close to vertical
382 2317 : const float reported_horizontal_sq = sq(reported_distance) - sq(reported_D);
383 2317 : if (reported_horizontal_sq < sq(minimum_antenna_separation)) {
384 : Debug("Insufficient horizontal separation %.2f of reported baseline (dist=%.2f, D=%.2f)",
385 : (double)safe_sqrt(reported_horizontal_sq), (double)reported_distance, (double)reported_D);
386 57 : goto bad_yaw;
387 : }
388 : }
389 :
390 : #if AP_AHRS_ENABLED
391 : {
392 : // get vehicle rotation, projected back in time using the gyro
393 : // this is not 100% accurate, but it is good enough for
394 : // this test. To do it completely accurately we'd need an
395 : // interface into DCM, EKF2 and EKF3 to ask for a
396 : // historical attitude. That is far too complex to justify
397 : // for this use case
398 2260 : const auto &ahrs = AP::ahrs();
399 2260 : const Vector3f &gyro = ahrs.get_gyro();
400 2260 : Matrix3f rot_body_to_ned_min_lag = ahrs.get_rotation_body_to_ned();
401 2260 : rot_body_to_ned_min_lag.rotate(gyro * -AP_GPS_MB_MIN_LAG);
402 2260 : Matrix3f rot_body_to_ned_max_lag = ahrs.get_rotation_body_to_ned();
403 2260 : rot_body_to_ned_max_lag.rotate(gyro * -AP_GPS_MB_MAX_LAG);
404 :
405 : // apply rotation to the offset to get the Z offset in NED
406 2260 : const Vector3f antenna_tilt_min_lag = rot_body_to_ned_min_lag * offset;
407 2260 : const Vector3f antenna_tilt_max_lag = rot_body_to_ned_max_lag * offset;
408 2260 : min_D = MIN(-antenna_tilt_min_lag.z, -antenna_tilt_max_lag.z);
409 2260 : max_D = MAX(-antenna_tilt_min_lag.z, -antenna_tilt_max_lag.z);
410 2260 : min_D -= permitted_error_length_pct * min_dist;
411 2260 : max_D += permitted_error_length_pct * min_dist;
412 2260 : if (reported_D < min_D || reported_D > max_D) {
413 : // the vertical component is out of range, reject it
414 : Debug("bad alt_err %f < %f < %f", (double)min_D, (double)reported_D, (double)max_D);
415 0 : goto bad_yaw;
416 : }
417 : }
418 : #endif // AP_AHRS_ENABLED
419 :
420 : {
421 : // at this point the offsets are looking okay, go ahead and actually calculate a useful heading
422 : // the bearing of the offset is only exact when the vehicle is level; the
423 : // offset is exported in mb_yaw_offset so consumers with an attitude
424 : // estimate can correct the yaw for vehicle roll and pitch
425 2260 : const float rotation_offset_rad = Vector2f(-offset.x, -offset.y).angle();
426 2260 : interim_state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad));
427 2260 : interim_state.have_gps_yaw = true;
428 2260 : interim_state.gps_yaw_time_ms = AP_HAL::millis();
429 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
430 2260 : interim_state.mb_yaw_offset = offset;
431 : #endif
432 : }
433 2260 : goto good_yaw;
434 : }
435 :
436 58 : bad_yaw:
437 58 : interim_state.have_gps_yaw = false;
438 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
439 58 : interim_state.mb_yaw_offset.zero();
440 : #endif
441 :
442 2318 : good_yaw:
443 :
444 : #if HAL_LOGGING_ENABLED
445 : // this log message helps diagnose GPS yaw issues
446 : // @LoggerMessage: GPYW
447 : // @Description: GPS Yaw
448 : // @Field: TimeUS: Time since system startup
449 : // @Field: Id: instance
450 : // @Field: RHD: reported heading,deg
451 : // @Field: RDist: antenna separation,m
452 : // @Field: RDown: vertical antenna separation,m
453 : // @Field: MinCDown: minimum tolerable vertical antenna separation,m
454 : // @Field: MaxCDown: maximum tolerable vertical antenna separation,m
455 : // @Field: OK: 1 if have yaw
456 4636 : AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,MinCDown,MaxCDown,OK",
457 : "s#dmmmm-",
458 : "F-------",
459 : "QBfffffB",
460 : AP_HAL::micros64(),
461 2318 : state.instance,
462 : reported_heading_deg,
463 : reported_distance,
464 : reported_D,
465 : min_D,
466 : max_D,
467 2318 : interim_state.have_gps_yaw);
468 : #endif
469 :
470 2318 : return interim_state.have_gps_yaw;
471 : }
472 : #endif // GPS_MOVING_BASELINE
473 :
474 : /*
475 : set altitude in location structure, honouring the driver option for
476 : MSL vs ellipsoid height
477 : */
478 695837 : void AP_GPS_Backend::set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm)
479 : {
480 695837 : if (option_set(AP_GPS::HeightEllipsoid) && _state.have_undulation) {
481 : // user has asked ArduPilot to use ellipsoid height in the
482 : // canonical height for mission and navigation
483 0 : _state.location.alt = alt_amsl_cm - _state.undulation*100;
484 : } else {
485 695837 : _state.location.alt = alt_amsl_cm;
486 : }
487 695837 : }
488 :
489 : #if AP_GPS_DEBUG_LOGGING_ENABLED
490 :
491 : /*
492 : log some data for debugging
493 :
494 : the logging format matches that used by SITL with SIM_GPS_TYPE=7,
495 : allowing for development of GPS drivers based on logged data
496 : */
497 : void AP_GPS_Backend::log_data(const uint8_t *data, uint16_t length)
498 : {
499 : if (state.instance < 2) {
500 : logging[state.instance].buf.write(data, length);
501 : }
502 : if (!log_thread_created) {
503 : log_thread_created = true;
504 : hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_GPS_Backend::logging_start, void), "gps_log", 4096, AP_HAL::Scheduler::PRIORITY_IO, 0);
505 : }
506 : }
507 :
508 : AP_GPS_Backend::loginfo AP_GPS_Backend::logging[2];
509 : bool AP_GPS_Backend::log_thread_created;
510 :
511 : // logging loop, needs to be static to allow for re-alloc of GPS backends
512 : void AP_GPS_Backend::logging_loop(void)
513 : {
514 : while (true) {
515 : hal.scheduler->delay(10);
516 : static uint16_t lognum;
517 : for (uint8_t instance=0; instance<2; instance++) {
518 : if (logging[instance].fd == -1 && logging[instance].buf.available()) {
519 : char fname[] = "gpsN_XXX.log";
520 : fname[3] = '1' + instance;
521 : if (lognum == 0) {
522 : for (lognum=1; lognum<1000; lognum++) {
523 : struct stat st;
524 : hal.util->snprintf(&fname[5], 8, "%03u.log", lognum);
525 : if (AP::FS().stat(fname, &st) != 0) {
526 : break;
527 : }
528 : }
529 : }
530 : hal.util->snprintf(&fname[5], 8, "%03u.log", lognum);
531 : logging[instance].fd = AP::FS().open(fname, O_WRONLY|O_CREAT|O_APPEND);
532 : }
533 : if (logging[instance].fd != -1) {
534 : uint32_t n = 0;
535 : const uint8_t *p;
536 : while ((p = logging[instance].buf.readptr(n)) != nullptr && n != 0) {
537 : struct {
538 : uint32_t magic = 0x7fe53b04U;
539 : uint32_t time_ms;
540 : uint32_t n;
541 : } header;
542 : header.n = n;
543 : header.time_ms = AP_HAL::millis();
544 : // short writes are unlikely and are ignored (only FS full errors)
545 : AP::FS().write(logging[instance].fd, (const uint8_t *)&header, sizeof(header));
546 : AP::FS().write(logging[instance].fd, p, n);
547 : logging[instance].buf.advance(n);
548 : AP::FS().fsync(logging[instance].fd);
549 : }
550 : }
551 : }
552 : }
553 : }
554 :
555 : // logging thread start, needs to be non-static for thread_create
556 : void AP_GPS_Backend::logging_start(void)
557 : {
558 : logging_loop();
559 : }
560 : #endif // AP_GPS_DEBUG_LOGGING_ENABLED
561 :
562 : #endif // AP_GPS_ENABLED
|
