diff options
Diffstat (limited to 'src/modules/mc_att_control/mc_att_control_main.cpp')
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 87 |
1 files changed, 54 insertions, 33 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 e8f89923d..20c7f8859 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -86,6 +86,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.01f +#define RATES_I_LIMIT 0.5f class MulticopterAttitudeControl { @@ -133,18 +134,21 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - math::Matrix<3, 3> _K; /**< diagonal gain matrix for position error */ - math::Matrix<3, 3> _K_rate_p; /**< diagonal gain matrix for angular rate error */ - math::Matrix<3, 3> _K_rate_d; /**< diagonal gain matrix for angular rate derivative */ + math::Vector<3> _K_p; /**< P gain for position error */ + math::Vector<3> _K_rate_p; /**< P gain for angular rate error */ + math::Vector<3> _K_rate_i; /**< I gain for angular rate error */ + math::Vector<3> _K_rate_d; /**< D gain for angular rate error */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ struct { param_t att_p; + param_t yaw_p; param_t att_rate_p; + param_t att_rate_i; param_t att_rate_d; - param_t yaw_p; param_t yaw_rate_p; + param_t yaw_rate_i; param_t yaw_rate_d; } _parameter_handles; /**< handles for interesting parameters */ @@ -229,17 +233,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - _K.zero(); + _K_p.zero(); _K_rate_p.zero(); _K_rate_d.zero(); _rates_prev.zero(); _parameter_handles.att_p = param_find("MC_ATT_P"); + _parameter_handles.yaw_p = param_find("MC_YAW_P"); _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _parameter_handles.att_rate_i = param_find("MC_ATTRATE_I"); _parameter_handles.att_rate_d = param_find("MC_ATTRATE_D"); - _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _parameter_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _parameter_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); /* fetch initial parameter values */ @@ -273,31 +279,31 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() int MulticopterAttitudeControl::parameters_update() { - float att_p; - float att_rate_p; - float att_rate_d; - float yaw_p; - float yaw_rate_p; - float yaw_rate_d; - - param_get(_parameter_handles.att_p, &att_p); - param_get(_parameter_handles.att_rate_p, &att_rate_p); - param_get(_parameter_handles.att_rate_d, &att_rate_d); - param_get(_parameter_handles.yaw_p, &yaw_p); - param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p); - param_get(_parameter_handles.yaw_rate_d, &yaw_rate_d); - - _K(0, 0) = att_p; - _K(1, 1) = att_p; - _K(2, 2) = yaw_p; - - _K_rate_p(0, 0) = att_rate_p; - _K_rate_p(1, 1) = att_rate_p; - _K_rate_p(2, 2) = yaw_rate_p; - - _K_rate_d(0, 0) = att_rate_d; - _K_rate_d(1, 1) = att_rate_d; - _K_rate_d(2, 2) = yaw_rate_d; + float v; + + param_get(_parameter_handles.att_p, &v); + _K_p(0) = v; + _K_p(1) = v; + param_get(_parameter_handles.yaw_p, &v); + _K_p(2) = v; + + param_get(_parameter_handles.att_rate_p, &v); + _K_rate_p(0) = v; + _K_rate_p(1) = v; + param_get(_parameter_handles.yaw_rate_p, &v); + _K_rate_p(2) = v; + + param_get(_parameter_handles.att_rate_i, &v); + _K_rate_i(0) = v; + _K_rate_i(1) = v; + param_get(_parameter_handles.yaw_rate_i, &v); + _K_rate_i(2) = v; + + param_get(_parameter_handles.att_rate_d, &v); + _K_rate_d(0) = v; + _K_rate_d(1) = v; + param_get(_parameter_handles.yaw_rate_d, &v); + _K_rate_d(2) = v; return OK; } @@ -403,6 +409,10 @@ MulticopterAttitudeControl::task_main() math::Vector<3> rates; rates.zero(); + /* angular rates integral error */ + math::Vector<3> rates_int; + rates_int.zero(); + /* identity matrix */ math::Matrix<3, 3> I; I.identity(); @@ -621,13 +631,24 @@ MulticopterAttitudeControl::task_main() } /* angular rates setpoint*/ - math::Vector<3> rates_sp = _K * e_R; + math::Vector<3> rates_sp = _K_p.emult(e_R); /* feed forward yaw setpoint rate */ rates_sp(2) += yaw_sp_move_rate * yaw_w; - math::Vector<3> control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f); + math::Vector<3> rates_err = rates_sp - rates; + math::Vector<3> control = _K_rate_p.emult(rates_err) + _K_rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; _rates_prev = rates; + /* update integral */ + for (int i = 0; i < 3; i++) { + float rate_i = rates_int(i) + _K_rate_i(i) * rates_err(i) * dt; + if (isfinite(rate_i)) { + if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { + rates_int(i) = rate_i; + } + } + } + /* publish the attitude rates setpoint */ _rates_sp.roll = rates_sp(0); _rates_sp.pitch = rates_sp(1); |