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

GCS_MAVLink: send gps messages as separate queued messages

Browse Source
This commit is contained in:
Peter Barker 2017-08-08 20:01:26 +10:00 committed by Francisco Ferreira
parent b14d99dc2c
commit 7aaabea4ea
2 changed files with 25 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/GCS_MAVLink/GCS.h
View File

@ -45,6 +45,9 @@ enum ap_message {
MSG_RAW_IMU2,
MSG_RAW_IMU3,
MSG_GPS_RAW,
MSG_GPS_RTK,
MSG_GPS2_RAW,
MSG_GPS2_RTK,
MSG_SYSTEM_TIME,
MSG_SERVO_OUT,
MSG_NEXT_WAYPOINT,

61
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -921,44 +921,6 @@ GCS_MAVLINK::update(run_cli_fn run_cli, uint32_t max_time_us)
}
/*
send raw GPS position information (GPS_RAW_INT, GPS2_RAW, GPS_RTK and GPS2_RTK).
returns true if messages fit into transmit buffer, false otherwise.
*/
bool GCS_MAVLINK::send_gps_raw(AP_GPS &gps)
{
if (HAVE_PAYLOAD_SPACE(chan, GPS_RAW_INT)) {
gps.send_mavlink_gps_raw(chan);
} else {
return false;
}
if (gps.highest_supported_status(0) > AP_GPS::GPS_OK_FIX_3D) {
if (HAVE_PAYLOAD_SPACE(chan, GPS_RTK)) {
gps.send_mavlink_gps_rtk(chan);
}
}
if (gps.num_sensors() > 1 && gps.status(1) > AP_GPS::NO_GPS) {
if (HAVE_PAYLOAD_SPACE(chan, GPS2_RAW)) {
gps.send_mavlink_gps2_raw(chan);
}
if (gps.highest_supported_status(1) > AP_GPS::GPS_OK_FIX_3D) {
if (HAVE_PAYLOAD_SPACE(chan, GPS2_RTK)) {
gps.send_mavlink_gps2_rtk(chan);
}
}
}
//TODO: Should check what else managed to get through...
return true;
}
/*
send the SYSTEM_TIME message
*/
@ -2129,7 +2091,22 @@ bool GCS_MAVLINK::try_send_gps_message(const enum ap_message id)
break;
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
send_gps_raw(*gps);
gps->send_mavlink_gps_raw(chan);
ret = true;
break;
case MSG_GPS_RTK:
CHECK_PAYLOAD_SIZE(GPS_RTK);
gps->send_mavlink_gps_rtk(chan);
ret = true;
break;
case MSG_GPS2_RAW:
CHECK_PAYLOAD_SIZE(GPS2_RAW);
gps->send_mavlink_gps2_raw(chan);
ret = true;
break;
case MSG_GPS2_RTK:
CHECK_PAYLOAD_SIZE(GPS2_RTK);
gps->send_mavlink_gps2_rtk(chan);
ret = true;
break;
default:
@ -2209,6 +2186,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
case MSG_GPS_RAW:
/* fall through */
case MSG_GPS_RTK:
/* fall through */
case MSG_GPS2_RAW:
/* fall through */
case MSG_GPS2_RTK:
/* fall through */
case MSG_SYSTEM_TIME:
ret = try_send_gps_message(id);
break;