Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: change handling of moving baseline yaw 

this changes yaw handling in a few ways:

 - GPS yaw now has a timestamp associated with the yaw separate from
   the timestamp associated with the GPS fix

 - we no longer force the primary to change to the UBLOX MB rover when
   it has a GPS yaw. This means we don't change GPS primary due to GPS
   loss, which keeps the GPS more stable. It also increases accuracy
   as the rover is always less accurate in position and velocity than
   the base

 - now we force the primary to be the MB base if the other GPS is a
   rover and the base has GPS lock
This commit is contained in:
Andrew Tridgell 2021-07-20 17:36:45 +10:00 committed by Randy Mackay
parent faec3e1a8a
commit f1cbfb3e46
7 changed files with 70 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
libraries/AP_GPS/AP_GPS.cpp
View File

@ -984,41 +984,22 @@ void AP_GPS::update_primary(void)
uint32_t now = AP_HAL::millis();
// special handling of RTK moving baseline pair. If a rover has a
// RTK fixed lock and yaw available then always select it as
// primary. This ensures that the yaw data and position/velocity
// data is time aligned whenever we provide yaw to the EKF
// special handling of RTK moving baseline pair. Always use the
// base as the rover position is derived from the base, which
// means the rover always has worse position and velocity than the
// base. This overrides the normal logic which would select the
// rover as it typically is in fix type 6 (RTK) whereas base is
// usually fix type 3
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER &&
state[i].status == GPS_OK_FIX_3D_RTK_FIXED &&
state[i].have_gps_yaw) {
if (primary_instance != i) {
_last_instance_swap_ms = now;
primary_instance = i;
}
return;
}
/*
if this is a BASE and the other GPS is a MB rover, then we
should force the use of the BASE GPS if the rover doesn't
have a yaw lock. This is important as when the rover doesn't
have a lock it will often report a higher status than the
base (eg. status=4), but the velocity and position data can
be very bad. As the rover is getting it's base position from
the base GPS then the position and velocity are expected to
be worse than the base GPS unless it has a full moving
baseline lock
*/
const uint8_t i2 = i^1; // the other GPS in the pair
if (_type[i] == GPS_TYPE_UBLOX_RTK_BASE &&
state[i].status >= GPS_OK_FIX_3D &&
_type[i2] == GPS_TYPE_UBLOX_RTK_ROVER &&
(state[i2].status != GPS_OK_FIX_3D_RTK_FIXED ||
!state[i2].have_gps_yaw)) {
_type[i^1] == GPS_TYPE_UBLOX_RTK_ROVER &&
((state[i].status >= GPS_OK_FIX_3D) || (state[i].status >= state[i^1].status))) {
if (primary_instance != i) {
_last_instance_swap_ms = now;
primary_instance = i;
}
// don't do any more switching logic. We don't want the
// RTK status of the rover to cause a switch
return;
}
}
@ -1195,7 +1176,8 @@ uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const
return 0;
}
float yaw_deg, accuracy_deg;
if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg)) {
uint32_t time_ms;
if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg, time_ms)) {
return 65535;
}
int yaw_cd = wrap_360_cd(yaw_deg * 100);
@ -1944,7 +1926,8 @@ void AP_GPS::Write_GPS(uint8_t i)
const struct Location &loc = location(i);
float yaw_deg=0, yaw_accuracy_deg=0;
gps_yaw_deg(i, yaw_deg, yaw_accuracy_deg);
uint32_t yaw_time_ms;
gps_yaw_deg(i, yaw_deg, yaw_accuracy_deg, yaw_time_ms);
const struct log_GPS pkt {
LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
@ -1987,6 +1970,41 @@ void AP_GPS::Write_GPS(uint8_t i)
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
}
/*
get GPS based yaw
*/
bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const
{
#if GPS_MAX_RECEIVERS > 1
if (instance < GPS_MAX_RECEIVERS &&
_type[instance] == GPS_TYPE_UBLOX_RTK_BASE &&
_type[instance^1] == GPS_TYPE_UBLOX_RTK_ROVER) {
// return the yaw from the rover
instance ^= 1;
}
#endif
if (!have_gps_yaw(instance)) {
return false;
}
yaw_deg = state[instance].gps_yaw;
// get lagged timestamp
time_ms = state[instance].gps_yaw_time_ms;
float lag_s;
if (get_lag(instance, lag_s)) {
uint32_t lag_ms = lag_s * 1000;
time_ms -= lag_ms;
}
if (state[instance].have_gps_yaw_accuracy) {
accuracy_deg = state[instance].gps_yaw_accuracy;
} else {
// fall back to 10 degrees as a generic default
accuracy_deg = 10;
}
return true;
}
namespace AP {
AP_GPS &gps()

19
libraries/AP_GPS/AP_GPS.h
View File

@ -168,6 +168,7 @@ public:
float ground_speed; ///< ground speed in m/sec
float ground_course; ///< ground course in degrees
float gps_yaw; ///< GPS derived yaw information, if available (degrees)
uint32_t gps_yaw_time_ms; ///< timestamp of last GPS yaw reading
bool gps_yaw_configured; ///< GPS is configured to provide yaw
uint16_t hdop; ///< horizontal dilution of precision in cm
uint16_t vdop; ///< vertical dilution of precision in cm
@ -328,21 +329,9 @@ public:
}
// yaw in degrees if available
bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const {
if (!have_gps_yaw(instance)) {
return false;
}
yaw_deg = state[instance].gps_yaw;
if (state[instance].have_gps_yaw_accuracy) {
accuracy_deg = state[instance].gps_yaw_accuracy;
} else {
// fall back to 10 degrees as a generic default
accuracy_deg = 10;
}
return true;
}
bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const {
return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg);
bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const;
bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const {
return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg, time_ms);
}
// number of locked satellites

