zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_TECS: use TECS log bitmask in constructor

master
Joshua Henderson 3 years ago committed by Andrew Tridgell
parent
a9da91bc8d
commit
e338e4cdb6
2 changed files with 76 additions and 70 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 138
      libraries/AP_TECS/AP_TECS.cpp
  2. 8
      libraries/AP_TECS/AP_TECS.h

138
libraries/AP_TECS/AP_TECS.cpp
Unescape View File

@ -1147,72 +1147,74 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
// Calculate pitch demand // Calculate pitch demand
_update_pitch(); _update_pitch();
// log to AP_Logger if (AP::logger().should_log(_log_bitmask)){
// @LoggerMessage: TECS // log to AP_Logger
// @Vehicles: Plane // @LoggerMessage: TECS
// @Description: Information about the Total Energy Control System // @Vehicles: Plane
// @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html // @Description: Information about the Total Energy Control System
// @Field: TimeUS: Time since system startup // @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
// @Field: h: height estimate (UP) currently in use by TECS // @Field: TimeUS: Time since system startup
// @Field: dh: current climb rate ("delta-height") // @Field: h: height estimate (UP) currently in use by TECS
// @Field: hdem: height TECS is currently trying to achieve // @Field: dh: current climb rate ("delta-height")
// @Field: dhdem: climb rate TECS is currently trying to achieve // @Field: hdem: height TECS is currently trying to achieve
// @Field: spdem: True AirSpeed TECS is currently trying to achieve // @Field: dhdem: climb rate TECS is currently trying to achieve
// @Field: sp: current estimated True AirSpeed // @Field: spdem: True AirSpeed TECS is currently trying to achieve
// @Field: dsp: x-axis acceleration estimate ("delta-speed") // @Field: sp: current estimated True AirSpeed
// @Field: ith: throttle integrator value // @Field: dsp: x-axis acceleration estimate ("delta-speed")
// @Field: iph: Specific Energy Balance integrator value // @Field: ith: throttle integrator value
// @Field: th: throttle output // @Field: iph: Specific Energy Balance integrator value
// @Field: ph: pitch output // @Field: th: throttle output
// @Field: dspdem: demanded acceleration output ("delta-speed demand") // @Field: ph: pitch output
// @Field: w: current TECS prioritization of height vs speed (0==100% height,2==100% speed, 1==50%height+50%speed // @Field: dspdem: demanded acceleration output ("delta-speed demand")
// @Field: f: flags // @Field: w: current TECS prioritization of height vs speed (0==100% height,2==100% speed, 1==50%height+50%speed
// @FieldBits: f: Underspeed,UnachievableDescent,AutoLanding,ReachedTakeoffSpd // @Field: f: flags
AP::logger().WriteStreaming( // @FieldBits: f: Underspeed,UnachievableDescent,AutoLanding,ReachedTakeoffSpd
"TECS", AP::logger().WriteStreaming(
"TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f", "TECS",
"smnmnnnn----o--", "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f",
"F0000000----0--", "smnmnnnn----o--",
"QfffffffffffffB", "F0000000----0--",
now, "QfffffffffffffB",
(double)_height, now,
(double)_climb_rate, (double)_height,
(double)_hgt_dem_adj, (double)_climb_rate,
(double)_hgt_rate_dem, (double)_hgt_dem_adj,
(double)_TAS_dem_adj, (double)_hgt_rate_dem,
(double)_TAS_state, (double)_TAS_dem_adj,
(double)_vel_dot, (double)_TAS_state,
(double)_integTHR_state, (double)_vel_dot,
(double)_integSEB_state, (double)_integTHR_state,
(double)_throttle_dem, (double)_integSEB_state,
(double)_pitch_dem, (double)_throttle_dem,
(double)_TAS_rate_dem, (double)_pitch_dem,
(double)logging.SKE_weighting, (double)_TAS_rate_dem,
_flags_byte); (double)logging.SKE_weighting,
// @LoggerMessage: TEC2 _flags_byte);
// @Vehicles: Plane // @LoggerMessage: TEC2
// @Description: Additional Information about the Total Energy Control System // @Vehicles: Plane
// @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html // @Description: Additional Information about the Total Energy Control System
// @Field: TimeUS: Time since system startup // @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
// @Field: pmax: maximum allowed pitch from parameter // @Field: TimeUS: Time since system startup
// @Field: pmin: minimum allowed pitch from parameter // @Field: pmax: maximum allowed pitch from parameter
// @Field: KErr: difference between estimated kinetic energy and desired kinetic energy // @Field: pmin: minimum allowed pitch from parameter
// @Field: PErr: difference between estimated potential energy and desired potential energy // @Field: KErr: difference between estimated kinetic energy and desired kinetic energy
// @Field: EDelta: current error in speed/balance weighting // @Field: PErr: difference between estimated potential energy and desired potential energy
// @Field: LF: aerodynamic load factor // @Field: EDelta: current error in speed/balance weighting
// @Field: hdem1: demanded height input // @Field: LF: aerodynamic load factor
// @Field: hdem2: rate-limited height demand // @Field: hdem1: demanded height input
AP::logger().WriteStreaming("TEC2", "TimeUS,pmax,pmin,KErr,PErr,EDelta,LF,hdem1,hdem2", // @Field: hdem2: rate-limited height demand
"s------mm", AP::logger().WriteStreaming("TEC2", "TimeUS,pmax,pmin,KErr,PErr,EDelta,LF,hdem1,hdem2",
"F--------", "s------mm",
"Qffffffff", "F--------",
now, "Qffffffff",
(double)degrees(_PITCHmaxf), now,
(double)degrees(_PITCHminf), (double)degrees(_PITCHmaxf),
(double)logging.SKE_error, (double)degrees(_PITCHminf),
(double)logging.SPE_error, (double)logging.SKE_error,
(double)logging.SEB_delta, (double)logging.SPE_error,
(double)load_factor, (double)logging.SEB_delta,
(double)hgt_dem_cm*0.01, (double)load_factor,
(double)_hgt_dem); (double)hgt_dem_cm*0.01,
(double)_hgt_dem);
}
} }

8
libraries/AP_TECS/AP_TECS.h
Unescape View File

@ -26,10 +26,11 @@
class AP_Landing; class AP_Landing;
class AP_TECS { class AP_TECS {
public: 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) : _ahrs(ahrs)
, aparm(parms) , aparm(parms)
, _landing(landing) , _landing(landing)
, _log_bitmask(log_bitmask)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -161,7 +162,10 @@ private:
// reference to const AP_Landing to access it's params // reference to const AP_Landing to access it's params
const AP_Landing &_landing; const AP_Landing &_landing;
// Logging bitmask
const uint32_t _log_bitmask;
// TECS tuning parameters // TECS tuning parameters
AP_Float _hgtCompFiltOmega; AP_Float _hgtCompFiltOmega;
AP_Float _spdCompFiltOmega; AP_Float _spdCompFiltOmega;

Write Preview
Loading…
Cancel
Save