Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed VTOL state error when using Q_ENABLE=2 

this ensures the poscontrol state is reset if it has not been
initialised before a VTOL loiter operation

thanks to Pete Hall for finding this
This commit is contained in:
Andrew Tridgell 2021-10-28 05:58:36 +11:00 committed by Randy Mackay
parent cadd0b0232
commit 9cb67adedb
2 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduPlane/quadplane.cpp
View File

@ -2614,6 +2614,10 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
}
state = s;
last_state_change_ms = AP_HAL::millis();
// we consider setting the state to be equivalent to running to
// prevent code from overriding the state as stale
last_run_ms = last_state_change_ms;
}
/*
@ -2636,6 +2640,10 @@ void QuadPlane::vtol_position_controller(void)
check_attitude_relax();
if (hal.util->get_soft_armed()) {
poscontrol.last_run_ms = now_ms;
}
// horizontal position control
switch (poscontrol.get_state()) {
@ -3207,8 +3215,14 @@ void QuadPlane::control_auto(void)
case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_LOITER_TIME:
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_LOITER_TO_ALT:
case MAV_CMD_NAV_LOITER_TO_ALT: {
const uint32_t now = AP_HAL::millis();
if (now - poscontrol.last_run_ms > 100) {
// ensure that poscontrol is reset
poscontrol.set_state(QPOS_POSITION1);
}
vtol_position_controller();
}
break;
default:
waypoint_controller();

1
ArduPlane/quadplane.h
View File

@ -486,6 +486,7 @@ private:
uint32_t thrust_loss_start_ms;
uint32_t last_log_ms;
bool reached_wp_speed;
uint32_t last_run_ms;
private:
uint32_t last_state_change_ms;
enum position_control_state state;