diff --git a/src/lib/rate_control/rate_control.cpp b/src/lib/rate_control/rate_control.cpp index baa4c8e7b5..f9e94e1337 100644 --- a/src/lib/rate_control/rate_control.cpp +++ b/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) { diff --git a/src/lib/rate_control/rate_control.hpp b/src/lib/rate_control/rate_control.hpp index 0ee8bd1516..91e8c1f8f7 100644 --- a/src/lib/rate_control/rate_control.hpp +++ b/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 diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index e8a371d1e2..4b5e2c6fc7 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/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) { diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 0f402c12c3..c0d0eada58 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -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_pub; uORB::Publication _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) _param_mc_yawrate_ff, (ParamFloat) _param_mc_yawrate_k, + (ParamFloat) _param_mc_precess_gain, + (ParamFloat) _param_mc_acro_r_max, (ParamFloat) _param_mc_acro_p_max, (ParamFloat) _param_mc_acro_y_max, diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index 2571a70c1f..8c61055cb1 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/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 *