diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp index 390a2de0ea..a5e0455f20 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -6,9 +6,33 @@ bool ModeQHover::_enter() return plane.mode_qstabilize._enter(); } +// init quadplane hover mode +void ModeQHover::init() +{ + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); + pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); + quadplane.set_climb_rate_cms(0, false); + + quadplane.init_throttle_wait(); +} + void ModeQHover::update() { plane.mode_qstabilize.update(); } - +/* + control QHOVER mode + */ +void ModeQHover::run() +{ + if (quadplane.throttle_wait) { + quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + attitude_control->set_throttle_out(0, true, 0); + quadplane.relax_attitude_control(); + pos_control->relax_z_controller(0); + } else { + quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms()); + } +} diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index d7baae0392..eb9a8f7e62 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1053,17 +1053,6 @@ void QuadPlane::check_attitude_relax(void) last_att_control_ms = now; } -// init quadplane hover mode -void ModeQHover::init() -{ - // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); - pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); - quadplane.set_climb_rate_cms(0, false); - - quadplane.init_throttle_wait(); -} - /* check for an EKF yaw reset */ @@ -1181,21 +1170,6 @@ float QuadPlane::get_pilot_land_throttle(void) const return constrain_float(throttle_in, 0, 1); } -/* - control QHOVER mode - */ -void ModeQHover::run() -{ - if (quadplane.throttle_wait) { - quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control->set_throttle_out(0, true, 0); - quadplane.relax_attitude_control(); - pos_control->relax_z_controller(0); - } else { - quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms()); - } -} - void ModeQLoiter::init() { // initialise loiter