Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: add a low pass filter to speed scaler

Browse Source 
this is needed due to the filtering done on the target rate in
AC_PID. With a low filter rate in AC_PID a step in the speed scaler
results in a step in the FF output due to the mismatch in the
instantaneous SS and the filtered target rate
This commit is contained in:
Andrew Tridgell 2021-12-29 12:23:09 +11:00
parent 662327f2ea
commit fb019eb0f3
3 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/Attitude.cpp
View File

@ -1,11 +1,11 @@
#include "Plane.h" #include "Plane.h"
/* /*
get a speed scaling number for control surfaces. This is applied to calculate speed scaling number for control surfaces. This is applied
PIDs to change the scaling of the PID with speed. At high speed we to PIDs to change the scaling of the PID with speed. At high speed
move the surfaces less, and at low speeds we move them more. we move the surfaces less, and at low speeds we move them more.
*/ */
float Plane::get_speed_scaler(void) float Plane::calc_speed_scaler(void)
{ {
float aspeed, speed_scaler; float aspeed, speed_scaler;
if (ahrs.airspeed_estimate(aspeed)) { if (ahrs.airspeed_estimate(aspeed)) {

6
ArduPlane/Plane.h
View File

@ -392,6 +392,9 @@ private:
// Difference between current altitude and desired altitude. Centimeters // Difference between current altitude and desired altitude. Centimeters
int32_t altitude_error_cm; int32_t altitude_error_cm;
// speed scaler for control surfaces, updated at 10Hz
float surface_speed_scaler = 1.0;
// Battery Sensors // Battery Sensors
AP_BattMonitor battery{MASK_LOG_CURRENT, AP_BattMonitor battery{MASK_LOG_CURRENT,
FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t), FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t),
@ -858,7 +861,8 @@ private:
void calc_throttle(); void calc_throttle();
void calc_nav_roll(); void calc_nav_roll();
void calc_nav_pitch(); void calc_nav_pitch();
float get_speed_scaler(void); float calc_speed_scaler(void);
float get_speed_scaler(void) const { return surface_speed_scaler; }
bool stick_mixing_enabled(void); bool stick_mixing_enabled(void);
void stabilize_roll(float speed_scaler); void stabilize_roll(float speed_scaler);
float stabilize_roll_get_roll_out(float speed_scaler); float stabilize_roll_get_roll_out(float speed_scaler);

6
ArduPlane/sensors.cpp
View File

@ -51,6 +51,12 @@ void Plane::read_airspeed(void)
if (ahrs.airspeed_estimate(aspeed)) { if (ahrs.airspeed_estimate(aspeed)) {
smoothed_airspeed = smoothed_airspeed * 0.8f + aspeed * 0.2f; smoothed_airspeed = smoothed_airspeed * 0.8f + aspeed * 0.2f;
} }
// low pass filter speed scaler, with 1Hz cutoff, at 10Hz
const float speed_scaler = calc_speed_scaler();
const float cutoff_Hz = 2.0;
const float dt = 0.1;
surface_speed_scaler += calc_lowpass_alpha_dt(dt, cutoff_Hz) * (speed_scaler - surface_speed_scaler);
} }
/* /*