mirror of https://github.com/ArduPilot/ardupilot
MAVLink: don't waste 100 bytes of bandwidth sending 1 byte of information
the GPS_STATUS message is a massive waste of bandwidth, but it is the only message that tells us the number of visible satellites. So only send it if that information changes. This makes MAVLink work better at low baud rates
This commit is contained in:
parent
e9773ea09c
commit
1e0a2fb0be
|
@ -43,6 +43,7 @@ public:
|
|||
void init(FastSerial *port) {
|
||||
_port = port;
|
||||
initialised = true;
|
||||
last_gps_satellites = 255;
|
||||
}
|
||||
|
||||
/// Update GCS state.
|
||||
|
@ -89,6 +90,9 @@ public:
|
|||
// set to true if this GCS link is active
|
||||
bool initialised;
|
||||
|
||||
// used to prevent wasting bandwidth with GPS_STATUS messages
|
||||
uint8_t last_gps_satellites;
|
||||
|
||||
protected:
|
||||
/// The stream we are communicating over
|
||||
FastSerial *_port;
|
||||
|
|
|
@ -697,11 +697,18 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
|
|||
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
|
||||
send_message(MSG_EXTENDED_STATUS1);
|
||||
send_message(MSG_EXTENDED_STATUS2);
|
||||
send_message(MSG_GPS_STATUS);
|
||||
send_message(MSG_CURRENT_WAYPOINT);
|
||||
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
|
||||
send_message(MSG_NAV_CONTROLLER_OUTPUT);
|
||||
//Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get());
|
||||
|
||||
if (last_gps_satellites != g_gps->num_sats) {
|
||||
// this message is mostly a huge waste of bandwidth,
|
||||
// except it is the only message that gives the number
|
||||
// of visible satellites. So only send it when that
|
||||
// changes.
|
||||
send_message(MSG_GPS_STATUS);
|
||||
last_gps_satellites = g_gps->num_sats;
|
||||
}
|
||||
}
|
||||
|
||||
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
|
||||
|
|
|
@ -43,6 +43,7 @@ public:
|
|||
void init(FastSerial *port) {
|
||||
_port = port;
|
||||
initialised = true;
|
||||
last_gps_satellites = 255;
|
||||
}
|
||||
|
||||
/// Update GCS state.
|
||||
|
@ -89,6 +90,9 @@ public:
|
|||
// set to true if this GCS link is active
|
||||
bool initialised;
|
||||
|
||||
// used to prevent wasting bandwidth with GPS_STATUS messages
|
||||
uint8_t last_gps_satellites;
|
||||
|
||||
protected:
|
||||
/// The stream we are communicating over
|
||||
FastSerial *_port;
|
||||
|
|
|
@ -900,11 +900,19 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
|
|||
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
|
||||
send_message(MSG_EXTENDED_STATUS1);
|
||||
send_message(MSG_EXTENDED_STATUS2);
|
||||
send_message(MSG_GPS_STATUS);
|
||||
send_message(MSG_CURRENT_WAYPOINT);
|
||||
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
|
||||
send_message(MSG_NAV_CONTROLLER_OUTPUT);
|
||||
send_message(MSG_FENCE_STATUS);
|
||||
|
||||
if (last_gps_satellites != g_gps->num_sats) {
|
||||
// this message is mostly a huge waste of bandwidth,
|
||||
// except it is the only message that gives the number
|
||||
// of visible satellites. So only send it when that
|
||||
// changes.
|
||||
send_message(MSG_GPS_STATUS);
|
||||
last_gps_satellites = g_gps->num_sats;
|
||||
}
|
||||
}
|
||||
|
||||
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
|
||||
|
|
Loading…
Reference in New Issue