Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Peter Barker 2021-02-17 19:13:04 +11:00 committed by Randy Mackay
parent 9a66a1564c
commit cfe9dc32d1
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/GCS_MAVLink/GCS.h
View File

@ -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;

11
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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