From f45c69e7adf16f5be7614eab4fe86ef8bbcf0288 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 18 Apr 2016 15:15:56 +0900 Subject: [PATCH] Copter: log terrain altitude in CTUN message --- ArduCopter/Log.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index ef06a96693..9547c2fa63 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -306,6 +306,7 @@ struct PACKED log_Control_Tuning { int32_t baro_alt; int16_t desired_sonar_alt; int16_t sonar_alt; + float terr_alt; int16_t desired_climb_rate; int16_t climb_rate; }; @@ -313,6 +314,12 @@ struct PACKED log_Control_Tuning { // Write a control tuning packet void Copter::Log_Write_Control_Tuning() { + // get terrain altitude + float terr_alt = 0.0f; +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + terrain.height_above_terrain(terr_alt, true); +#endif + struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), time_us : AP_HAL::micros64(), @@ -324,6 +331,7 @@ void Copter::Log_Write_Control_Tuning() baro_alt : baro_alt, desired_sonar_alt : (int16_t)target_sonar_alt, sonar_alt : sonar_alt, + terr_alt : terr_alt, desired_climb_rate : (int16_t)pos_control.get_vel_target_z(), climb_rate : climb_rate }; @@ -716,7 +724,7 @@ const struct LogStructure Copter::log_structure[] = { { LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning), "NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" }, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qfffffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" }, + "CTUN", "Qfffffeccfhh", "TimeUS,ThrIn,ABst,ThrOut,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt" }, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "QHHIhBHI", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,LogDrop" }, { LOG_MOTBATT_MSG, sizeof(log_MotBatt),