dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fixed wing position control: 

- only lock on yaw if yawrate is below threshold
- remove more magic numbers
This commit is contained in:
Roman 2015-06-05 16:01:52 +02:00 committed by Lorenz Meier
parent 1f3c5d00e4
commit ef6e07fc9b
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -94,10 +94,13 @@
#include <platforms/px4_defines.h>
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 HDG_HOLD_REACHED_DIST 1000.0f
#define HDG_HOLD_SET_BACK_DIST 100.0f
#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes
/**
@ -1398,7 +1401,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* throttle limiting */
throttle_max = _parameters.throttle_max;
if (fabsf(_manual.z) < 0.05f) {
if (fabsf(_manual.z) < THROTTLE_THRESH) {
throttle_max = 0.0f;
}
@ -1418,7 +1421,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* 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 */
/* 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_max = _parameters.throttle_max;
if (fabsf(_manual.z) < 0.05f) {
if (fabsf(_manual.z) < THROTTLE_THRESH) {
throttle_max = 0.0f;
}