From 88a230e897ef8d416a37878d0f7637bd968c4a4e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 27 Aug 2018 16:03:43 +0900 Subject: [PATCH] Rover: DPTH message only written if range finder last read time changed --- APMrover2/Log.cpp | 7 +++++++ APMrover2/Rover.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index ebed896252..47f44644e4 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -60,6 +60,13 @@ void Rover::Log_Write_Depth() return; } + // 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; + } + rangefinder_last_reading_ms = reading_ms; + DataFlash.Log_Write("DPTH", "TimeUS,Lat,Lng,Depth", "sDUm", "FGG0", "QLLf", AP_HAL::micros64(), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index d7fe194102..5fd48f2797 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -280,6 +280,9 @@ private: uint32_t detected_time_ms; } obstacle; + // range finder last update (used for DPTH logging) + uint32_t rangefinder_last_reading_ms; + // Ground speed // The amount current ground speed is below min ground speed. meters per second float ground_speed;