From 4e167689b00bb429a8c33f2b8ea4d53c8826633d Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 27 May 2015 16:51:29 -0700 Subject: [PATCH] Plane: Log height of zero instead of old value --- ArduPlane/Log.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index d042b8eb1d..14caed1113 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -337,7 +337,7 @@ void Plane::Log_Write_Status() struct PACKED log_Sonar { LOG_PACKET_HEADER; uint64_t time_us; - float distance; + uint16_t distance; float voltage; float baro_alt; float groundspeed; @@ -350,10 +350,15 @@ struct PACKED log_Sonar { void Plane::Log_Write_Sonar() { #if RANGEFINDER_ENABLED == ENABLED + uint16_t distance = 0; + if (rangefinder.status() == RangeFinder::RangeFinder_Good) { + distance = rangefinder.distance_cm(); + } + struct log_Sonar pkt = { LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), time_us : hal.scheduler->micros64(), - distance : (float)rangefinder.distance_cm(), + distance : distance, voltage : rangefinder.voltage_mv()*0.001f, baro_alt : barometer.get_altitude(), groundspeed : gps.ground_speed(), @@ -478,7 +483,7 @@ static const struct LogStructure log_structure[] PROGMEM = { { LOG_NTUN_MSG, sizeof(log_Nav_Tuning), "NTUN", "QCfccccfI", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" }, { LOG_SONAR_MSG, sizeof(log_Sonar), - "SONR", "QffffBBf", "TimeUS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" }, + "SONR", "QHfffBBf", "TimeUS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" }, { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), "ARM", "QBH", "TimeUS,ArmState,ArmChecks" }, { LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP),