From 6906d966ce818d0b428e58ad9fca428e60cf1a72 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Tue, 22 Nov 2016 11:39:39 +0100 Subject: [PATCH] manual input mapping to roll and pitch --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 +++++----- 1 file changed, 5 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 b8c46d791c..1692b4b7c9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -2160,11 +2160,11 @@ MulticopterPositionControl::task_main() 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 */