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

fixed wing position control:

Browse Source 
- only lock on yaw if yawrate is below threshold
- remove more magic numbers
This commit is contained in:
Roman
2015-06-05 22:45:07 +02:00
committed by Lorenz Meier
parent 1f3c5d00e4
commit ef6e07fc9b
1 changed files with 9 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+9 -6
View File
@@ -94,10 +94,13 @@
#include <platforms/px4_defines.h> #include <platforms/px4_defines.h>
static int _control_task = -1; /**< task handle for sensor task */ static int _control_task = -1; /**< task handle for sensor task */
#define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode
#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane
#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode
#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading
#define HDG_HOLD_DIST_NEXT 3000.0f #define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes
#define HDG_HOLD_REACHED_DIST 1000.0f
#define HDG_HOLD_SET_BACK_DIST 100.0f
/** /**
@@ -1398,7 +1401,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* throttle limiting */ /* throttle limiting */
throttle_max = _parameters.throttle_max; throttle_max = _parameters.throttle_max;
if (fabsf(_manual.z) < 0.05f) { if (fabsf(_manual.z) < THROTTLE_THRESH) {
throttle_max = 0.0f; throttle_max = 0.0f;
} }
@@ -1418,7 +1421,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* heading control */ /* heading control */
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH) {
/* heading / roll is zero, lock onto current heading */ /* heading / roll is zero, lock onto current heading */
/* just switched back from non heading-hold to heading hold */ /* just switched back from non heading-hold to heading hold */
@@ -1482,7 +1485,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* throttle limiting */ /* throttle limiting */
throttle_max = _parameters.throttle_max; throttle_max = _parameters.throttle_max;
if (fabsf(_manual.z) < 0.05f) { if (fabsf(_manual.z) < THROTTLE_THRESH) {
throttle_max = 0.0f; throttle_max = 0.0f;
} }