aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: fixed sending of GPS_RAW_INT with multiple links

Browse Source 
thanks to ziltoid2 for this fix! See PR #2132
This commit is contained in:
Andrew Tridgell 2015-04-24 08:21:00 +10:00
parent eae9452cd1
commit f21aa17980
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_GPS/AP_GPS.cpp
View File

@ -475,20 +475,20 @@ AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint8_t len)
void
AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms;
static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS];
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)) {
if (last_send_time_ms[chan] == last_message_time_ms(0)) {
return;
}
last_send_time_ms = last_message_time_ms(0);
last_send_time_ms[chan] = last_message_time_ms(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) {
if (now - last_send_time_ms[chan] < 1000) {
return;
}
last_send_time_ms = now;
last_send_time_ms[chan] = now;
}
const Location &loc = location(0);
mavlink_msg_gps_raw_int_send(
@ -509,15 +509,15 @@ AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
void
AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms;
static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS];
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)) {
if (last_send_time_ms[chan] == last_message_time_ms(1)) {
return;
}
last_send_time_ms = last_message_time_ms(1);
last_send_time_ms[chan] = last_message_time_ms(1);
const Location &loc = location(1);
mavlink_msg_gps2_raw_send(