diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index 7047beaf09..6a1fc4182c 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -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; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8cb0957367..bdd0f217cd 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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)) { diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 7047beaf09..6a1fc4182c 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -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; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 2cede18da0..3d7e2d35b3 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -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)) {