mirror of https://github.com/ArduPilot/ardupilot
AP_GPS: implement missing "RTK number of satellites" and "RTK correction age" information in the GPS2_RAW MAVLink message
This commit is contained in:
parent
059c213d19
commit
ae47ac5a09
|
@ -900,8 +900,8 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
|
|||
ground_speed(1)*100, // cm/s
|
||||
ground_course(1)*100, // 1/100 degrees,
|
||||
num_sats(1),
|
||||
0,
|
||||
0);
|
||||
rtk_num_sats(1),
|
||||
rtk_age_ms(1));
|
||||
}
|
||||
|
||||
void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan)
|
||||
|
|
|
@ -142,6 +142,10 @@ public:
|
|||
bool have_horizontal_accuracy:1; ///< does GPS give horizontal position accuracy? Set to true only once available.
|
||||
bool have_vertical_accuracy:1; ///< does GPS give vertical position accuracy? Set to true only once available.
|
||||
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
|
||||
|
||||
// all the following fields must only all be filled by RTK capable backend drivers
|
||||
uint32_t rtk_age_ms; ///< GPS age of last baseline correction in milliseconds (0 when no corrections, 0xFFFFFFFF indicates overflow)
|
||||
uint8_t rtk_num_sats; ///< Current number of satellites used for RTK calculation
|
||||
};
|
||||
|
||||
/// Startup initialisation.
|
||||
|
@ -305,6 +309,22 @@ public:
|
|||
return have_vertical_velocity(primary_instance);
|
||||
}
|
||||
|
||||
// return number of satellites used for RTK calculation
|
||||
uint8_t rtk_num_sats(uint8_t instance) const {
|
||||
return state[instance].rtk_num_sats;
|
||||
}
|
||||
uint8_t rtk_num_sats(void) const {
|
||||
return rtk_num_sats(primary_instance);
|
||||
}
|
||||
|
||||
// return age of last baseline correction in milliseconds
|
||||
uint32_t rtk_age_ms(uint8_t instance) const {
|
||||
return state[instance].rtk_age_ms;
|
||||
}
|
||||
uint32_t rtk_age_ms(void) const {
|
||||
return rtk_age_ms(primary_instance);
|
||||
}
|
||||
|
||||
// the expected lag (in seconds) in the position and velocity readings from the gps
|
||||
// return true if the GPS hardware configuration is known or the lag parameter has been set manually
|
||||
bool get_lag(uint8_t instance, float &lag_sec) const;
|
||||
|
|
Loading…
Reference in New Issue