From f6b3692696b0ab0cce6bf5e659e72b1d12013745 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jul 2013 09:09:11 +0200 Subject: [PATCH] Minor fixes and cleanups, ready for first bench tests --- .../ecl_mc_att_control_vector.cpp | 21 +++++++++---------- .../ecl_mc_att_control_vector.h | 16 ++++++++++++++ .../mc_att_control_vector_main.cpp | 17 ++++++++------- 3 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp index b21b0cf67e..8bee18ba49 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp @@ -48,20 +48,20 @@ #include #include "ecl_mc_att_control_vector.h" +ECL_MCAttControlVector::ECL_MCAttControlVector() : + _integral_error(0.0f, 0.0f), + _integral_max(1000.0f), + _integral_lock(false) + { + + } + void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des_in, float Kp, float Kd, float Ki, const math::Vector &angular_rates, math::Vector &rates_des, float &thrust) { - //function [rates_des,Thrust,ie_out] = attitudeController(R1,yaw,F_des,Kp,Kd,Ki,e_Rd_v,ie) - //% dt - //dt=1/50; - - //% shoult the controll be in the body frame or in the inertial frame: 0 is - //% body frame, 1 is inertial frame. - // XXX bool earth = true; - bool integral_freeze = false; bool integral_reset = false; math::Matrix R_bn = R_nb.transpose(); @@ -70,7 +70,6 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, float sy = sinf(yaw); math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f); - //%F_des=RYaw*F_des; math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2)); math::Vector3 F_des = F_des_in; @@ -97,7 +96,7 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, // desired direction in world coordinates (yaw angle) math::Vector3 x_C(cy, sy, 0.0f); - //desired body y axis + // desired body y axis y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm(); // desired body x axis x_B_des = y_B_des.cross(z_B_des); @@ -122,7 +121,7 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, // include an integral part math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f); - if (!integral_freeze) { + if (!_integral_lock) { if (thrust > 0.3f && !integral_reset) { if (fabsf(_integral_error(0)) < _integral_max(0)) { _integral_error(0) = _integral_error(0) + e_R_v(0) * dt; diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h index 98689f0bee..b975f2f7f5 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h @@ -50,11 +50,27 @@ class ECL_MCAttControlVector { public: + ECL_MCAttControlVector(float integral_max); void control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des, float Kp, float Kd, float Ki, const math::Vector &angular_rates, math::Vector &rates_des, float &thrust); + void set_imax(float integral_max) { + _integral_max(0) = integral_max; + _integral_max(1) = integral_max; + } + + void reset_integral() { + _integral_error(0) = 0.0f; + _integral_error(1) = 0.0f; + } + + void lock_integral(bool lock) { + _integral_lock = lock; + } + protected: math::Vector2 _integral_error; math::Vector2 _integral_max; + bool _integral_lock; }; diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 5b4a3b6000..368d343bde 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -309,11 +309,12 @@ MulticopterAttitudeControl::parameters_update() param_get(_parameter_handles.att_rate_p, &(_parameters.att_rate_p)); - /* class control parameters */ - // _att_ctrl.set_tau(_parameters.p_tconst); - // _att_ctrl.set_k_p(math::radians(_parameters.p_p)); - // _att_ctrl.set_k_i(math::radians(_parameters.p_i)); - // _att_ctrl.set_k_d(math::radians(_parameters.p_d)); + /* att control parameters */ + _att_control.set_imax(_parameters.att_imax); + //_att_control.set_tau(_parameters.p_tconst); + // _att_control.set_k_p(math::radians(_parameters.p_p)); + // _att_control.set_k_i(math::radians(_parameters.p_i)); + // _att_control.set_k_d(math::radians(_parameters.p_d)); return OK; } @@ -488,11 +489,11 @@ MulticopterAttitudeControl::task_main() vehicle_manual_poll(); /* lock integrator until armed */ - bool lock_integrator; if (_arming.armed) { - lock_integrator = false; + _att_control.lock_integrator(false); } else { - lock_integrator = true; + _att_control.reset_integral(); + _att_control.lock_integrator(true); } /* decide if in auto or full manual control */