diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 08f66ba4da..48a33cccd9 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2440,6 +2440,17 @@ bool QuadPlane::in_vtol_posvel_mode(void) const in_vtol_auto()); } +/* + run (and possibly init) xy controller + */ +void QuadPlane::run_xy_controller(void) +{ + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } + pos_control->update_xy_controller(); +} + /* main landing controller. Used for landing and RTL. */ @@ -2512,7 +2523,7 @@ void QuadPlane::vtol_position_controller(void) pos_control->set_accel_desired_xy_cmss(0.0f, 0.0f); // run horizontal velocity controller - pos_control->update_xy_controller(); + run_xy_controller(); // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); @@ -2579,7 +2590,7 @@ void QuadPlane::vtol_position_controller(void) } else { pos_control->set_pos_target_xy_cm(poscontrol.target.x, poscontrol.target.y); } - pos_control->update_xy_controller(); + run_xy_controller(); // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); @@ -2714,7 +2725,8 @@ void QuadPlane::takeoff_controller(void) // set position control target and update pos_control->set_pos_target_xy_cm(poscontrol.target.x, poscontrol.target.y); - pos_control->update_xy_controller(); + + run_xy_controller(); // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 4be42f7f42..6585003e8d 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -292,6 +292,7 @@ private: void update_throttle_suppression(void); void run_z_controller(void); + void run_xy_controller(void); void setup_defaults(void);