Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: cycle through rangefinders when sending WATER_DEPTH

Browse Source 
similarly to the way we do batteries, do not scale the amount of telemetry sent according to the number of backends we have.
This commit is contained in:
Peter Barker 2024-12-16 12:06:33 +11:00 committed by Randy Mackay
parent f94a698471
commit 169d6f6058
2 changed files with 20 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
Rover/GCS_Mavlink.cpp
View File

@ -186,7 +186,7 @@ void GCS_MAVLINK_Rover::send_rangefinder() const
voltage);
}
void GCS_MAVLINK_Rover::send_water_depth() const
void GCS_MAVLINK_Rover::send_water_depth()
{
if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) {
return;
@ -213,9 +213,15 @@ void GCS_MAVLINK_Rover::send_water_depth() const
Location loc;
IGNORE_RETURN(ahrs.get_location(loc));
for (uint8_t i=0; i<rangefinder->num_sensors(); i++) {
const AP_RangeFinder_Backend *s = rangefinder->get_backend(i);
const auto num_sensors = rangefinder->num_sensors();
for (uint8_t i=0; i<num_sensors; i++) {
last_WATER_DEPTH_index += 1;
if (last_WATER_DEPTH_index >= num_sensors) {
last_WATER_DEPTH_index = 0;
}
const AP_RangeFinder_Backend *s = rangefinder->get_backend(last_WATER_DEPTH_index);
if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
continue;
}
@ -231,7 +237,7 @@ void GCS_MAVLINK_Rover::send_water_depth() const
mavlink_msg_water_depth_send(
chan,
AP_HAL::millis(), // time since system boot TODO: take time of measurement
i, // rangefinder instance
last_WATER_DEPTH_index, // rangefinder instance
sensor_healthy, // sensor healthy
loc.lat, // latitude of vehicle
loc.lng, // longitude of vehicle
@ -241,6 +247,8 @@ void GCS_MAVLINK_Rover::send_water_depth() const
ahrs.get_yaw(), // yaw in radians
s->distance(), // distance in meters
temp_C); // temperature in degC
break; // only send one WATER_DEPTH message per loop
}
}

10
Rover/GCS_Mavlink.h
View File

@ -42,9 +42,6 @@ protected:
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
// send WATER_DEPTH - metres and temperature
void send_water_depth() const;
private:
void handle_message(const mavlink_message_t &msg) override;
@ -69,6 +66,13 @@ private:
#if AP_RANGEFINDER_ENABLED
void send_rangefinder() const override;
// send WATER_DEPTH - metres and temperature
void send_water_depth();
// state variable for the last rangefinder we sent a WATER_DEPTH
// message for. We cycle through the rangefinder backends to
// limit the amount of telemetry bandwidth we consume.
uint8_t last_WATER_DEPTH_index;
#endif
#if HAL_HIGH_LATENCY2_ENABLED