Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4             : #if HAL_QUADPLANE_ENABLED
       5             : 
       6          26 : bool ModeQHover::_enter()
       7             : {
       8             :     // set vertical speed and acceleration limits
       9             :     // All limits must be positive
      10          26 :     pos_control->D_set_max_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss);
      11          26 :     pos_control->D_set_correction_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss);
      12          26 :     quadplane.set_climb_rate_ms(0);
      13             : 
      14          26 :     quadplane.init_throttle_wait();
      15          26 :     return true;
      16             : }
      17             : 
      18      338829 : void ModeQHover::update()
      19             : {
      20      338829 :     plane.mode_qstabilize.update();
      21      338829 : }
      22             : 
      23             : /*
      24             :   control QHOVER mode
      25             :  */
      26      355233 : void ModeQHover::run()
      27             : {
      28      355233 :     quadplane.assist.check_VTOL_recovery();
      29             : 
      30      355233 :     const uint32_t now = AP_HAL::millis();
      31      355233 :     if (quadplane.tailsitter.in_vtol_transition(now)) {
      32             :         // Tailsitters in FW pull up phase of VTOL transition run FW controllers
      33           1 :         Mode::run();
      34           1 :         return;
      35             :     }
      36             : 
      37      355232 :     if (quadplane.throttle_wait) {
      38      203662 :         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      39      203662 :         attitude_control->set_throttle_out(0, true, 0);
      40      203662 :         quadplane.relax_attitude_control();
      41      203662 :         pos_control->D_relax_controller(0);
      42             :     } else {
      43      151570 :         plane.quadplane.assign_tilt_to_fwd_thr();
      44      151570 :         quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
      45             :     }
      46             : 
      47             :     // Stabilize with fixed wing surfaces
      48      355232 :     plane.stabilize_roll();
      49      355232 :     plane.stabilize_pitch();
      50             : 
      51             :     // Center rudder
      52      355232 :     output_rudder_and_steering(0.0);
      53             : 
      54             :     // possibly apply spin recovery
      55      355232 :     quadplane.assist.output_spin_recovery();
      56             : }
      57             : 
      58             : #endif