Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MC rate controller: add low-pass filter for D-term #8920

Merged
merged 6 commits into from
Feb 19, 2018
Merged

MC rate controller: add low-pass filter for D-term #8920

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
51ae6a8
MC_ROLL_P, MC_PITCH_P: increase maximum value to 12
bkueng Feb 19, 2018
adb5365
IMU_GYRO_CUTOFF, IMU_ACCEL_CUTOFF: reduce minimum to 0, allowing to d…
bkueng Feb 19, 2018
3f1230c
mc_attitude_control: avoid using a static variable for last_run
bkueng Feb 19, 2018
01ad247
mc_att_control rate controller: use a butterworth lowpass filter on t…
bkueng Feb 19, 2018
d0268cb
mc_att_control_main: remove unused _rates_sp_prev
bkueng Feb 19, 2018
41cdf41
mc_att_control: update & reset d-term lowpass filter only when freque…
bkueng Feb 19, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
74 changes: 65 additions & 9 deletions src/modules/mc_att_control/mc_att_control_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer/mixer.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
Expand Down Expand Up @@ -167,11 +168,16 @@ class MulticopterAttitudeControl
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _controller_latency_perf;

math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */
math::LowPassFilter2p _lp_filters_d[3]; /**< low-pass filters for D-term (roll, pitch & yaw) */
static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we're soon going to need a mechanism to set this system wide (single param or programmatically).

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is only for initialization. A module needs to be able to determine this on its own and at runtime.

float _loop_update_rate_hz; /**< current rate-controller loop update rate in [Hz] */

math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */
math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */
math::Vector<3> _rates_sp; /**< angular rates setpoint */
math::Vector<3> _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
float _thrust_sp; /**< thrust setpoint */
math::Vector<3> _att_control; /**< attitude control vector */

math::Matrix<3, 3> _I; /**< identity matrix */
Expand All @@ -191,6 +197,7 @@ class MulticopterAttitudeControl
param_t pitch_rate_integ_lim;
param_t pitch_rate_d;
param_t pitch_rate_ff;
param_t d_term_cutoff_freq;
param_t tpa_breakpoint_p;
param_t tpa_breakpoint_i;
param_t tpa_breakpoint_d;
Expand Down Expand Up @@ -240,6 +247,8 @@ class MulticopterAttitudeControl
math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */
float yaw_ff; /**< yaw control feed-forward */

float d_term_cutoff_freq; /**< Cutoff frequency for the D-term filter */

float tpa_breakpoint_p; /**< Throttle PID Attenuation breakpoint */
float tpa_breakpoint_i; /**< Throttle PID Attenuation breakpoint */
float tpa_breakpoint_d; /**< Throttle PID Attenuation breakpoint */
Expand Down Expand Up @@ -368,7 +377,14 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_saturation_status{},
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),

_lp_filters_d{
{initial_update_rate_hz, 50.f},
{initial_update_rate_hz, 50.f},
{initial_update_rate_hz, 50.f}}, // will be initialized correctly when params are loaded

_loop_update_rate_hz(initial_update_rate_hz)
{
for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) {
_sensor_gyro_sub[i] = -1;
Expand Down Expand Up @@ -400,6 +416,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.board_offset[2] = 0.0f;

_rates_prev.zero();
_rates_prev_filtered.zero();
_rates_sp.zero();
_rates_sp_prev.zero();
_rates_int.zero();
Expand All @@ -423,6 +440,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF");

_params_handles.d_term_cutoff_freq = param_find("MC_DTERM_CUTOFF");

_params_handles.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P");
_params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I");
_params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D");
Expand Down Expand Up @@ -540,6 +559,14 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.pitch_rate_ff, &v);
_params.rate_ff(1) = v;

param_get(_params_handles.d_term_cutoff_freq, &_params.d_term_cutoff_freq);
_lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
_lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
_lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
_lp_filters_d[0].reset(_rates_prev(0));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reset on any param change?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Right, I changed it to update only when the frequency changes.

_lp_filters_d[1].reset(_rates_prev(1));
_lp_filters_d[2].reset(_rates_prev(2));

