From 221c222ca257862aec6876ce1774cb690e13150d Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 19 Aug 2015 22:43:13 +0200 Subject: [PATCH 1/2] comment out code which relied on old tecs logging format --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index cfba54d452..c07956fd6e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight * is running) */ limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); - /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; - _status.flightPathAngleFiltered = flightPathAngleFiltered; - _status.airspeedDerivativeSp = airspeedDerivativeSp; - _status.airspeedDerivative = airspeedDerivative; - _status.totalEnergyRateSp = totalEnergyRateSp; - _status.totalEnergyRate = totalEnergyRate; - _status.energyDistributionRateSp = energyDistributionRateSp; - _status.energyDistributionRate = energyDistributionRate; - _status.mode = mode; + // /* Write part of the status message */ + // _status.flightPathAngleSp = flightPathAngleSp; + // _status.flightPathAngle = flightPathAngle; + // _status.flightPathAngleFiltered = flightPathAngleFiltered; + // _status.airspeedDerivativeSp = airspeedDerivativeSp; + // _status.airspeedDerivative = airspeedDerivative; + // _status.totalEnergyRateSp = totalEnergyRateSp; + // _status.totalEnergyRate = totalEnergyRate; + // _status.energyDistributionRateSp = energyDistributionRateSp; + // _status.energyDistributionRate = energyDistributionRate; + // _status.mode = mode; /** update control blocks **/ /* update total energy rate control block */ From 6cdceb1829d2674c27a9e53639badd9c901fe03d Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 19 Aug 2015 22:43:42 +0200 Subject: [PATCH 2/2] fix tecs status logging --- msg/tecs_status.msg | 11 +++-- src/lib/external_lgpl/tecs/tecs.cpp | 49 +++++++++++-------- src/lib/external_lgpl/tecs/tecs.h | 41 +++++++++++----- .../fw_pos_control_l1_main.cpp | 29 ++++++----- src/modules/sdlog2/sdlog2.c | 11 +++-- src/modules/sdlog2/sdlog2_messages.h | 14 +++--- 6 files changed, 94 insertions(+), 61 deletions(-) diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index ccac921e2b..7e9d6670c9 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -18,9 +18,12 @@ float32 airspeed_filtered float32 airspeedDerivativeSp float32 airspeedDerivative -float32 totalEnergyRateSp -float32 totalEnergyRate -float32 energyDistributionRateSp -float32 energyDistributionRate +float32 totalEnergyError +float32 energyDistributionError +float32 totalEnergyRateError +float32 energyDistributionRateError + +float32 throttle_sp +float32 pitch_sp uint8 mode diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6f049d4f52..9e47590fd2 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -309,12 +309,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); - float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; + _STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement - STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast; - _STEdotErrLast = STEdot_error; + _STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast; + _STEdotErrLast = _STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle @@ -342,7 +342,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later - _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle @@ -438,11 +438,11 @@ void TECS::_update_pitch(void) // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; - float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); - float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); + _SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + _SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded - float integ7_input = SEB_error * _integGain; + float integ7_input = _SEB_error * _integGain; if (_pitch_dem_unc > _PITCHmaxf) { integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); @@ -460,7 +460,7 @@ void TECS::_update_pitch(void) // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); - float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; if (_climbOutDem) { temp += _PITCHminf * gainInv; @@ -598,18 +598,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _tecs_state.mode = ECL_TECS_MODE_NORMAL; } - _tecs_state.hgt_dem = _hgt_dem_adj; - _tecs_state.hgt = _integ3_state; - _tecs_state.dhgt_dem = _hgt_rate_dem; - _tecs_state.dhgt = _integ2_state; - _tecs_state.spd_dem = _TAS_dem_adj; - _tecs_state.spd = _integ5_state; - _tecs_state.dspd = _vel_dot; - _tecs_state.ithr = _integ6_state; - _tecs_state.iptch = _integ7_state; - _tecs_state.thr = _throttle_dem; - _tecs_state.ptch = _pitch_dem; - _tecs_state.dspd_dem = _TAS_rate_dem; + _tecs_state.altitude_sp = _hgt_dem_adj; + _tecs_state.altitude_filtered = _integ3_state; + _tecs_state.altitude_rate_sp = _hgt_rate_dem; + _tecs_state.altitude_rate = _integ2_state; + + _tecs_state.airspeed_sp = _TAS_dem_adj; + _tecs_state.airspeed_rate_sp = _TAS_rate_dem; + _tecs_state.airspeed_filtered = _integ5_state; + _tecs_state.airspeed_rate = _vel_dot; + + _tecs_state.total_energy_error = _STE_error; + _tecs_state.energy_distribution_error = _SEB_error; + _tecs_state.total_energy_rate_error = _STEdot_error; + _tecs_state.energy_distribution_error = _SEBdot_error; + + _tecs_state.energy_error_integ = _integ6_state; + _tecs_state.energy_distribution_error_integ = _integ7_state; + + + _tecs_state.throttle_sp = _throttle_dem; + _tecs_state.pitch_sp = _pitch_dem; _update_pitch_throttle_last_usec = now; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 81a5e2d691..0fbda0fe9a 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -79,6 +79,10 @@ public: _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), + _STE_error(0.0f), + _STEdot_error(0.0f), + _SEB_error(0.0f), + _SEBdot_error(0.0f), _airspeed_enabled(false), _states_initalized(false), _in_air(false), @@ -137,18 +141,22 @@ public: struct tecs_state { uint64_t timestamp; - float hgt; - float dhgt; - float hgt_dem; - float dhgt_dem; - float spd_dem; - float spd; - float dspd; - float ithr; - float iptch; - float thr; - float ptch; - float dspd_dem; + float altitude_filtered; + float altitude_sp; + float altitude_rate; + float altitude_rate_sp; + float airspeed_filtered; + float airspeed_sp; + float airspeed_rate; + float airspeed_rate_sp; + float energy_error_integ; + float energy_distribution_error_integ; + float total_energy_error; + float total_energy_rate_error; + float energy_distribution_error; + float energy_distribution_rate_error; + float throttle_sp; + float pitch_sp; enum ECL_TECS_MODE mode; }; @@ -376,6 +384,15 @@ private: // Specific energy error quantities float _STE_error; + // Energy error rate + float _STEdot_error; + + // Specific energy balance error + float _SEB_error; + + // Specific energy balance error rate + float _SEBdot_error; + // Time since last update of main TECS loop (seconds) float _DT; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ceea71fca9..e14b84fb39 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1920,22 +1920,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ break; } - t.altitudeSp = s.hgt_dem; - t.altitude_filtered = s.hgt; - t.airspeedSp = s.spd_dem; - t.airspeed_filtered = s.spd; + t.altitudeSp = s.altitude_sp; + t.altitude_filtered = s.altitude_filtered; + t.airspeedSp = s.airspeed_sp; + t.airspeed_filtered = s.airspeed_filtered; - t.flightPathAngleSp = s.dhgt_dem; - t.flightPathAngle = s.dhgt; - t.flightPathAngleFiltered = s.dhgt; + t.flightPathAngleSp = s.altitude_rate_sp; + t.flightPathAngle = s.altitude_rate; + t.flightPathAngleFiltered = s.altitude_rate; - t.airspeedDerivativeSp = s.dspd_dem; - t.airspeedDerivative = s.dspd; + t.airspeedDerivativeSp = s.airspeed_rate_sp; + t.airspeedDerivative = s.airspeed_rate; - t.totalEnergyRateSp = s.thr; - t.totalEnergyRate = s.ithr; - t.energyDistributionRateSp = s.ptch; - t.energyDistributionRate = s.iptch; + t.totalEnergyError = s.total_energy_error; + t.totalEnergyRateError = s.total_energy_rate_error; + t.energyDistributionError = s.energy_distribution_error; + t.energyDistributionRateError = s.energy_distribution_rate_error; + + t.throttle_sp = s.throttle_sp; + t.pitch_sp = s.pitch_sp; if (_tecs_status_pub > 0) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 71ac96350a..c1c01ec06e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1863,15 +1863,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; - log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; - log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; - log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; - log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; - log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError; + log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; + log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; + log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; + log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp; + log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp; log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index e75b6ca256..1363b37ba7 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -360,16 +360,16 @@ struct log_TECS_s { float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; - float flightPathAngleFiltered; float airspeedSp; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; - - float totalEnergyRateSp; - float totalEnergyRate; - float energyDistributionRateSp; - float energyDistributionRate; + float totalEnergyError; + float totalEnergyRateError; + float energyDistributionError; + float energyDistributionRateError; + float throttle_sp; + float pitch_sp; uint8_t mode; }; @@ -527,7 +527,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"),