From 5aef5473ded1ef2458bfea8756debf522835446f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Oct 2022 09:33:38 +1100 Subject: [PATCH] Plane: log TECS target alt log the target alt we pass into TECS to help debug a CRUISE height issue --- ArduPlane/ArduPlane.cpp | 6 +++--- ArduPlane/Log.cpp | 17 ++++++++++------- ArduPlane/Plane.h | 3 +++ 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 61b48b4bf2..f3887c6951 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -547,16 +547,16 @@ void Plane::update_alt() distance_beyond_land_wp = current_loc.get_distance(next_WP_loc); } - float target_alt = relative_target_altitude_cm(); + tecs_target_alt_cm = relative_target_altitude_cm(); if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) { // ensure we do the initial climb in RTL. We add an extra // 10m in the demanded height to push TECS to climb // quickly - target_alt = MAX(target_alt, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100; + tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100; } - TECS_controller.update_pitch_throttle(target_alt, + TECS_controller.update_pitch_throttle(tecs_target_alt_cm, target_airspeed_cm, flight_stage, distance_beyond_land_wp, diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index fae1750883..8ed1f0d575 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -147,7 +147,8 @@ struct PACKED log_Nav_Tuning { float airspeed_error; int32_t target_lat; int32_t target_lng; - int32_t target_alt; + int32_t target_alt_wp; + int32_t target_alt_tecs; int32_t target_airspeed; }; @@ -166,7 +167,8 @@ void Plane::Log_Write_Nav_Tuning() airspeed_error : airspeed_error, target_lat : next_WP_loc.lat, target_lng : next_WP_loc.lng, - target_alt : next_WP_loc.alt, + target_alt_wp : next_WP_loc.alt, + target_alt_tecs : tecs_target_alt_cm, target_airspeed : target_airspeed_cm, }; logger.WriteBlock(&pkt, sizeof(pkt)); @@ -306,16 +308,17 @@ const struct LogStructure Plane::log_structure[] = { // @Field: Dist: distance to the current navigation waypoint // @Field: TBrg: bearing to the current navigation waypoint // @Field: NavBrg: the vehicle's desired heading -// @Field: AltErr: difference between current vehicle height and target height +// @Field: AltE: difference between current vehicle height and target height // @Field: XT: the vehicle's current distance from the current travel segment // @Field: XTi: integration of the vehicle's crosstrack error -// @Field: AspdE: difference between vehicle's airspeed and desired airspeed +// @Field: AsE: difference between vehicle's airspeed and desired airspeed // @Field: TLat: target latitude // @Field: TLng: target longitude -// @Field: TAlt: target altitude -// @Field: TAspd: target airspeed +// @Field: TAW: target altitude WP +// @Field: TAT: target altitude TECS +// @Field: TAsp: target airspeed { LOG_NTUN_MSG, sizeof(log_Nav_Tuning), - "NTUN", "QfcccfffLLii", "TimeUS,Dist,TBrg,NavBrg,AltErr,XT,XTi,AspdE,TLat,TLng,TAlt,TAspd", "smddmmmnDUmn", "F0BBB0B0GGBB" , true }, + "NTUN", "QfcccfffLLeee", "TimeUS,Dist,TBrg,NavBrg,AltE,XT,XTi,AsE,TLat,TLng,TAW,TAT,TAsp", "smddmmmnDUmmn", "F0BBB0B0GG000" , true }, // @LoggerMessage: ATRP // @Description: Plane AutoTune diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b210e11443..637c1b169a 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1204,6 +1204,9 @@ private: // mode reason for entering previous mode ModeReason previous_mode_reason = ModeReason::UNKNOWN; + // last target alt we passed to tecs + int32_t tecs_target_alt_cm; + public: void failsafe_check(void); #if AP_SCRIPTING_ENABLED