Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -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 2022-01-03 12:53:05 +11:00 committed by Randy Mackay
parent 718a80619a
commit 695a59b278
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"
/*
get a speed scaling number for control surfaces. This is applied to
PIDs to change the scaling of the PID with speed. At high speed we
move the surfaces less, and at low speeds we move them more.
calculate speed scaling number for control surfaces. This is applied
to PIDs to change the scaling of the PID with speed. At high speed
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;
if (ahrs.airspeed_estimate(aspeed)) {

6
ArduPlane/Plane.h
View File

@ -394,6 +394,9 @@ private:
// Difference between current altitude and desired altitude. Centimeters
int32_t altitude_error_cm;
// speed scaler for control surfaces, updated at 10Hz
float surface_speed_scaler = 1.0;
// Battery Sensors
AP_BattMonitor battery{MASK_LOG_CURRENT,
FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t),
@ -844,7 +847,8 @@ private:
void calc_throttle();
void calc_nav_roll();
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);
void stabilize_roll(float speed_scaler);
void stabilize_pitch(float speed_scaler);

6
ArduPlane/sensors.cpp
View File

@ -65,6 +65,12 @@ void Plane::read_airspeed(void)
if (ahrs.airspeed_estimate(aspeed)) {
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);
}
/*