ArduPlane/mode_qhover.cpp

   1 #include "mode.h"
   2 #include "Plane.h"
   3
   4 #if HAL_QUADPLANE_ENABLED
   5
   6 bool ModeQHover::_enter()
   7 {
   8     // set vertical speed and acceleration limits
   9     pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100);
  10     pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100);
  11     quadplane.set_climb_rate_cms(0);
  12
  13     quadplane.init_throttle_wait();
  14     return true;
  15 }
  16
  17 void ModeQHover::update()
  18 {
  19     plane.mode_qstabilize.update();
  20 }
  21
  22 /*
  23   control QHOVER mode
  24  */
  25 void ModeQHover::run()
  26 {
  27     const uint32_t now = AP_HAL::millis();
  28     if (quadplane.tailsitter.in_vtol_transition(now)) {
  29         // Tailsitters in FW pull up phase of VTOL transition run FW controllers
  30         Mode::run();
  31         return;
  32     }
  33
  34     if (quadplane.throttle_wait) {
  35         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
  36         attitude_control->set_throttle_out(0, true, 0);
  37         quadplane.relax_attitude_control();
  38         pos_control->relax_z_controller(0);
  39     } else {
  40         plane.quadplane.assign_tilt_to_fwd_thr();
  41         quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
  42     }
  43
  44     // Stabilize with fixed wing surfaces
  45     plane.stabilize_roll();
  46     plane.stabilize_pitch();
  47 }
  48
  49 #endif