From cfe9dc32d18ff54cecb3d8cfc8363ec5a901f92e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Feb 2021 19:13:04 +1100 Subject: [PATCH] GCS_MAVLink: only send distance_sensor messages if valid data seen In the case you only have a forward-pointing LIDAR we'd send messages for each of the other orientations from proximty's horizontal-distances array, chewing up bandwidth and processing time. --- libraries/GCS_MAVLink/GCS.h | 9 +++++++-- libraries/GCS_MAVLink/GCS_Common.cpp | 11 +++++++++-- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 0381139d2b..d9f5383a3a 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -223,11 +223,11 @@ public: void send_power_status(void); void send_battery_status(const uint8_t instance) const; bool send_battery_status(); - void send_distance_sensor() const; + void send_distance_sensor(); // send_rangefinder sends only if a downward-facing instance is // found. Rover overrides this! virtual void send_rangefinder() const; - void send_proximity() const; + void send_proximity(); virtual void send_nav_controller_output() const = 0; virtual void send_pid_tuning() = 0; void send_ahrs2(); @@ -840,6 +840,11 @@ private: uint8_t last_battery_status_idx; + // if we've ever sent a DISTANCE_SENSOR message out of an + // orientation we continue to send it out, even if it is not + // longer valid. + uint8_t proximity_ever_valid_bitmask; + // true if we should NOT do MAVLink on this port (usually because // someone's doing SERIAL_CONTROL over mavlink) bool _locked; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ffe5611b07..dd8080d6d2 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -315,7 +315,7 @@ void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, con // send any and all distance_sensor messages. This starts by sending // any distance sensors not used by a Proximity sensor, then sends the // proximity sensor ones. -void GCS_MAVLINK::send_distance_sensor() const +void GCS_MAVLINK::send_distance_sensor() { RangeFinder *rangefinder = RangeFinder::get_singleton(); if (rangefinder == nullptr) { @@ -369,7 +369,7 @@ void GCS_MAVLINK::send_rangefinder() const s->voltage_mv() * 0.001f); } -void GCS_MAVLINK::send_proximity() const +void GCS_MAVLINK::send_proximity() { AP_Proximity *proximity = AP_Proximity::get_singleton(); if (proximity == nullptr) { @@ -388,6 +388,13 @@ void GCS_MAVLINK::send_proximity() const if (!HAVE_PAYLOAD_SPACE(chan, DISTANCE_SENSOR)) { return; } + if (dist_array.valid(i)) { + proximity_ever_valid_bitmask |= (1U << i); + } else if (!(proximity_ever_valid_bitmask & (1U << i))) { + // we've never sent this distance out, so we don't + // need to send an invalid one. + continue; + } mavlink_msg_distance_sensor_send( chan, AP_HAL::millis(), // time since system boot