From e338e4cdb67603a46241894e92f09e9a9987b143 Mon Sep 17 00:00:00 2001 From: Joshua Henderson Date: Thu, 28 Jul 2022 19:43:09 -0400 Subject: [PATCH] AP_TECS: use TECS log bitmask in constructor --- libraries/AP_TECS/AP_TECS.cpp | 138 +++++++++++++++++----------------- libraries/AP_TECS/AP_TECS.h | 8 +- 2 files changed, 76 insertions(+), 70 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 1651f702ab..b779d71cce 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -1147,72 +1147,74 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // Calculate pitch demand _update_pitch(); - // log to AP_Logger - // @LoggerMessage: TECS - // @Vehicles: Plane - // @Description: Information about the Total Energy Control System - // @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html - // @Field: TimeUS: Time since system startup - // @Field: h: height estimate (UP) currently in use by TECS - // @Field: dh: current climb rate ("delta-height") - // @Field: hdem: height TECS is currently trying to achieve - // @Field: dhdem: climb rate TECS is currently trying to achieve - // @Field: spdem: True AirSpeed TECS is currently trying to achieve - // @Field: sp: current estimated True AirSpeed - // @Field: dsp: x-axis acceleration estimate ("delta-speed") - // @Field: ith: throttle integrator value - // @Field: iph: Specific Energy Balance integrator value - // @Field: th: throttle output - // @Field: ph: pitch output - // @Field: dspdem: demanded acceleration output ("delta-speed demand") - // @Field: w: current TECS prioritization of height vs speed (0==100% height,2==100% speed, 1==50%height+50%speed - // @Field: f: flags - // @FieldBits: f: Underspeed,UnachievableDescent,AutoLanding,ReachedTakeoffSpd - AP::logger().WriteStreaming( - "TECS", - "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f", - "smnmnnnn----o--", - "F0000000----0--", - "QfffffffffffffB", - now, - (double)_height, - (double)_climb_rate, - (double)_hgt_dem_adj, - (double)_hgt_rate_dem, - (double)_TAS_dem_adj, - (double)_TAS_state, - (double)_vel_dot, - (double)_integTHR_state, - (double)_integSEB_state, - (double)_throttle_dem, - (double)_pitch_dem, - (double)_TAS_rate_dem, - (double)logging.SKE_weighting, - _flags_byte); - // @LoggerMessage: TEC2 - // @Vehicles: Plane - // @Description: Additional Information about the Total Energy Control System - // @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html - // @Field: TimeUS: Time since system startup - // @Field: pmax: maximum allowed pitch from parameter - // @Field: pmin: minimum allowed pitch from parameter - // @Field: KErr: difference between estimated kinetic energy and desired kinetic energy - // @Field: PErr: difference between estimated potential energy and desired potential energy - // @Field: EDelta: current error in speed/balance weighting - // @Field: LF: aerodynamic load factor - // @Field: hdem1: demanded height input - // @Field: hdem2: rate-limited height demand - AP::logger().WriteStreaming("TEC2", "TimeUS,pmax,pmin,KErr,PErr,EDelta,LF,hdem1,hdem2", - "s------mm", - "F--------", - "Qffffffff", - now, - (double)degrees(_PITCHmaxf), - (double)degrees(_PITCHminf), - (double)logging.SKE_error, - (double)logging.SPE_error, - (double)logging.SEB_delta, - (double)load_factor, - (double)hgt_dem_cm*0.01, - (double)_hgt_dem); + if (AP::logger().should_log(_log_bitmask)){ + // log to AP_Logger + // @LoggerMessage: TECS + // @Vehicles: Plane + // @Description: Information about the Total Energy Control System + // @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html + // @Field: TimeUS: Time since system startup + // @Field: h: height estimate (UP) currently in use by TECS + // @Field: dh: current climb rate ("delta-height") + // @Field: hdem: height TECS is currently trying to achieve + // @Field: dhdem: climb rate TECS is currently trying to achieve + // @Field: spdem: True AirSpeed TECS is currently trying to achieve + // @Field: sp: current estimated True AirSpeed + // @Field: dsp: x-axis acceleration estimate ("delta-speed") + // @Field: ith: throttle integrator value + // @Field: iph: Specific Energy Balance integrator value + // @Field: th: throttle output + // @Field: ph: pitch output + // @Field: dspdem: demanded acceleration output ("delta-speed demand") + // @Field: w: current TECS prioritization of height vs speed (0==100% height,2==100% speed, 1==50%height+50%speed + // @Field: f: flags + // @FieldBits: f: Underspeed,UnachievableDescent,AutoLanding,ReachedTakeoffSpd + AP::logger().WriteStreaming( + "TECS", + "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f", + "smnmnnnn----o--", + "F0000000----0--", + "QfffffffffffffB", + now, + (double)_height, + (double)_climb_rate, + (double)_hgt_dem_adj, + (double)_hgt_rate_dem, + (double)_TAS_dem_adj, + (double)_TAS_state, + (double)_vel_dot, + (double)_integTHR_state, + (double)_integSEB_state, + (double)_throttle_dem, + (double)_pitch_dem, + (double)_TAS_rate_dem, + (double)logging.SKE_weighting, + _flags_byte); + // @LoggerMessage: TEC2 + // @Vehicles: Plane + // @Description: Additional Information about the Total Energy Control System + // @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html + // @Field: TimeUS: Time since system startup + // @Field: pmax: maximum allowed pitch from parameter + // @Field: pmin: minimum allowed pitch from parameter + // @Field: KErr: difference between estimated kinetic energy and desired kinetic energy + // @Field: PErr: difference between estimated potential energy and desired potential energy + // @Field: EDelta: current error in speed/balance weighting + // @Field: LF: aerodynamic load factor + // @Field: hdem1: demanded height input + // @Field: hdem2: rate-limited height demand + AP::logger().WriteStreaming("TEC2", "TimeUS,pmax,pmin,KErr,PErr,EDelta,LF,hdem1,hdem2", + "s------mm", + "F--------", + "Qffffffff", + now, + (double)degrees(_PITCHmaxf), + (double)degrees(_PITCHminf), + (double)logging.SKE_error, + (double)logging.SPE_error, + (double)logging.SEB_delta, + (double)load_factor, + (double)hgt_dem_cm*0.01, + (double)_hgt_dem); + } } diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 6d6ed2c0bd..7b62492231 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -26,10 +26,11 @@ class AP_Landing; class AP_TECS { public: - AP_TECS(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, const AP_Landing &landing) + AP_TECS(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, const AP_Landing &landing, const uint32_t log_bitmask) : _ahrs(ahrs) , aparm(parms) , _landing(landing) + , _log_bitmask(log_bitmask) { AP_Param::setup_object_defaults(this, var_info); } @@ -161,7 +162,10 @@ private: // reference to const AP_Landing to access it's params const AP_Landing &_landing; - + + // Logging bitmask + const uint32_t _log_bitmask; + // TECS tuning parameters AP_Float _hgtCompFiltOmega; AP_Float _spdCompFiltOmega;