mc_rate_control: use vehicle_angular_acceleration topic instead of differentiating

* 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)

ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark

@@ -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

ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500

@@ -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

ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision

@@ -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

ROMFS/px4fmu_common/init.d/airframes/4041_beta75x

@@ -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

ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250

@@ -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

ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2

@@ -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

ROMFS/px4fmu_common/init.d/airframes/4071_ifo

@@ -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

ROMFS/px4fmu_common/init.d/airframes/4072_draco

@@ -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

ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie

@@ -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

ROMFS/px4fmu_common/init.d/airframes/6002_draco_r

@@ -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)

src/modules/logger/logged_topics.cpp

@@ -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");

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -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{};

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -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,

src/modules/mc_rate_control/RateControl/RateControl.cpp

@@ -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) {

src/modules/mc_rate_control/RateControl/RateControl.hpp

@@ -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] {};
 };

src/modules/mc_rate_control/RateControl/RateControlTest.cpp

@@ -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());
 }

src/modules/mc_rate_control/mc_rate_control_params.c

@@ -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);

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -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};
 

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -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);