Skip to content

Commit

Permalink
Plane: fixed nav_roll/nav_pitch when waiting for VTOL takeoff
Browse files Browse the repository at this point in the history
the nav_roll_cd and nav_pitch_cd were not being set in the VTOL
takeoff code when disarmed. This led to small increments accumulating
in the stick mixing code, leading to large control surface movements
before arming
  • Loading branch information
tridge committed Sep 2, 2023
1 parent 9742997 commit 9d98244
Showing 1 changed file with 4 additions and 2 deletions.
6 changes: 4 additions & 2 deletions ArduPlane/quadplane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2946,6 +2946,10 @@ void QuadPlane::setup_target_position(void)
*/
void QuadPlane::takeoff_controller(void)
{
// reset fixed wing controller to neutral as base output
plane.nav_roll_cd = 0;
plane.nav_pitch_cd = 0;

if (!plane.arming.is_armed_and_safety_off()) {
return;
}
Expand Down Expand Up @@ -3007,8 +3011,6 @@ void QuadPlane::takeoff_controller(void)
takeoff_last_run_ms = now;

if (no_navigation) {
plane.nav_roll_cd = 0;
plane.nav_pitch_cd = 0;
pos_control->relax_velocity_controller_xy();
} else {
pos_control->set_accel_desired_xy_cmss(zero);
Expand Down

0 comments on commit 9d98244

Please sign in to comment.