From cf658494ad5d95ebdab50ef2de03c89088859fe6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 23 Apr 2020 11:50:39 +0200 Subject: [PATCH] mc_attitude_control: keep last attitude setpoint as member The last attitude setpoint that is known from the position controller is now kept inside the AttitudeControl class such that we don't keep the whole vehicle_attitude_setpoint struct and always copy over from there to run an update step. --- .../AttitudeControl/AttitudeControl.cpp | 10 ++++++---- .../AttitudeControl/AttitudeControl.hpp | 16 +++++++++++---- .../AttitudeControl/AttitudeControlTest.cpp | 20 +++++++++++-------- src/modules/mc_att_control/mc_att_control.hpp | 6 +++--- .../mc_att_control/mc_att_control_main.cpp | 15 ++++++++------ 5 files changed, 42 insertions(+), 25 deletions(-) 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);