10
libraries/AP_GPS/AP_GPS_MAV.cpp
View File

@ -104,8 +104,11 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
state.have_vertical_accuracy = true;
}
const uint32_t now_ms = AP_HAL::millis();
if (have_yaw) {
state.gps_yaw = wrap_360(packet.yaw*0.01);
state.gps_yaw_time_ms = now_ms;
state.have_gps_yaw = true;
state.gps_yaw_configured = true;
}
@ -120,12 +123,15 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
first_week = packet.time_week;
}
uint32_t timestamp_ms = (packet.time_week - first_week) * AP_MSEC_PER_WEEK + packet.time_week_ms;
uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, AP_HAL::millis());
uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, now_ms);
state.uart_timestamp_ms = corrected_ms;
if (have_yaw) {
state.gps_yaw_time_ms = corrected_ms;
}
}
state.num_sats = packet.satellites_visible;
state.last_gps_time_ms = AP_HAL::millis();
state.last_gps_time_ms = now_ms;
_new_data = true;
break;
}

5
libraries/AP_GPS/AP_GPS_MSP.cpp
View File

@ -77,13 +77,14 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt)
state.vertical_accuracy = pkt.vertical_pos_accuracy * 0.01;
state.speed_accuracy = pkt.horizontal_vel_accuracy * 0.01;
state.last_gps_time_ms = AP_HAL::millis();
if (pkt.true_yaw != 65535) {
state.gps_yaw = wrap_360(pkt.true_yaw*0.01);
state.have_gps_yaw = true;
state.gps_yaw_time_ms = state.last_gps_time_ms;
}
state.last_gps_time_ms = AP_HAL::millis();
new_data = pkt.fix_type>0;
}

1
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -339,6 +339,7 @@ bool AP_GPS_NMEA::_term_complete()
_last_HDT_THS_ms = now;
state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
state.have_gps_yaw = true;
state.gps_yaw_time_ms = AP_HAL::millis();
// remember that we are setup to provide yaw. With
// a NMEA GPS we can only tell if the GPS is
// configured to provide yaw when it first sends a

3
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -525,6 +525,9 @@ void AP_GPS_UAVCAN::handle_heading_msg(const HeadingCb &cb)
interim_state.have_gps_yaw = cb.msg->heading_valid;
interim_state.gps_yaw = degrees(cb.msg->heading_rad);
if (interim_state.have_gps_yaw) {
interim_state.gps_yaw_time_ms = AP_HAL::millis();
}
interim_state.have_gps_yaw_accuracy = cb.msg->heading_accuracy_valid;
interim_state.gps_yaw_accuracy = degrees(cb.msg->heading_accuracy_rad);

1
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -384,6 +384,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg,
const float rotation_offset_rad = Vector2f(-offset.x, -offset.y).angle();
state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad));
state.have_gps_yaw = true;
state.gps_yaw_time_ms = AP_HAL::millis();
}
}