diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 4a6ecb4a20..4fffc249d6 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -40,6 +40,28 @@ void Rover::Log_Write_Attitude() DataFlash.Log_Write_PID(LOG_PIDA_MSG, g2.attitude_control.get_throttle_speed_pid().get_pid_info()); } +// 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)) { + return; + } + + // get position + Location loc; + if (!rover.ahrs.get_position(loc)) { + return; + } + + DataFlash.Log_Write("DPTH", "TimeUS,Lat,Lng,Depth", + "sDUm", "FGG0", "QLLf", + AP_HAL::micros64(), + loc.lat, + loc.lng, + (double)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f)); +} + struct PACKED log_Error { LOG_PACKET_HEADER; uint64_t time_us; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 310d04ca1c..3910be1fca 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -478,6 +478,7 @@ private: // Log.cpp void Log_Write_Arm_Disarm(); void Log_Write_Attitude(); + void Log_Write_Depth(); void Log_Write_Error(uint8_t sub_system, uint8_t error_code); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_Nav_Tuning(); diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index b5e7f6327c..c37cd7f3f0 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -226,6 +226,7 @@ void Rover::read_rangefinders(void) } Log_Write_Rangefinder(); + Log_Write_Depth(); // no object detected - reset after the turn time if (obstacle.detected_count >= g.rangefinder_debounce &&