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

mc_rate_control: use vehicle_angular_acceleration topic instead of differentiating

Browse Source 
* mc_rate_control: use vehicle_angular_acceleration topic
* IMU_DGYRO_CUTOFF change default to 30 Hz
* improve IMU_DGYRO_CUTOFF param documentation (updated from MC_DTERM_CUTOFF)
This commit is contained in:
Daniel Agar 2020-03-12 15:07:03 -04:00 committed by GitHub
parent 2b03aa3d1b
commit 093e9ba1ce
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
19 changed files with 56 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
View File

@ -53,7 +53,7 @@ then
param set IMU_GYRO_CUTOFF 40
param set MC_DTERM_CUTOFF 15
param set IMU_DGYRO_CUTOFF 15
param set MC_PITCHRATE_I 0.2
param set MC_PITCHRATE_MAX 60
param set MC_ROLLRATE_I 0.2

2
ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500
View File

@ -18,7 +18,7 @@ set PWM_OUT 1234
if [ $AUTOCNF = yes ]
then
param set IMU_GYRO_CUTOFF 80
param set MC_DTERM_CUTOFF 40
param set IMU_DGYRO_CUTOFF 40
param set MC_ROLLRATE_P 0.14
param set MC_PITCHRATE_P 0.14
param set MC_ROLLRATE_I 0.3

2
ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
View File

@ -66,7 +66,7 @@ then
param set MC_ACRO_SUPEXPO 0
param set MC_ACRO_SUPEXPOY 0
param set MC_ACRO_Y_MAX 150
param set MC_DTERM_CUTOFF 30
param set IMU_DGYRO_CUTOFF 30
param set MC_PITCHRATE_D 0.0015
param set MC_PITCHRATE_I 0.2
param set MC_PITCHRATE_K 1

2
ROMFS/px4fmu_common/init.d/airframes/4041_beta75x
View File

@ -29,7 +29,7 @@ then
param set CBRK_USB_CHK 197848
param set IMU_GYRO_CUTOFF 100
param set MC_DTERM_CUTOFF 60
param set IMU_DGYRO_CUTOFF 60
param set MC_AIRMODE 2
param set MC_PITCHRATE_D 0.0010

2
ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250
View File

@ -22,7 +22,7 @@ then
param set BAT_N_CELLS 4
param set IMU_GYRO_CUTOFF 120
param set MC_DTERM_CUTOFF 45
param set IMU_DGYRO_CUTOFF 45
param set MC_AIRMODE 1
param set MC_PITCHRATE_D 0.0012

2
ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2
View File

@ -23,7 +23,7 @@ then
param set RC_PORT_CONFIG 201
param set IMU_GYRO_CUTOFF 120
param set MC_DTERM_CUTOFF 0
param set IMU_DGYRO_CUTOFF 0
param set MC_ROLLRATE_P 0.075
param set MC_ROLLRATE_I 0.25

4
ROMFS/px4fmu_common/init.d/airframes/4071_ifo
View File

@ -60,7 +60,7 @@ then
param set RC_FLT_CUTOFF 0.00000
# Filter settings
param set MC_DTERM_CUTOFF 90.00000
param set IMU_DGYRO_CUTOFF 90.00000
param set IMU_GYRO_CUTOFF 100.00000
# Thrust curve (avoids the need for TPA)
@ -106,7 +106,7 @@ then
# Filter settings
param set IMU_GYRO_CUTOFF 90.00000
param set MC_DTERM_CUTOFF 70.00000
param set IMU_DGYRO_CUTOFF 70.00000
# Don't try to be intelligent on RC loss: just cut the motors
param set NAV_RCL_ACT 6

2
ROMFS/px4fmu_common/init.d/airframes/4072_draco
View File

@ -84,7 +84,7 @@ then
# Filter settings
param set IMU_GYRO_CUTOFF 90.00000
param set MC_DTERM_CUTOFF 100.00000
param set IMU_DGYRO_CUTOFF 100.00000
# Don't try to be intelligent on RC loss: just cut the motors
#param set NAV_RCL_ACT 6

2
ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie
View File

