fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 00:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control_main: simplify manual control handling

Browse Source
This commit is contained in:
Beat Küng
2017-09-19 11:44:29 +02:00
committed by Matthias Grob
parent d6df692b7a
commit 05e3c58e18
1 changed files with 5 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+5 -8
View File
@@ -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: