diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 7462dd84cc..8f79b874d7 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -55,8 +55,8 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil * inertial nav data is not available. It also calculates a true airspeed derivative * which is used by the airspeed complimentary filter. */ -void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, - const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, +void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deriv_forward, bool altitude_lock, + bool in_air, float altitude, float vz) { // calculate the time lapsed since the last update @@ -89,12 +89,8 @@ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &ro // Update and average speed rate of change if airspeed is being measured if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) { - // Assuming the vehicle is flying X axis forward, use the X axis measured acceleration - // compensated for gravity to estimate the rate of change of speed - const float speed_deriv_raw = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0); - // Apply some noise filtering - _TAS_rate_filter.update(speed_deriv_raw); + _TAS_rate_filter.update(speed_deriv_forward); _speed_derivative = _TAS_rate_filter.getState(); } else { diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 67fd70c70f..64de3b4f16 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -74,8 +74,7 @@ public: * Must be called prior to udating tecs control loops * Must be called at 50Hz or greater */ - void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, - const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, + void update_vehicle_state_estimates(float airspeed, const float speed_deriv_forward, bool altitude_lock, bool in_air, float altitude, float vz); /** diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 1d9bed7df1..bddbaa776b 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1852,15 +1852,17 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo /* Using tecs library */ float pitch_for_tecs = _pitch - radians(_param_fw_psp_off.get()); - /* filter speed and altitude for controller */ - Vector3f accel_body(_vehicle_acceleration_sub.get().xyz); + // calculate speed derivative in forward direction, which is used by complimentary filter to calculate filtered airspeed + Vector3f speed_deriv_body = _R_nb.transpose() * Vector3f(_local_pos.ax, _local_pos.ay, _local_pos.az); + float speed_deriv_forward; // tailsitters use the multicopter frame as reference, in fixed wing // we need to use the fixed wing frame if (_vtol_tailsitter) { - float tmp = accel_body(0); - accel_body(0) = -accel_body(2); - accel_body(2) = tmp; + speed_deriv_forward = -speed_deriv_body(2); + + } else { + speed_deriv_forward = speed_deriv_body(0); } /* tell TECS to update its state, but let it know when it cannot actually control the plane */ @@ -1871,8 +1873,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo _control_mode.flag_control_altitude_enabled)); /* update TECS vehicle state estimates */ - _tecs.update_vehicle_state_estimates(_airspeed, _R_nb, - accel_body, (_local_pos.timestamp > 0), in_air_alt_control, + _tecs.update_vehicle_state_estimates(_airspeed, speed_deriv_forward, (_local_pos.timestamp > 0), in_air_alt_control, _current_altitude, _local_pos.vz); /* scale throttle cruise by baro pressure */