mc rate control: add gyroscopic torque compensation loop

Single (rigid) rotor aircrafts have their dynamics dominated by the
gyroscopic torque. Compensating for this effect by a feedback greatly
improves stability.
Multirotors can also benefit from this feature when the difference in
RPM between the CW and CCW rotors is large (the sum of the angular
momentum of the rotors is non zero).

src/lib/rate_control/rate_control.cpp

@@ -74,8 +74,12 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
 	// angular rates error
 	Vector3f rate_error = rate_sp - rate;
 
+	// assume the rotor angular momentum is aligned with the Z axis
+	const Vector3f gyroscopic_torque = rate.cross(Vector3f(0.f, 0.f, _rotor_angular_momentum));
+
 	// PID control with feed forward
-	const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);
+	const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(
+					rate_sp) - gyroscopic_torque;
 
 	// update integral only if we are not landed
 	if (!landed) {

src/lib/rate_control/rate_control.hpp

@@ -71,6 +71,12 @@ public:
 	 */
 	void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
 
+	/**
+	 * Set the total angular momentum produced by the rotors
+	 * @param norm of angular momentum along the Z body axis
+	 */
+	void setRotorAngularMomentum(float momentum) { _rotor_angular_momentum = momentum; };
+
 	/**
 	 * Set saturation status
 	 * @param control saturation vector from control allocator
@@ -129,6 +135,7 @@ private:
 	matrix::Vector3f _gain_d; ///< rate control derivative gain
 	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
 	matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
+	float _rotor_angular_momentum{0.f}; ///< used to cancel out the gyroscopic torque generated by the rotors
 
 	// States
 	matrix::Vector3f _rate_int; ///< integral term of the rate controller

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -183,6 +183,28 @@ MulticopterRateControl::Run()
 			}
 		}
 
+		if (_esc_status_sub.updated()) {
+			esc_status_s esc_status;
+
+			if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + 1_s)) {
+				float rpm_sum = 0.f;
+
+				for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESCS); esc++) {
+					const esc_report_s &esc_report = esc_status.esc[esc];
+
+					const bool esc_connected = (esc_status.esc_online_flags & (1 << esc)) || (esc_report.esc_rpm != 0);
+
+					// assuming all the rotors are in the same plane, the gyroscopic torque is proportional to the
+					// difference between the CW and CCW rotation speed
+					if (esc_connected && (now < esc_report.timestamp + 1_s)) {
+						rpm_sum += esc_report.esc_rpm;
+					}
+				}
+
+				_rate_control.setRotorAngularMomentum(rpm_sum * _param_mc_precess_gain.get());
+			}
+		}
+
 		// run the rate controller
 		if (_vehicle_control_mode.flag_control_rates_enabled) {
 

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -49,6 +49,7 @@
 #include <uORB/topics/actuator_controls_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
@@ -93,6 +94,7 @@ private:
 
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
+	uORB::Subscription _esc_status_sub {ORB_ID(esc_status)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
@@ -109,6 +111,8 @@ private:
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub;
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub;
 
+	static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
+
 	vehicle_control_mode_s	_vehicle_control_mode{};
 	vehicle_status_s	_vehicle_status{};
 
@@ -151,6 +155,8 @@ 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_PRECESS_GAIN>) _param_mc_precess_gain,
+
 		(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
 		(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
 		(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,

src/modules/mc_rate_control/mc_rate_control_params.c

@@ -279,6 +279,22 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f);
 
+/**
+ * Gyroscopic torque compensation gain
+ *
+ * Compensates for the gyroscopic torque generated
+ * by a difference in speed between the CW and CCW rotors.
+ *
+ * Assumes that all the rotors have the same inertia and
+ * their thrust is pointing upwards.
+ *
+ * @min -1.0
+ * @max  1.0
+ * @decimal 8
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_PRECESS_GAIN, 0.0f);
+
 /**
  * Battery power level scaler
  *