Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 19:32:36 +08:00
Code Issues Actions 10 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 09:56:56 +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);
/**
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+8 -7
View File
@@ -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 */