diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 771f1afaa1..a5e5be3e7c 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -52,12 +52,11 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g } } -matrix::Vector3f AttitudeControl::update(matrix::Quatf q) const +matrix::Vector3f AttitudeControl::update(const Quatf &q) const { Quatf qd = _attitude_setpoint_q; // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN - q.normalize(); qd.normalize(); // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index 25f809164e..9b55e9e8b9 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -89,7 +89,7 @@ public: * @param q estimation of the current vehicle attitude unit quaternion * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller */ - matrix::Vector3f update(matrix::Quatf q) const; + matrix::Vector3f update(const matrix::Quatf &q) const; private: matrix::Vector3f _proportional_gain; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 58b8bb0a07..fe86d29a60 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -94,7 +94,7 @@ private: /** * Generate & publish an attitude setpoint from stick inputs */ - void generate_attitude_setpoint(float dt, bool reset_yaw_sp); + void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp); AttitudeControl _attitude_control; ///< class for attitude control calculations @@ -111,7 +111,6 @@ private: uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ uORB::Publication _vehicle_attitude_setpoint_pub; - struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */ struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ @@ -130,6 +129,8 @@ private: bool _reset_yaw_sp{true}; + uint8_t _quat_reset_counter{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_mc_roll_p, (ParamFloat) _param_mc_pitch_p, diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 824691c2c2..6b9ce11005 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -67,9 +67,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - /* initialize quaternions in messages to be valid */ - _v_att.q[0] = 1.f; - parameters_updated(); } @@ -122,10 +119,10 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) } void -MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp) +MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp) { vehicle_attitude_setpoint_s attitude_setpoint{}; - const float yaw = Eulerf(Quatf(_v_att.q)).psi(); + const float yaw = Eulerf(q).psi(); /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { @@ -242,9 +239,9 @@ MulticopterAttitudeControl::Run() } // run controller on attitude updates - const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; + vehicle_attitude_s v_att; - if (_vehicle_attitude_sub.update(&_v_att)) { + if (_vehicle_attitude_sub.update(&v_att)) { // Check for new attitude setpoint if (_vehicle_attitude_setpoint_sub.updated()) { @@ -255,18 +252,18 @@ MulticopterAttitudeControl::Run() } // Check for a heading reset - if (prev_quat_reset_counter != _v_att.quat_reset_counter) { - const Quatf delta_q_reset(_v_att.delta_q_reset); + if (_quat_reset_counter != v_att.quat_reset_counter) { + const Quatf delta_q_reset(v_att.delta_q_reset); // for stabilized attitude generation only extract the heading change from the delta quaternion _man_yaw_sp += Eulerf(delta_q_reset).psi(); _attitude_control.adaptAttitudeSetpoint(delta_q_reset); + + _quat_reset_counter = v_att.quat_reset_counter; } - const hrt_abstime now = _v_att.timestamp; - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.0002f, 0.02f); - _last_run = now; + const float dt = math::constrain(((v_att.timestamp - _last_run) * 1e-6f), 0.0002f, 0.02f); + _last_run = v_att.timestamp; /* check for updates in other topics */ _manual_control_setpoint_sub.update(&_manual_control_setpoint); @@ -294,13 +291,16 @@ MulticopterAttitudeControl::Run() bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); if (run_att_ctrl) { + + const Quatf q{v_att.q}; + // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode if (_v_control_mode.flag_control_manual_enabled && !_v_control_mode.flag_control_altitude_enabled && !_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_position_enabled) { - generate_attitude_setpoint(dt, _reset_yaw_sp); + generate_attitude_setpoint(q, dt, _reset_yaw_sp); attitude_setpoint_generated = true; } else { @@ -308,7 +308,7 @@ MulticopterAttitudeControl::Run() _man_y_input_filter.reset(0.f); } - Vector3f rates_sp = _attitude_control.update(Quatf(_v_att.q)); + Vector3f rates_sp = _attitude_control.update(q); if (_v_control_mode.flag_control_yawrate_override_enabled) { /* Yaw rate override enabled, overwrite the yaw setpoint */