mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 01:48:29 -04:00
87c7781be2
this detects GPS data lag, and if 5 samples in a row are lagged by more than 50ms beyond the expected lag for the GPS then we declare the GPS as unhealthy. This is useful to detect users who have asked for more data from the GPS then it can send at the baudrate that is being used. The case that led to this path was a F9 GPS with GPS_RAW_DATA=1 at 115200 baud. In that case the UART data is quickly lagged by over 1s
306 lines
9.2 KiB
C++
306 lines
9.2 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "AP_GPS.h"
|
|
#include "GPS_Backend.h"
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
#define GPS_BACKEND_DEBUGGING 0
|
|
|
|
#if GPS_BACKEND_DEBUGGING
|
|
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
|
#else
|
|
# define Debug(fmt, args ...)
|
|
#endif
|
|
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
|
|
port(_port),
|
|
gps(_gps),
|
|
state(_state)
|
|
{
|
|
state.have_speed_accuracy = false;
|
|
state.have_horizontal_accuracy = false;
|
|
state.have_vertical_accuracy = false;
|
|
}
|
|
|
|
int32_t AP_GPS_Backend::swap_int32(int32_t v) const
|
|
{
|
|
const uint8_t *b = (const uint8_t *)&v;
|
|
union {
|
|
int32_t v;
|
|
uint8_t b[4];
|
|
} u;
|
|
|
|
u.b[0] = b[3];
|
|
u.b[1] = b[2];
|
|
u.b[2] = b[1];
|
|
u.b[3] = b[0];
|
|
|
|
return u.v;
|
|
}
|
|
|
|
int16_t AP_GPS_Backend::swap_int16(int16_t v) const
|
|
{
|
|
const uint8_t *b = (const uint8_t *)&v;
|
|
union {
|
|
int16_t v;
|
|
uint8_t b[2];
|
|
} u;
|
|
|
|
u.b[0] = b[1];
|
|
u.b[1] = b[0];
|
|
|
|
return u.v;
|
|
}
|
|
|
|
|
|
/**
|
|
fill in time_week_ms and time_week from BCD date and time components
|
|
assumes MTK19 millisecond form of bcd_time
|
|
*/
|
|
void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
|
|
{
|
|
uint8_t year, mon, day, hour, min, sec;
|
|
uint16_t msec;
|
|
|
|
year = bcd_date % 100;
|
|
mon = (bcd_date / 100) % 100;
|
|
day = bcd_date / 10000;
|
|
|
|
uint32_t v = bcd_milliseconds;
|
|
msec = v % 1000; v /= 1000;
|
|
sec = v % 100; v /= 100;
|
|
min = v % 100; v /= 100;
|
|
hour = v % 100;
|
|
|
|
int8_t rmon = mon - 2;
|
|
if (0 >= rmon) {
|
|
rmon += 12;
|
|
year -= 1;
|
|
}
|
|
|
|
// get time in seconds since unix epoch
|
|
uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day;
|
|
ret += year*365 + 10501;
|
|
ret = ret*24 + hour;
|
|
ret = ret*60 + min;
|
|
ret = ret*60 + sec;
|
|
|
|
// convert to time since GPS epoch
|
|
ret -= 272764785UL;
|
|
|
|
// get GPS week and time
|
|
state.time_week = ret / AP_SEC_PER_WEEK;
|
|
state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC;
|
|
state.time_week_ms += msec;
|
|
}
|
|
|
|
/*
|
|
fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
|
|
*/
|
|
void AP_GPS_Backend::fill_3d_velocity(void)
|
|
{
|
|
float gps_heading = radians(state.ground_course);
|
|
|
|
state.velocity.x = state.ground_speed * cosf(gps_heading);
|
|
state.velocity.y = state.ground_speed * sinf(gps_heading);
|
|
state.velocity.z = 0;
|
|
state.have_vertical_velocity = false;
|
|
}
|
|
|
|
void
|
|
AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len)
|
|
{
|
|
// not all backends have valid ports
|
|
if (port != nullptr) {
|
|
if (port->txspace() > len) {
|
|
port->write(data, len);
|
|
} else {
|
|
Debug("GPS %d: Not enough TXSPACE", state.instance + 1);
|
|
}
|
|
}
|
|
}
|
|
|
|
void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const
|
|
{
|
|
const uint8_t instance = state.instance;
|
|
const struct AP_GPS::detect_state dstate = gps.detect_state[instance];
|
|
|
|
if (dstate.auto_detected_baud) {
|
|
hal.util->snprintf(buffer, buflen,
|
|
"GPS %d: detected as %s at %d baud",
|
|
instance + 1,
|
|
name(),
|
|
gps._baudrates[dstate.current_baud]);
|
|
} else {
|
|
hal.util->snprintf(buffer, buflen,
|
|
"GPS %d: specified as %s",
|
|
instance + 1,
|
|
name());
|
|
}
|
|
}
|
|
|
|
|
|
void AP_GPS_Backend::broadcast_gps_type() const
|
|
{
|
|
#ifndef HAL_NO_GCS
|
|
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
|
|
_detection_message(buffer, sizeof(buffer));
|
|
gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer);
|
|
#endif
|
|
}
|
|
|
|
void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const
|
|
{
|
|
#ifndef HAL_NO_LOGGING
|
|
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
|
|
_detection_message(buffer, sizeof(buffer));
|
|
AP::logger().Write_Message(buffer);
|
|
#endif
|
|
}
|
|
|
|
bool AP_GPS_Backend::should_log() const
|
|
{
|
|
return gps.should_log();
|
|
}
|
|
|
|
|
|
void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
|
|
{
|
|
#ifndef HAL_NO_GCS
|
|
const uint8_t instance = state.instance;
|
|
// send status
|
|
switch (instance) {
|
|
case 0:
|
|
mavlink_msg_gps_rtk_send(chan,
|
|
0, // Not implemented yet
|
|
0, // Not implemented yet
|
|
state.rtk_week_number,
|
|
state.rtk_time_week_ms,
|
|
0, // Not implemented yet
|
|
0, // Not implemented yet
|
|
state.rtk_num_sats,
|
|
state.rtk_baseline_coords_type,
|
|
state.rtk_baseline_x_mm,
|
|
state.rtk_baseline_y_mm,
|
|
state.rtk_baseline_z_mm,
|
|
state.rtk_accuracy,
|
|
state.rtk_iar_num_hypotheses);
|
|
break;
|
|
case 1:
|
|
mavlink_msg_gps2_rtk_send(chan,
|
|
0, // Not implemented yet
|
|
0, // Not implemented yet
|
|
state.rtk_week_number,
|
|
state.rtk_time_week_ms,
|
|
0, // Not implemented yet
|
|
0, // Not implemented yet
|
|
state.rtk_num_sats,
|
|
state.rtk_baseline_coords_type,
|
|
state.rtk_baseline_x_mm,
|
|
state.rtk_baseline_y_mm,
|
|
state.rtk_baseline_z_mm,
|
|
state.rtk_accuracy,
|
|
state.rtk_iar_num_hypotheses);
|
|
break;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
|
|
/*
|
|
set a timestamp based on arrival time on uart at current byte,
|
|
assuming the message started nbytes ago
|
|
*/
|
|
void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
|
|
{
|
|
if (port) {
|
|
state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
|
|
}
|
|
}
|
|
|
|
|
|
void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
|
|
{
|
|
if (itow != _last_itow) {
|
|
_last_itow = itow;
|
|
|
|
/*
|
|
we need to calculate a pseudo-itow, which copes with the
|
|
iTow from the GPS changing in unexpected ways. We assume
|
|
that timestamps from the GPS are always in multiples of
|
|
50ms. That means we can't handle a GPS with an update rate
|
|
of more than 20Hz. We could do more, but we'd need the GPS
|
|
poll time to be higher
|
|
*/
|
|
const uint32_t gps_min_period_ms = 50;
|
|
|
|
// get the time the packet arrived on the UART
|
|
uint64_t uart_us = port->receive_time_constraint_us(msg_length);
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
uint32_t dt_ms = now - _last_ms;
|
|
_last_ms = now;
|
|
|
|
// round to nearest 50ms period
|
|
dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms;
|
|
|
|
// work out an actual message rate. If we get 5 messages in a
|
|
// row with a new rate we switch rate
|
|
if (_last_rate_ms == dt_ms) {
|
|
if (_rate_counter < 5) {
|
|
_rate_counter++;
|
|
} else if (_rate_ms != dt_ms) {
|
|
_rate_ms = dt_ms;
|
|
}
|
|
} else {
|
|
_rate_counter = 0;
|
|
_last_rate_ms = dt_ms;
|
|
}
|
|
if (_rate_ms == 0) {
|
|
// only allow 5Hz to 20Hz in user config
|
|
_rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200);
|
|
}
|
|
|
|
// round to calculated message rate
|
|
dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms;
|
|
|
|
// calculate pseudo-itow
|
|
_pseudo_itow += dt_ms * 1000U;
|
|
|
|
// use msg arrival time, and correct for jitter
|
|
uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
|
|
state.uart_timestamp_ms = local_us / 1000U;
|
|
|
|
// look for lagged data from the GPS. This is meant to detect
|
|
// the case that the GPS is trying to push more data into the
|
|
// UART than can fit (eg. with GPS_RAW_DATA at 115200).
|
|
float expected_lag;
|
|
if (gps.get_lag(state.instance, expected_lag)) {
|
|
float lag_s = (now - state.uart_timestamp_ms) * 0.001;
|
|
if (lag_s > expected_lag+0.05) {
|
|
// more than 50ms over expected lag, increment lag counter
|
|
state.lagged_sample_count++;
|
|
} else {
|
|
state.lagged_sample_count = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|