@ -39,7 +39,7 @@ then
param set IMU_ACCEL_CUTOFF 30
param set MC_AIRMODE 1
param set MC_DTERM_CUTOFF 70
param set IMU_DGYRO_CUTOFF 70
param set MC_PITCHRATE_D 0.002
param set MC_PITCHRATE_I 0.2
param set MC_PITCHRATE_P 0.07

2
ROMFS/px4fmu_common/init.d/airframes/6002_draco_r
View File

@ -78,7 +78,7 @@ then
param set RC_FLT_CUTOFF 0.00000
# Filter settings
param set MC_DTERM_CUTOFF 90.00000
param set IMU_DGYRO_CUTOFF 90.00000
param set IMU_GYRO_CUTOFF 100.00000
# Thrust curve (avoids the need for TPA)

2
src/modules/logger/logged_topics.cpp
View File

@ -122,6 +122,7 @@ void LoggedTopics::add_default_topics()
add_topic("fw_virtual_attitude_setpoint");
add_topic("mc_virtual_attitude_setpoint");
add_topic("time_offset");
add_topic("vehicle_angular_acceleration", 10);
add_topic("vehicle_angular_velocity", 10);
add_topic("vehicle_attitude_groundtruth", 10);
add_topic("vehicle_global_position_groundtruth", 100);
@ -137,6 +138,7 @@ void LoggedTopics::add_high_rate_topics()
add_topic("manual_control_setpoint");
add_topic("rate_ctrl_status", 20);
add_topic("sensor_combined");
add_topic("vehicle_angular_acceleration");
add_topic("vehicle_angular_velocity");
add_topic("vehicle_attitude");
add_topic("vehicle_attitude_setpoint");

24
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -85,8 +85,6 @@ MulticopterRateControl::parameters_updated()
_rate_control.setIntegratorLimit(
Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()));
_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), false);
_rate_control.setFeedForwardGain(
Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()));
@ -147,12 +145,18 @@ MulticopterRateControl::Run()
vehicle_angular_velocity_s angular_velocity;
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
// grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy
vehicle_angular_acceleration_s v_angular_acceleration{};
_vehicle_angular_acceleration_sub.copy(&v_angular_acceleration);
const hrt_abstime now = hrt_absolute_time();
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
_last_run = now;
const Vector3f angular_accel{v_angular_acceleration.xyz};
const Vector3f rates{angular_velocity.xyz};
/* check for updates in other topics */
@ -240,20 +244,6 @@ MulticopterRateControl::Run()
}
}
// 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.0f) {
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;
_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true);
}
}
// run the rate controller
if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
@ -275,7 +265,7 @@ MulticopterRateControl::Run()
}
// run rate controller
const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed);
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};

