aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: reset yaw controller on entry to POSITION1

Browse Source 
this fixes a yaw change when we finish landing approach in
QRTL. Thanks to Henry for noticing this
This commit is contained in:
Andrew Tridgell 2021-06-13 12:18:39 +10:00
parent ce4ca0ac08
commit 0c8e81964e
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/quadplane.cpp
View File

@ -2590,14 +2590,15 @@ void QuadPlane::poscontrol_init_approach(void)
void QuadPlane::PosControlState::set_state(enum position_control_state s)
{
if (state != s) {
auto &qp = plane.quadplane;
pilot_correction_done = false;
// handle resets needed for when the state changes
if (s == QPOS_POSITION1) {
reached_wp_speed = false;
qp.attitude_control->reset_yaw_target_and_rate();
} else if (s == QPOS_POSITION2) {
// POSITION2 changes target speed, so we need to change it
// back to normal
auto &qp = plane.quadplane;
qp.pos_control->set_max_speed_accel_xy(qp.wp_nav->get_default_speed_xy(),
qp.wp_nav->get_wp_acceleration());
}