diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index cc312f5dbc..771f1afaa1 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -52,8 +52,10 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g } } -matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, const float yawspeed_feedforward) +matrix::Vector3f AttitudeControl::update(matrix::Quatf q) const { + Quatf qd = _attitude_setpoint_q; + // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN q.normalize(); qd.normalize(); @@ -93,13 +95,13 @@ matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, cons matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); // Feed forward the yaw setpoint rate. - // yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis, + // yawspeed_setpoint is the feed forward commanded rotation around the world z-axis, // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame). // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed) - // and multiply it by the yaw setpoint rate (yaw_sp_move_rate). + // and multiply it by the yaw setpoint rate (yawspeed_setpoint). // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame // such that it can be added to the rates setpoint. - rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward; + rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; // limit rates for (int i = 0; i < 3; i++) { diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index 82106f8727..8883708b84 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -70,17 +70,25 @@ public: */ void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; } + /** + * Set a new attitude setpoint replacing the one tracked before + * @param qd desired vehicle attitude setpoint + * @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame + */ + void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _yawspeed_setpoint = yawspeed_setpoint; } + /** * Run one control loop cycle calculation * @param q estimation of the current vehicle attitude unit quaternion - * @param qd desired vehicle attitude setpoint - * @param yawspeed_feedforward [rad/s] yaw feed forward angular rate in world frame * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller */ - matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward); + matrix::Vector3f update(matrix::Quatf q) const; private: matrix::Vector3f _proportional_gain; matrix::Vector3f _rate_limit; - float _yaw_w{0.f}; /**< yaw weight [0,1] to prioritize roll and pitch */ + float _yaw_w{0.f}; ///< yaw weight [0,1] to deprioritize caompared to roll and pitch + + matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control + float _yawspeed_setpoint{0.f}; ///< latest known yawspeed feed-forward setpoint }; diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp index ffa9dc1103..2803686c12 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp @@ -40,7 +40,7 @@ using namespace matrix; TEST(AttitudeControlTest, AllZeroCase) { AttitudeControl attitude_control; - Vector3f rate_setpoint = attitude_control.update(Quatf(), Quatf(), 0.f); + Vector3f rate_setpoint = attitude_control.update(Quatf()); EXPECT_EQ(rate_setpoint, Vector3f()); } @@ -58,9 +58,11 @@ public: int i; // need function scope to check how many steps Vector3f rate_setpoint(1000.f, 1000.f, 1000.f); + _attitude_control.setAttitudeSetpoint(_quat_goal, 0.f); + for (i = 100; i > 0; i--) { // run attitude control to get rate setpoints - const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state, _quat_goal, 0.f); + const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state); // rotate the simulated state quaternion according to the rate setpoint _quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new)); _quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem @@ -112,25 +114,27 @@ TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence) TEST(AttitudeControlTest, YawWeightScaling) { - // GIVEN: default tuning + // GIVEN: default tuning and pure yaw turn command AttitudeControl attitude_control; const float yaw_gain = 2.8f; const float yaw_sp = .1f; + Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)); attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), .4f); attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f)); + attitude_control.setAttitudeSetpoint(pure_yaw_attitude, 0.f); - // WHEN: we command a pure yaw turn - Vector3f rate_setpoint = attitude_control.update(Quatf(), Quatf(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)), 0.f); + // WHEN: we run one iteration of the controller + Vector3f rate_setpoint = attitude_control.update(Quatf()); // THEN: no actuation in roll, pitch EXPECT_EQ(Vector2f(rate_setpoint), Vector2f()); // THEN: actuation error * gain in yaw EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f); - // GIVEN: corner case of zero yaw weight + // GIVEN: additional corner case of zero yaw weight attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), 0.f); - // WHEN: we command a pure yaw turn - rate_setpoint = attitude_control.update(Quatf(), Quatf(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)), 0.f); + // WHEN: we run one iteration of the controller + rate_setpoint = attitude_control.update(Quatf()); // THEN: no actuation (also no NAN) EXPECT_EQ(rate_setpoint, Vector3f()); } diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 24a8f19791..c8e9bfa543 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -105,7 +105,7 @@ private: AttitudeControl _attitude_control; ///< class for attitude control calculations - uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; 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 _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ @@ -119,7 +119,6 @@ private: uORB::Publication _vehicle_attitude_setpoint_pub; struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _v_att_sp {}; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp {}; /**< vehicle rates setpoint */ struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */ struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ @@ -128,7 +127,8 @@ private: perf_counter_t _loop_perf; /**< loop duration performance counter */ - matrix::Vector3f _rates_sp; /**< angular rates setpoint */ + matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector + matrix::Vector3f _rates_sp; ///< angular rates setpoint float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ 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 4de41fa3b9..6173e1a908 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -69,7 +69,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : /* initialize quaternions in messages to be valid */ _v_att.q[0] = 1.f; - _v_att_sp.q_d[0] = 1.f; parameters_updated(); } @@ -234,8 +233,14 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ void MulticopterAttitudeControl::control_attitude() { - _v_att_sp_sub.update(&_v_att_sp); - _rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate); + if (_vehicle_attitude_setpoint_sub.updated()) { + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + _vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint); + _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); + _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); + } + + _rates_sp = _attitude_control.update(Quatf(_v_att.q)); } void @@ -246,9 +251,7 @@ MulticopterAttitudeControl::publish_rates_setpoint() v_rates_sp.roll = _rates_sp(0); v_rates_sp.pitch = _rates_sp(1); v_rates_sp.yaw = _rates_sp(2); - v_rates_sp.thrust_body[0] = _v_att_sp.thrust_body[0]; - v_rates_sp.thrust_body[1] = _v_att_sp.thrust_body[1]; - v_rates_sp.thrust_body[2] = _v_att_sp.thrust_body[2]; + _thrust_setpoint_body.copyTo(v_rates_sp.thrust_body); v_rates_sp.timestamp = hrt_absolute_time(); _v_rates_sp_pub.publish(v_rates_sp);