From c4fb2b26fd5705d13df2fc7912eb2d37a76cf0c3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 3 Mar 2018 20:57:32 +0000 Subject: [PATCH] mc_att_control: prepare yaw weight from gain ratio According to the paper the quaternion controller is built on the yaw weight represents the ratio between the roll/pitch and the yaw attitude control time constant. It also states that as a thumb rule a value of ~0.4 works alright for most multicopter platforms. The default attitude gains of PX4 which were determined independent of the paper from experimental results have a ratio of 2.8/6.5 = 0.43 which matches. --- .../mc_att_control/mc_att_control_main.cpp | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) 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 9ca899f99c..07a78a56c8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -101,6 +101,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MAX_GYRO_COUNT 3 +using namespace matrix; + class MulticopterAttitudeControl { public: @@ -235,7 +237,7 @@ private: } _params_handles; /**< handles for interesting parameters */ struct { - math::Vector<3> att_p; /**< P gain for angular error */ + Vector3f attitude_p; /**< P gain for attitude control */ math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_int_lim; /**< integrator state limit for rate loop */ @@ -388,7 +390,6 @@ _loop_update_rate_hz(initial_update_rate_hz) _vehicle_status.is_rotary_wing = true; - _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_int_lim.zero(); @@ -520,7 +521,7 @@ MulticopterAttitudeControl::parameters_update() /* roll gains */ param_get(_params_handles.roll_p, &v); - _params.att_p(0) = v; + _params.attitude_p(0) = v; param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v; param_get(_params_handles.roll_rate_i, &v); @@ -534,7 +535,7 @@ MulticopterAttitudeControl::parameters_update() /* pitch gains */ param_get(_params_handles.pitch_p, &v); - _params.att_p(1) = v; + _params.attitude_p(1) = v; param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v; param_get(_params_handles.pitch_rate_i, &v); @@ -566,7 +567,7 @@ MulticopterAttitudeControl::parameters_update() /* yaw gains */ param_get(_params_handles.yaw_p, &v); - _params.att_p(2) = v; + _params.attitude_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; param_get(_params_handles.yaw_rate_i, &v); @@ -826,18 +827,20 @@ MulticopterAttitudeControl::control_attitude(float dt) vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; - using namespace matrix; - float yaw_w = .4f; + /* prepare yaw weight from the ratio between roll/pitch and yaw gains */ + Vector3f attitude_gain = _params.attitude_p; + const float roll_pitch_gain = (attitude_gain(0) + attitude_gain(1)) / 2.f; + const float yaw_w = math::constrain(attitude_gain(2) / roll_pitch_gain, 0.f, 1.f); + attitude_gain(2) = roll_pitch_gain; /* get estimated and desired vehicle attitude */ Quatf q(_v_att.q); Quatf qd(_v_att_sp.q_d); - /* ensure quaternions are exactly normalized because acosf(1.00001) == NaN */ + /* ensure input quaternions are exactly normalized because acosf(1.00001) == NaN */ q.normalize(); qd.normalize(); - /* calculate reduced attitude which we would command if we about the vehicle's yaw */ Vector3f e_z = q.dcm_z(); Vector3f e_z_d = qd.dcm_z(); @@ -849,22 +852,21 @@ MulticopterAttitudeControl::control_attitude(float dt) q_mix *= math::sign(q_mix(0)); qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3)))); - /* quaternion attitude control law, qe is rotation from q to qd */ Quatf qe = q.inversed() * qd; /* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) * also taking care of the antipodal unit quaternion ambiguity */ Vector3f eq = 2.f * math::sign(qe(0)) * qe.imag(); - math::Vector<3> e_R(eq.data()); /* calculate angular rates setpoint */ - _rates_sp = _params.att_p.emult(e_R); + eq = eq.emult(attitude_gain); + _rates_sp = math::Vector<3>(eq.data()); /* Feed forward the yaw setpoint rate. We need to transform the yaw from world into body frame. * The following is a simplification of: - * R.transposed() * math::Vector<3>(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff) + * R.transposed() * Vector3f(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff) */ Vector3f yaw_feedforward_rate = q.inversed().dcm_z(); yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff; @@ -1148,7 +1150,8 @@ MulticopterAttitudeControl::task_main() _thrust_sp = _v_att_sp.thrust; math::Quaternion q(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]); math::Quaternion q_sp(&_v_att_sp.q_d[0]); - _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); + math::Vector<3> attitude_p(_params.attitude_p.data()); + _ts_opt_recovery->setAttGains(attitude_p, _params.yaw_ff); _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); /* limit rates */