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

Rover : log multiple rangefinder instances in DPTH message 

This makes log_dpth method to log multiple instances of rangefinder

Co-Authored-By: Josh Henderson <69225461+hendjoshsr71@users.noreply.github.com>
This commit is contained in:
Shiv Tyagi 2021-09-29 23:14:02 +05:30 committed by Randy Mackay
parent 9a29f2558c
commit 6faa586dec
2 changed files with 40 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
Rover/Log.cpp
View File

@ -38,8 +38,8 @@ void Rover::Log_Write_Attitude()
// Write a range finder depth message
void Rover::Log_Write_Depth()
{
// only log depth on boats with working downward facing range finders
if (!rover.is_boat() || !rangefinder.has_data_orient(ROTATION_PITCH_270)) {
// only log depth on boats
if (!rover.is_boat() || !rangefinder.has_orientation(ROTATION_PITCH_270)) {
return;
}
@ -47,35 +47,43 @@ void Rover::Log_Write_Depth()
Location loc;
IGNORE_RETURN(ahrs.get_position(loc));
// check if new sensor reading has arrived
uint32_t reading_ms = rangefinder.last_reading_ms(ROTATION_PITCH_270);
if (reading_ms == rangefinder_last_reading_ms) {
return;
for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
const AP_RangeFinder_Backend *s = rangefinder.get_backend(i);
if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
continue;
}
// check if new sensor reading has arrived
const uint32_t reading_ms = s->last_reading_ms();
if (reading_ms == rangefinder_last_reading_ms[i]) {
continue;
}
rangefinder_last_reading_ms[i] = reading_ms;
float temp_C;
if (!s->get_temp(temp_C)) {
temp_C = 0.0f;
}
// @LoggerMessage: DPTH
// @Description: Depth messages on boats with downwards facing range finder
// @Field: TimeUS: Time since system startup
// @Field: Inst: Instance
// @Field: Lat: Latitude
// @Field: Lng: Longitude
// @Field: Depth: Depth as detected by the sensor
// @Field: Temp: Temperature
logger.Write("DPTH", "TimeUS,Inst,Lat,Lng,Depth,Temp",
"s#DUmO", "F-GG00", "QBLLff",
AP_HAL::micros64(),
i,
loc.lat,
loc.lng,
(double)(s->distance_cm() * 0.01f),
temp_C);
}
rangefinder_last_reading_ms = reading_ms;
// get temperature
float temp_C;
if (!rangefinder.get_temp(ROTATION_PITCH_270, temp_C)) {
temp_C = 0.0f;
}
// @LoggerMessage: DPTH
// @Description: Depth messages on boats with downwards facing range finder
// @Field: TimeUS: Time since system startup
// @Field: Lat: Latitude
// @Field: Lng: Longitude
// @Field: Depth: Depth as detected by the sensor
// @Field: Temp: Temperature
logger.Write("DPTH", "TimeUS,Lat,Lng,Depth,Temp",
"sDUmO", "FGG00", "QLLff",
AP_HAL::micros64(),
loc.lat,
loc.lng,
(double)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f),
temp_C);
// send water depth and temp to ground station
gcs().send_message(MSG_WATER_DEPTH);
}

4
Rover/Rover.h
View File

@ -216,8 +216,8 @@ private:
// true if we have a position estimate from AHRS
bool have_position;
// range finder last update (used for DPTH logging)
uint32_t rangefinder_last_reading_ms;
// range finder last update for each instance (used for DPTH logging)
uint32_t rangefinder_last_reading_ms[RANGEFINDER_MAX_INSTANCES];
// Ground speed
// The amount current ground speed is below min ground speed. meters per second