mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 07:13:56 -04:00
Rover: DPTH message logs water temperature
This commit is contained in:
parent
fee30b4447
commit
09718a0f11
@ -57,19 +57,27 @@ void Rover::Log_Write_Depth()
|
||||
}
|
||||
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",
|
||||
"sDUm", "FGG0", "QLLf",
|
||||
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));
|
||||
(double)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f),
|
||||
temp_C);
|
||||
}
|
||||
|
||||
// guided mode logging
|
||||
|
Loading…
Reference in New Issue
Block a user