forked from Archive/PX4-Autopilot
log raw airspeed derivative, pitch setpoint and airspeed innovation for TECS
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
parent
806b462935
commit
b06b46b224
|
@ -17,6 +17,8 @@ float32 true_airspeed_sp
|
||||||
float32 true_airspeed_filtered
|
float32 true_airspeed_filtered
|
||||||
float32 true_airspeed_derivative_sp
|
float32 true_airspeed_derivative_sp
|
||||||
float32 true_airspeed_derivative
|
float32 true_airspeed_derivative
|
||||||
|
float32 true_airspeed_derivative_raw
|
||||||
|
float32 true_airspeed_innovation
|
||||||
|
|
||||||
float32 total_energy_error
|
float32 total_energy_error
|
||||||
float32 energy_distribution_error
|
float32 energy_distribution_error
|
||||||
|
@ -37,5 +39,6 @@ float32 throttle_integ
|
||||||
float32 pitch_integ
|
float32 pitch_integ
|
||||||
|
|
||||||
float32 throttle_sp
|
float32 throttle_sp
|
||||||
|
float32 pitch_sp_rad
|
||||||
|
|
||||||
uint8 mode
|
uint8 mode
|
||||||
|
|
|
@ -133,8 +133,8 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
|
||||||
// Obtain a smoothed TAS estimate using a second order complementary filter
|
// Obtain a smoothed TAS estimate using a second order complementary filter
|
||||||
|
|
||||||
// Update TAS rate state
|
// Update TAS rate state
|
||||||
float tas_error = (_EAS * EAS2TAS) - _tas_state;
|
_tas_innov = (_EAS * EAS2TAS) - _tas_state;
|
||||||
float tas_rate_state_input = tas_error * _tas_estimate_freq * _tas_estimate_freq;
|
float tas_rate_state_input = _tas_innov * _tas_estimate_freq * _tas_estimate_freq;
|
||||||
|
|
||||||
// limit integrator input to prevent windup
|
// limit integrator input to prevent windup
|
||||||
if (_tas_state < 3.1f) {
|
if (_tas_state < 3.1f) {
|
||||||
|
@ -143,7 +143,7 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
|
||||||
|
|
||||||
// Update TAS state
|
// Update TAS state
|
||||||
_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
|
_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
|
||||||
float tas_state_input = _tas_rate_state + _tas_rate_raw + tas_error * _tas_estimate_freq * 1.4142f;
|
float tas_state_input = _tas_rate_state + _tas_rate_raw + _tas_innov * _tas_estimate_freq * 1.4142f;
|
||||||
_tas_state = _tas_state + tas_state_input * dt;
|
_tas_state = _tas_state + tas_state_input * dt;
|
||||||
|
|
||||||
// Limit the TAS state to a minimum of 3 m/s
|
// Limit the TAS state to a minimum of 3 m/s
|
||||||
|
|
|
@ -144,6 +144,7 @@ public:
|
||||||
|
|
||||||
float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
|
float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
|
||||||
float tas_state() { return _tas_state; }
|
float tas_state() { return _tas_state; }
|
||||||
|
float getTASInnovation() { return _tas_innov; }
|
||||||
|
|
||||||
float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
|
float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
|
||||||
float vert_vel_state() { return _vert_vel_state; }
|
float vert_vel_state() { return _vert_vel_state; }
|
||||||
|
@ -151,6 +152,7 @@ public:
|
||||||
float get_EAS_setpoint() { return _EAS_setpoint; };
|
float get_EAS_setpoint() { return _EAS_setpoint; };
|
||||||
float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
|
float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
|
||||||
float speed_derivative() { return _tas_rate_filtered; }
|
float speed_derivative() { return _tas_rate_filtered; }
|
||||||
|
float speed_derivative_raw() { return _tas_rate_raw; }
|
||||||
|
|
||||||
float STE_error() { return _STE_error; }
|
float STE_error() { return _STE_error; }
|
||||||
float STE_rate_error() { return _STE_rate_error; }
|
float STE_rate_error() { return _STE_rate_error; }
|
||||||
|
@ -235,6 +237,7 @@ private:
|
||||||
float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m)
|
float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m)
|
||||||
float _tas_rate_state{0.0f}; ///< complimentary filter state - true airspeed first derivative (m/sec**2)
|
float _tas_rate_state{0.0f}; ///< complimentary filter state - true airspeed first derivative (m/sec**2)
|
||||||
float _tas_state{0.0f}; ///< complimentary filter state - true airspeed (m/sec)
|
float _tas_state{0.0f}; ///< complimentary filter state - true airspeed (m/sec)
|
||||||
|
float _tas_innov{0.0f}; ///< complimentary filter true airspeed innovation (m/sec)
|
||||||
|
|
||||||
// controller states
|
// controller states
|
||||||
float _throttle_integ_state{0.0f}; ///< throttle integrator state
|
float _throttle_integ_state{0.0f}; ///< throttle integrator state
|
||||||
|
|
|
@ -398,6 +398,8 @@ FixedwingPositionControl::tecs_status_publish()
|
||||||
t.equivalent_airspeed_sp = _tecs.get_EAS_setpoint();
|
t.equivalent_airspeed_sp = _tecs.get_EAS_setpoint();
|
||||||
t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint();
|
t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint();
|
||||||
t.true_airspeed_derivative = _tecs.speed_derivative();
|
t.true_airspeed_derivative = _tecs.speed_derivative();
|
||||||
|
t.true_airspeed_derivative_raw = _tecs.speed_derivative_raw();
|
||||||
|
t.true_airspeed_innovation = _tecs.getTASInnovation();
|
||||||
|
|
||||||
t.total_energy_error = _tecs.STE_error();
|
t.total_energy_error = _tecs.STE_error();
|
||||||
t.total_energy_rate_error = _tecs.STE_rate_error();
|
t.total_energy_rate_error = _tecs.STE_rate_error();
|
||||||
|
@ -419,6 +421,7 @@ FixedwingPositionControl::tecs_status_publish()
|
||||||
t.pitch_integ = _tecs.pitch_integ_state();
|
t.pitch_integ = _tecs.pitch_integ_state();
|
||||||
|
|
||||||
t.throttle_sp = _tecs.get_throttle_setpoint();
|
t.throttle_sp = _tecs.get_throttle_setpoint();
|
||||||
|
t.pitch_sp_rad = _tecs.get_pitch_setpoint();
|
||||||
|
|
||||||
t.timestamp = hrt_absolute_time();
|
t.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue