From 072ba510642ba69103ffbfa617b5076b79b0973f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 9 Dec 2016 15:56:19 +0100 Subject: [PATCH] mc_pos_control_main: changes got lost in rebase --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6a91f72880..1d0cfc4ca1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -2139,11 +2139,10 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; // use the formula z_roll_pitch_sp = R_tilt * [0;0;1] - // to calculate the new desired roll and pitch angles - // R_tilt can be written as a function of the new desired roll and pitch - // angles. we get three equations and have to solve for 2 unknowns - _att_sp.pitch_body = asinf(z_roll_pitch_sp(0)); - _att_sp.roll_body = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2)); + // R_tilt is computed from_euler; only true if cos(roll) not equal zero + // -> valid if roll is not +-pi/2; + _att_sp.roll_body = -asinf(z_roll_pitch_sp(1)); + _att_sp.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2)); } /* copy quaternion setpoint to attitude setpoint topic */