From 05e3c58e18c85041dc26a635f2243d7b3d428633 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 18 Sep 2017 14:18:18 +0200 Subject: [PATCH] mc_pos_control_main: simplify manual control handling --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 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 38c82521f9..cf95b11544 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -2825,7 +2825,7 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) * ---------------------------------------- * This simplest thing to do is map the y & x inputs directly to roll and pitch, and scale according to the max * tilt angle. - * But this has several issue: + * But this has several issues: * - The maximum tilt angle cannot easily be restricted. By limiting the roll and pitch separately, * it would be possible to get to a higher tilt angle by combining roll and pitch (the difference is * around 15 degrees maximum, so quite noticeable). Limiting this angle is not simple in roll-pitch-space, @@ -2836,7 +2836,7 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) * on the tilt angle (for a tilt angle of 35 degrees, it's off by about 5 degrees). * * So instead we control the following 2 angles: - * - tilt angle + * - tilt angle, given by sqrt(x*x + y*y) * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion * * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick @@ -2844,18 +2844,15 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) */ const float x = _manual.x * _params.man_tilt_max; const float y = _manual.y * _params.man_tilt_max; - const float tilt_angle_sp = math::min(_params.man_tilt_max, sqrtf(x * x + y * y)); // we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane matrix::Vector2f v = matrix::Vector2f(y, -x); - float v_norm = v.norm(); + float v_norm = v.norm(); // the norm of v defines the tilt angle - if (v_norm > FLT_EPSILON) { - v /= v_norm; + if (v_norm > _params.man_tilt_max) { // limit to the configured maximum tilt angle + v *= _params.man_tilt_max / v_norm; } - v *= tilt_angle_sp; // the norm of v defines the tilt angle - matrix::Quatf q_sp_rpy = matrix::AxisAnglef(v(0), v(1), 0.f); // The axis angle can change the yaw as well (but only at higher tilt angles). // This the the formula by how much the yaw changes: