Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_Common : send water depth for each rangefinder instance

Browse Source 
This sends depth message for each downward facing rangefinder instance which has data.

Co-Authored-By: Josh Henderson <69225461+hendjoshsr71@users.noreply.github.com>
This commit is contained in:
Shiv Tyagi 2021-09-29 23:15:07 +05:30 committed by Randy Mackay
parent 6faa586dec
commit 5cca39e50a
1 changed files with 31 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -4866,35 +4866,45 @@ void GCS_MAVLINK::send_water_depth() const
} }
RangeFinder *rangefinder = RangeFinder::get_singleton(); RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder == nullptr || !rangefinder->has_data_orient(ROTATION_PITCH_270)) {
// no rangefinder or not facing downwards if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){
return; return;
} }
const bool sensor_healthy = (rangefinder->status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good);
// get position // get position
const AP_AHRS &ahrs = AP::ahrs(); const AP_AHRS &ahrs = AP::ahrs();
Location loc; Location loc;
IGNORE_RETURN(ahrs.get_position(loc)); IGNORE_RETURN(ahrs.get_position(loc));
// get temperature for (uint8_t i=0; i<rangefinder->num_sensors(); i++) {
float temp_C = 0.0f; const AP_RangeFinder_Backend *s = rangefinder->get_backend(i);
IGNORE_RETURN(rangefinder->get_temp(ROTATION_PITCH_270, temp_C));
mavlink_msg_water_depth_send( if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
chan, continue;
AP_HAL::millis(), // time since system boot TODO: take time of measurement }
0, // sensor id always zero
sensor_healthy, // sensor healthy // get temperature
loc.lat, // latitude of vehicle float temp_C;
loc.lng, // longitude of vehicle if (!s->get_temp(temp_C)) {
loc.alt * 0.01f, // altitude of vehicle (MSL) temp_C = 0.0f;
ahrs.get_roll(), // roll in radians }
ahrs.get_pitch(), // pitch in radians
ahrs.get_yaw(), // yaw in radians const bool sensor_healthy = (s->status() == RangeFinder::Status::Good);
rangefinder->distance_cm_orient(ROTATION_PITCH_270) * 0.01f, // distance in meters
temp_C); // temperature in degC mavlink_msg_water_depth_send(
chan,
AP_HAL::millis(), // time since system boot TODO: take time of measurement
i, // rangefinder instance
sensor_healthy, // sensor healthy
loc.lat, // latitude of vehicle
loc.lng, // longitude of vehicle
loc.alt * 0.01f, // altitude of vehicle (MSL)
ahrs.get_roll(), // roll in radians
ahrs.get_pitch(), // pitch in radians
ahrs.get_yaw(), // yaw in radians
s->distance_cm() * 0.01f, // distance in meters
temp_C); // temperature in degC
}
#endif #endif
} }