param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p);
param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i);
param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d);
Expand Down Expand Up @@ -998,13 +1025,20 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;

/* apply low-pass filtering to the rates for D-term */
math::Vector<3> rates_filtered(
_lp_filters_d[0].apply(rates(0)),
_lp_filters_d[1].apply(rates(1)),
_lp_filters_d[2].apply(rates(2)));

_att_control = rates_p_scaled.emult(rates_err) +
_rates_int +
rates_d_scaled.emult(_rates_prev - rates) / dt +
_rates_int -
rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt +
_params.rate_ff.emult(_rates_sp);

_rates_sp_prev = _rates_sp;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think _rates_sp_prev is used anywhere?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yep, removed.

_rates_prev = rates;
_rates_prev_filtered = rates_filtered;

/* update integral only if motors are providing enough thrust to be effective */
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
Expand Down Expand Up @@ -1093,6 +1127,11 @@ MulticopterAttitudeControl::task_main()
px4_pollfd_struct_t poll_fds = {};
poll_fds.events = POLLIN;

const hrt_abstime task_start = hrt_absolute_time();
hrt_abstime last_run = task_start;
float dt_accumulator = 0.f;
int loop_counter = 0;

while (!_task_should_exit) {

poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
Expand All @@ -1117,9 +1156,9 @@ MulticopterAttitudeControl::task_main()

/* run controller on gyro changes */
if (poll_fds.revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
const hrt_abstime now = hrt_absolute_time();
float dt = (now - last_run) / 1e6f;
last_run = now;

/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
Expand Down Expand Up @@ -1297,6 +1336,23 @@ MulticopterAttitudeControl::task_main()
}
}
}

/* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */
if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) {
dt_accumulator += dt;
++loop_counter;

if (dt_accumulator > 1.f) {
const float loop_update_rate = (float)loop_counter / dt_accumulator;
_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
dt_accumulator = 0;
loop_counter = 0;
_lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
_lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
_lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
}
}

}

perf_end(_loop_perf);
Expand Down
21 changes: 19 additions & 2 deletions src/modules/mc_att_control/mc_att_control_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_TC, 0.2f);
*
* @unit 1/s
* @min 0.0
* @max 8
* @max 12
* @decimal 2
* @increment 0.1
* @group Multicopter Attitude Control
Expand Down Expand Up @@ -149,7 +149,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f);
*
* @unit 1/s
* @min 0.0
* @max 10
* @max 12
* @decimal 2
* @increment 0.1
* @group Multicopter Attitude Control
Expand Down Expand Up @@ -554,3 +554,20 @@ PARAM_DEFINE_FLOAT(MC_TPA_RATE_I, 0.0f);
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f);

/**
* Cutoff frequency for the low pass filter on the D-term in the rate controller
*
* The D-term uses the derivative of the rate and thus is the most susceptible to noise.
* Therefore, using a D-term filter allows to decrease the driver-level filtering, which
* leads to reduced control latency and permits to increase the P gains.
* A value of 0 disables the filter.
*
* @unit Hz
* @min 0
* @max 1000
* @decimal 0
* @increment 10
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);
12 changes: 6 additions & 6 deletions src/modules/sensors/sensor_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,13 +227,13 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);

/**
* Driver level cut frequency for gyro
* Driver level cutoff frequency for gyro
*
* The cut frequency for the 2nd order butterworth filter on the gyro driver. This features
* The cutoff frequency for the 2nd order butterworth filter on the gyro driver. This features
* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the
* controllers, not the estimators. 0 disables the filter.
*
* @min 5
* @min 0
* @max 1000
* @unit Hz
* @reboot_required true
Expand All @@ -242,13 +242,13 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);

/**
* Driver level cut frequency for accel
* Driver level cutoff frequency for accel
*
* The cut frequency for the 2nd order butterworth filter on the accel driver. This features
* The cutoff frequency for the 2nd order butterworth filter on the accel driver. This features
* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the
* controllers, not the estimators. 0 disables the filter.
*
* @min 5
* @min 0
* @max 1000
* @unit Hz
* @reboot_required true
Expand Down