diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-25 18:41:23 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-25 18:41:23 +0400 |
commit | 26daae0b0af2f73171f2e741178474873724fdbe (patch) | |
tree | 82ef7934249fa285d7ea32d0d839812641c0a6a1 /src/modules/mc_att_control_vector | |
parent | 1e4bb764a61335c0e209f83438e74264e972a164 (diff) | |
parent | 69218d2bd58a12a2ce89066a5e6a9e8682529cd5 (diff) | |
download | px4-firmware-26daae0b0af2f73171f2e741178474873724fdbe.tar.gz px4-firmware-26daae0b0af2f73171f2e741178474873724fdbe.tar.bz2 px4-firmware-26daae0b0af2f73171f2e741178474873724fdbe.zip |
Merge branch 'mathlib_new' into vector_control2
Diffstat (limited to 'src/modules/mc_att_control_vector')
-rw-r--r-- | src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | 128 |
1 files changed, 65 insertions, 63 deletions
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 76f053372..30c073c29 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 @@ -133,13 +133,11 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - bool _att_sp_valid; /**< flag if the attitude setpoint is valid */ + 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::Matrix _K; /**< diagonal gain matrix for position error */ - math::Matrix _K_rate_p; /**< diagonal gain matrix for angular rate error */ - math::Matrix _K_rate_d; /**< diagonal gain matrix for angular rate derivative */ - - math::Vector3 _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_prev; /**< angular rates on previous step */ struct { param_t att_p; @@ -222,17 +220,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), - -/* states */ - _att_sp_valid(false), - -/* gain matrices */ - _K(3, 3), - _K_rate_p(3, 3), - _K_rate_d(3, 3), - - _rates_prev(0.0f, 0.0f, 0.0f) + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) { memset(&_att, 0, sizeof(_att)); @@ -241,6 +229,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); + _K.zero(); + _K_rate_p.zero(); + _K_rate_d.zero(); + + _rates_prev.zero(); + _parameter_handles.att_p = param_find("MC_ATT_P"); _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); _parameter_handles.att_rate_d = param_find("MC_ATTRATE_D"); @@ -293,17 +287,14 @@ MulticopterAttitudeControl::parameters_update() param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p); param_get(_parameter_handles.yaw_rate_d, &yaw_rate_d); - _K.setAll(0.0f); _K(0, 0) = att_p; _K(1, 1) = att_p; _K(2, 2) = yaw_p; - _K_rate_p.setAll(0.0f); _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.setAll(0.0f); _K_rate_d(0, 0) = att_rate_d; _K_rate_d(1, 1) = att_rate_d; _K_rate_d(2, 2) = yaw_rate_d; @@ -348,7 +339,6 @@ MulticopterAttitudeControl::vehicle_setpoint_poll() if (att_sp_updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); - _att_sp_valid = true; } } @@ -401,6 +391,24 @@ MulticopterAttitudeControl::task_main() vehicle_manual_poll(); arming_status_poll(); + /* setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + R_sp.identity(); + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.identity(); + + /* current angular rates */ + math::Vector<3> rates; + rates.zero(); + + /* identity matrix */ + math::Matrix<3, 3> I; + I.identity(); + + math::Quaternion q; + bool reset_yaw_sp = true; /* wakeup source(s) */ @@ -486,6 +494,7 @@ MulticopterAttitudeControl::task_main() /* move yaw setpoint */ yaw_sp_move_rate = _manual.yaw; _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + _att_sp.R_valid = false; publish_att_sp = true; } } @@ -494,6 +503,7 @@ MulticopterAttitudeControl::task_main() if (reset_yaw_sp) { reset_yaw_sp = false; _att_sp.yaw_body = _att.yaw; + _att_sp.R_valid = false; publish_att_sp = true; } @@ -501,11 +511,10 @@ MulticopterAttitudeControl::task_main() /* update attitude setpoint if not in position control mode */ _att_sp.roll_body = _manual.roll; _att_sp.pitch_body = _manual.pitch; + _att_sp.R_valid = false; publish_att_sp = true; } - _att_sp_valid = true; - } else { /* manual rate inputs (ACRO) */ // TODO @@ -528,17 +537,15 @@ MulticopterAttitudeControl::task_main() reset_yaw_sp = true; } - if (publish_att_sp || (_att_sp_valid && !_att_sp.R_valid)) { - /* controller uses rotation matrix, not euler angles, convert if necessary */ - math::EulerAngles euler_sp(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); - math::Dcm R_sp(euler_sp); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - _att_sp.R_body[i][j] = R_sp(i, j); - } - } + if (_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_att_sp.R_body[0][0]); + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + /* copy rotation matrix back to setpoint struct */ + memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body)); _att_sp.R_valid = true; } @@ -554,41 +561,41 @@ MulticopterAttitudeControl::task_main() } } - /* desired rotation matrix */ - math::Dcm R_des(_att_sp.R_body); - /* rotation matrix for current state */ - math::Dcm R(_att.R); + R.set(_att.R); + /* current body angular rates */ - math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); + rates(0) = _att.rollspeed; + rates(1) = _att.pitchspeed; + rates(2) = _att.yawspeed; /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2)); + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ - math::Vector3 e_R = R.transpose() * R_z.cross(R_des_z); + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ - float e_R_z_sin = e_R.norm(); - float e_R_z_cos = R_z * R_des_z; + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; /* calculate weight for yaw control */ - float yaw_w = R_des(2, 2) * R_des(2, 2); + float yaw_w = R_sp(2, 2) * R_sp(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix R_rp(3, 3); + math::Matrix<3, 3> R_rp; if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector3 e_R_z_axis = e_R / e_R_z_sin; + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; e_R = e_R_z_axis * e_R_z_angle; /* cross product matrix for e_R_axis */ - math::Matrix e_R_cp(3, 3); - e_R_cp.setAll(0.0f); + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); e_R_cp(0, 1) = -e_R_z_axis(2); e_R_cp(0, 2) = e_R_z_axis(1); e_R_cp(1, 0) = e_R_z_axis(2); @@ -597,11 +604,6 @@ MulticopterAttitudeControl::task_main() e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ - math::Matrix I(3, 3); - I.setAll(0.0f); - I(0, 0) = 1.0f; - I(1, 1) = 1.0f; - I(2, 2) = 1.0f; R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { @@ -609,17 +611,17 @@ MulticopterAttitudeControl::task_main() R_rp = R; } - /* R_rp and R_des has the same Z axis, calculate yaw error */ - math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0)); - math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f(R_rp_x.cross(R_des_x) * R_des_z, R_rp_x * R_des_x) * yaw_w; + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; if (e_R_z_cos < 0.0) { /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_des rotation directly */ - math::Quaternion q(R.transpose() * R_des); - float angle_d = 2.0f * atan2f(sqrtf(q.getB() * q.getB() + q.getC() * q.getC() + q.getD() * q.getD()), q.getA()); - math::Vector3 e_R_d(q.getB(), q.getC(), q.getD()); + * calculate angle and axis for R -> R_sp rotation directly */ + q.from_dcm(R.transposed() * R_sp); + math::Vector<3> e_R_d = q.imag(); + float angle_d = 2.0f * atan2f(e_R_d.length(), q(0)); /* use fusion of Z axis based rotation and direct rotation */ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; @@ -627,11 +629,11 @@ MulticopterAttitudeControl::task_main() } /* angular rates setpoint*/ - math::Vector3 rates_sp = _K * e_R; + math::Vector<3> rates_sp = _K * e_R; /* feed forward yaw setpoint rate */ rates_sp(2) += yaw_sp_move_rate * yaw_w; - math::Vector3 control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f); + math::Vector<3> control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f); _rates_prev = rates; /* publish the attitude rates setpoint */ |