From 16df7e1656235b2184955363186e08d7882ad923 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 22 May 2015 21:12:18 +0200 Subject: [PATCH] fixed rates feedforward: plant for feedforward is given by derivative operator --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 47df1b4698..d65694224b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -161,6 +161,7 @@ private: perf_counter_t _controller_latency_perf; math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ float _thrust_sp; /**< thrust setpoint */ @@ -356,6 +357,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _rates_prev.zero(); _rates_sp.zero(); + _rates_sp_prev.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); @@ -715,7 +717,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; + _rates_sp_prev = _rates_sp; _rates_prev = rates; /* update integral only if not saturated on low limit and if motor commands are not saturated */