fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 13:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

tecs: use speed derivative provided by local position

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-01-13 21:21:07 +03:00
committed by Roman Bapst
parent b230bbfe88
commit f05599caff
3 changed files with 12 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+3 -7
View File
@@ -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 {
src/lib/tecs/TECS.hpp
+1 -2
View File
@@ -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);
/**