Skip to content

Commit

Permalink
Plane: quadplane: check_attitude_relax use the last time the controll…
Browse files Browse the repository at this point in the history
…ers were run not the last time relax was checked
  • Loading branch information
IamPete1 authored and tridge committed Sep 6, 2021
1 parent fb8fb34 commit 95fbf39
Showing 1 changed file with 4 additions and 4 deletions.
8 changes: 4 additions & 4 deletions ArduPlane/quadplane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -987,11 +987,9 @@ void QuadPlane::relax_attitude_control()
*/
void QuadPlane::check_attitude_relax(void)
{
uint32_t now = AP_HAL::millis();
if (now - last_att_control_ms > 100) {
if (AP_HAL::millis() - last_att_control_ms > 100) {
relax_attitude_control();
}
last_att_control_ms = now;
}

/*
Expand Down Expand Up @@ -1943,8 +1941,10 @@ void QuadPlane::update_throttle_hover()
*/
void QuadPlane::motors_output(bool run_rate_controller)
{
const uint32_t now = AP_HAL::millis();
if (run_rate_controller) {
attitude_control->rate_controller_run();
last_att_control_ms = now;
}

/* Delay for ARMING_DELAY_MS after arming before allowing props to spin:
Expand Down Expand Up @@ -1992,7 +1992,7 @@ void QuadPlane::motors_output(bool run_rate_controller)

// remember when motors were last active for throttle suppression
if (motors->get_throttle() > 0.01f || tilt.motors_active) {
last_motors_active_ms = AP_HAL::millis();
last_motors_active_ms = now;
}

}
Expand Down

0 comments on commit 95fbf39

Please sign in to comment.