27
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -35,7 +35,6 @@
#include <RateControl.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
@ -54,6 +53,7 @@
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
@ -93,15 +93,16 @@ private:
RateControl _rate_control; ///< class for rate control calculations
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
@ -123,19 +124,13 @@ private:
perf_counter_t _loop_perf; /**< loop duration performance counter */
static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */
matrix::Vector3f _rates_sp; /**< angular rates setpoint */
float _thrust_sp{0.0f}; /**< thrust setpoint */
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
hrt_abstime _task_start{hrt_absolute_time()};
hrt_abstime _last_run{0};
float _dt_accumulator{0.0f};
int _loop_counter{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
@ -159,8 +154,6 @@ private:
(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,

25
src/modules/mc_rate_control/RateControl/RateControl.cpp
View File

@ -47,15 +47,6 @@ void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f
_gain_d = D;
}
void RateControl::setDTermCutoff(const float loop_rate, const float cutoff, const bool force)
{
// only do expensive filter update if the cutoff changed
if (force || fabsf(_lp_filters_d.get_cutoff_freq() - cutoff) > 0.01f) {
_lp_filters_d.set_cutoff_frequency(loop_rate, cutoff);
_lp_filters_d.reset(_rate_prev);
}
}
void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status)
{
_mixer_saturation_positive[0] = status.flags.roll_pos;
@ -66,24 +57,14 @@ void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &
_mixer_saturation_negative[2] = status.flags.yaw_neg;
}
Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const float dt, const bool landed)
Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
const float dt, const bool landed)
{
// angular rates error
Vector3f rate_error = rate_sp - rate;
// prepare D-term based on low-pass filtered rates
const Vector3f rate_filtered(_lp_filters_d.apply(rate));
Vector3f rate_d;
if (dt > FLT_EPSILON) {
rate_d = (rate_filtered - _rate_prev_filtered) / dt;
}
// PID control with feed forward
const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp);
_rate_prev = rate;
_rate_prev_filtered = rate_filtered;
const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);
// update integral only if we are not landed
if (!landed) {

16
src/modules/mc_rate_control/RateControl/RateControl.hpp
View File

@ -65,14 +65,6 @@ public:
*/
void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
/**
* Set update frequency and low-pass filter cutoff that is applied to the derivative term
* @param loop_rate [Hz] rate with which update function is called
* @param cutoff [Hz] cutoff frequency for the low-pass filter on the dervative term
* @param force flag to force an expensive update even if the cutoff didn't change
*/
void setDTermCutoff(const float loop_rate, const float cutoff, const bool force);
/**
* Set direct rate to torque feed forward gain
* @see _gain_ff
@ -93,8 +85,8 @@ public:
* @param dt desired vehicle angular rate setpoint
* @return [-1,1] normalized torque vector to apply to the vehicle
*/
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt,
const bool landed);
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp,
const matrix::Vector3f &angular_accel, const float dt, const bool landed);
/**
* Set the integral term to 0 to prevent windup
@ -119,10 +111,8 @@ private:
matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
// States
matrix::Vector3f _rate_prev; ///< angular rates of previous update
matrix::Vector3f _rate_prev_filtered; ///< low-pass filtered angular rates of previous update
matrix::Vector3f _rate_int; ///< integral term of the rate controller
math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw)
bool _mixer_saturation_positive[3] {};
bool _mixer_saturation_negative[3] {};
};

2
src/modules/mc_rate_control/RateControl/RateControlTest.cpp
View File

@ -39,6 +39,6 @@ using namespace matrix;
TEST(RateControlTest, AllZeroCase)
{
RateControl rate_control;
Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false);
Vector3f torque = rate_control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
EXPECT_EQ(torque, Vector3f());
}

17
src/modules/mc_rate_control/mc_rate_control_params.c
View File

@ -395,20 +395,3 @@ PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f);
* @group Multicopter Rate Control
*/
PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
/**
* 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 Rate Control
*/
PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);

2
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -108,7 +108,7 @@ private:
math::NotchFilter<matrix::Vector3f> _notch_filter_velocity{};
// angular acceleration filter
math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 10.0f};
math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 30.0f};
float _filter_sample_rate{kInitialRateHz};

24
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
View File

@ -36,9 +36,12 @@
*
* The center frequency for the 2nd order notch filter on the primary gyro.
* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency.
* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
* This only affects the signal sent to the controllers, not the estimators.
* Applies to both angular velocity and angular acceleration sent to the controllers.
* See "IMU_GYRO_NF_BW" to set the bandwidth of the filter.
*
* A value of 0 disables the filter.
*
* @min 0
* @max 1000
* @unit Hz
@ -52,6 +55,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
*
* The frequency width of the stop band for the 2nd order notch filter on the primary gyro.
* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency.
* Applies to both angular velocity and angular acceleration sent to the controllers.
*
* @min 0
* @max 100
@ -65,7 +69,10 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
* Low pass filter cutoff frequency for gyro
*
* The cutoff frequency for the 2nd order butterworth filter on the primary gyro.
* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
* This only affects the angular velocity sent to the controllers, not the estimators.
* Doesn't apply to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF.
*
* A value of 0 disables the filter.
*
* @min 0
* @max 1000
@ -96,11 +103,16 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
/**
* Cutoff frequency for angular acceleration
* Cutoff frequency for angular acceleration (D-Term filter)
*
* The cutoff frequency for the 2nd order butterworth filter used on
* the time derivative of the measured angular velocity.
* Set to 0 to disable the filter.
* the time derivative of the measured angular velocity, also known as
* the D-term filter 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.
*
* @min 0
* @max 1000
@ -108,4 +120,4 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);
PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f);