diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-15 20:43:21 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-15 20:43:21 +0400 |
commit | 69c4f6f5e45c054a22ed1a36b323d1b9bd7561d3 (patch) | |
tree | 15c157ab8d017a1635b95b5a2d186ff13691d98d /src | |
parent | 86d5f0808d0146efc2442f651de224956a2818cc (diff) | |
download | px4-firmware-69c4f6f5e45c054a22ed1a36b323d1b9bd7561d3.tar.gz px4-firmware-69c4f6f5e45c054a22ed1a36b323d1b9bd7561d3.tar.bz2 px4-firmware-69c4f6f5e45c054a22ed1a36b323d1b9bd7561d3.zip |
mc_att_control_vector: code style fixed
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | 7 |
1 files changed, 6 insertions, 1 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 bc75885c2..45dd639da 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 @@ -459,7 +459,7 @@ MulticopterAttitudeControl::task_main() if (_att_sp.thrust < 0.1f) { // TODO //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ + /* reset yaw setpoint if on ground */ // reset_yaw_sp = true; //} } else { @@ -486,6 +486,7 @@ MulticopterAttitudeControl::task_main() } _att_sp_valid = true; + } else { /* manual rate inputs (ACRO) */ // TODO @@ -512,17 +513,20 @@ MulticopterAttitudeControl::task_main() /* 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); } } + _att_sp.R_valid = true; } if (publish_att_sp) { /* publish the attitude setpoint */ _att_sp.timestamp = hrt_absolute_time(); + if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); @@ -575,6 +579,7 @@ MulticopterAttitudeControl::task_main() I(1, 1) = 1.0f; I(2, 2) = 1.0f; R_rp = R * (I + e_R_cp * e_R_sin + e_R_cp * e_R_cp * (1.0f - e_R_cos)); + } else { /* zero roll/pitch rotation */ R_rp = R; |