Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: keep reporting GPS status messages when no GPS at 1Hz

Browse Source 
to ensure the GCS knows that we have lost the GPS we need to keep
reporting GPS_RAW_INT messages when the GPS disappears. Sending at 1Hz
should be sufficient

Fixes issue #1722
This commit is contained in:
Andrew Tridgell 2015-01-02 17:08:11 +11:00
parent 8d3940ce1b
commit ef55a3c6a6
1 changed files with 49 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
libraries/AP_GPS/AP_GPS.cpp
View File

@ -448,49 +448,64 @@ void
AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms;
if (last_send_time_ms == 0 || last_send_time_ms != last_message_time_ms(0)) {
if (status(0) > AP_GPS::NO_GPS) {
// when we have a GPS then only send new data
if (last_send_time_ms == last_message_time_ms(0)) {
return;
}
last_send_time_ms = last_message_time_ms(0);
const Location &loc = location(0);
mavlink_msg_gps_raw_int_send(
chan,
last_fix_time_ms(0)*(uint64_t)1000,
status(0),
loc.lat, // in 1E7 degrees
loc.lng, // in 1E7 degrees
loc.alt * 10UL, // in mm
get_hdop(0),
65535,
ground_speed(0)*100, // cm/s
ground_course_cd(0), // 1/100 degrees,
num_sats(0));
} else {
// when we don't have a GPS then send at 1Hz
uint32_t now = hal.scheduler->millis();
if (now - last_send_time_ms < 1000) {
return;
}
last_send_time_ms = now;
}
const Location &loc = location(0);
mavlink_msg_gps_raw_int_send(
chan,
last_fix_time_ms(0)*(uint64_t)1000,
status(0),
loc.lat, // in 1E7 degrees
loc.lng, // in 1E7 degrees
loc.alt * 10UL, // in mm
get_hdop(0),
65535,
ground_speed(0)*100, // cm/s
ground_course_cd(0), // 1/100 degrees,
num_sats(0));
}
#if GPS_MAX_INSTANCES > 1
void
AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms2;
if (num_sensors() > 1 &&
status(1) > AP_GPS::NO_GPS &&
(last_send_time_ms2 == 0 || last_send_time_ms2 != last_message_time_ms(1))) {
const Location &loc = location(1);
last_send_time_ms2 = last_message_time_ms(1);
mavlink_msg_gps2_raw_send(
chan,
last_fix_time_ms(1)*(uint64_t)1000,
status(1),
loc.lat,
loc.lng,
loc.alt * 10UL,
get_hdop(1),
65535,
ground_speed(1)*100, // cm/s
ground_course_cd(1), // 1/100 degrees,
num_sats(1),
0,
0);
static uint32_t last_send_time_ms;
if (num_sensors() < 2 || status(1) <= AP_GPS::NO_GPS) {
return;
}
// when we have a GPS then only send new data
if (last_send_time_ms == last_message_time_ms(1)) {
return;
}
last_send_time_ms = last_message_time_ms(1);
const Location &loc = location(1);
mavlink_msg_gps2_raw_send(
chan,
last_fix_time_ms(1)*(uint64_t)1000,
status(1),
loc.lat,
loc.lng,
loc.alt * 10UL,
get_hdop(1),
65535,
ground_speed(1)*100, // cm/s
ground_course_cd(1), // 1/100 degrees,
num_sats(1),
0,
0);
}
#endif