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

fixed wing posctrl: 

- lock desired yaw once yaw speed is small
This commit is contained in:
tumbili 2015-06-07 22:27:31 +02:00
parent 02efa5a24c
commit d40f94bf26
1 changed files with 37 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -176,6 +176,7 @@ private:
float _ground_alt; /**< ground altitude at which plane was launched */
float _hdg_hold_yaw; /**< hold heading for velocity mode */
bool _hdg_hold_enabled; /**< heading hold enabled */
bool _yaw_lock_engaged; /**< yaw is locked for heading hold */
struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */
struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */
hrt_abstime _control_position_last_called; /**<last call of control_position */
@ -495,6 +496,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_ground_alt(0.0f),
_hdg_hold_yaw(0.0f),
_hdg_hold_enabled(false),
_yaw_lock_engaged(false),
_hdg_hold_prev_wp{},
_hdg_hold_curr_wp{},
_control_position_last_called(0),
@ -1377,6 +1379,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_hold_alt = _global_pos.alt;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
}
/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
@ -1421,15 +1424,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* heading control */
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH) {
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
}
if (_yaw_lock_engaged) {
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _att.yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
/* we have a valid heading hold position, are we too close? */
@ -1451,8 +1461,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
}
} else {
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
}
} else if (_control_mode.flag_control_altitude_enabled) {