diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-03 13:18:22 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-03 13:18:22 +0200 |
commit | 43ef622725b1114beccb722bad0538c4c0cc3175 (patch) | |
tree | 0d674eab47c152c2f8853ba9f886bfc242fef289 | |
parent | 020ac409be62e6c459e7aa5cb3181941c9bbfd23 (diff) | |
download | px4-firmware-43ef622725b1114beccb722bad0538c4c0cc3175.tar.gz px4-firmware-43ef622725b1114beccb722bad0538c4c0cc3175.tar.bz2 px4-firmware-43ef622725b1114beccb722bad0538c4c0cc3175.zip |
Reinstate TECS logging
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 40 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 49 |
2 files changed, 57 insertions, 32 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 579fb5d42..a57a0481a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -554,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // Calculate pitch demand _update_pitch(); -// // Write internal variables to the log_tuning structure. This -// // structure will be logged in dataflash at 10Hz - // log_tuning.hgt_dem = _hgt_dem_adj; - // log_tuning.hgt = _integ3_state; - // log_tuning.dhgt_dem = _hgt_rate_dem; - // log_tuning.dhgt = _integ2_state; - // log_tuning.spd_dem = _TAS_dem_adj; - // log_tuning.spd = _integ5_state; - // log_tuning.dspd = _vel_dot; - // log_tuning.ithr = _integ6_state; - // log_tuning.iptch = _integ7_state; - // log_tuning.thr = _throttle_dem; - // log_tuning.ptch = _pitch_dem; - // log_tuning.dspd_dem = _TAS_rate_dem; + _tecs_state.timestamp = now; + + if (_underspeed) { + _tecs_state.mode = ECL_TECS_MODE_UNDERSPEED; + } else if (_badDescent) { + _tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT; + } else if (_climbOutDem) { + _tecs_state.mode = ECL_TECS_MODE_CLIMBOUT; + } else { + // If no error flag applies, conclude normal + _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; + } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 752b0ddd9..bcc2d90e5 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,7 @@ class __EXPORT TECS { public: TECS() : + _tecs_state {}, _update_50hz_last_usec(0), _update_speed_last_usec(0), _update_pitch_throttle_last_usec(0), @@ -120,24 +121,33 @@ public: return _spdWeight; } - // log data on internal state of the controller. Called at 10Hz - // void log_data(DataFlash_Class &dataflash, uint8_t msgid); - - // struct PACKED log_TECS_Tuning { - // LOG_PACKET_HEADER; - // 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; - // } log_tuning; + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; + + 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; + enum ECL_TECS_MODE mode; + }; + + void get_tecs_state(struct tecs_state& state) { + state = _tecs_state; + } void set_time_const(float time_const) { _timeConst = time_const; @@ -212,6 +222,9 @@ public: } private: + + struct tecs_state _tecs_state; + // Last time update_50Hz was called uint64_t _update_50hz_last_usec; |