Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4             : #if HAL_QUADPLANE_ENABLED
       5             : 
       6           3 : bool ModeQAcro::_enter()
       7             : {
       8           3 :     quadplane.throttle_wait = false;
       9           3 :     quadplane.transition->force_transition_complete();
      10           3 :     attitude_control->relax_attitude_controllers();
      11             : 
      12             :     // disable yaw rate time constant to maintain old behaviour
      13           3 :     quadplane.disable_yaw_rate_time_constant();
      14             : 
      15           3 :     IGNORE_RETURN(ahrs.get_quaternion(plane.mode_acro.acro_state.q));
      16             : 
      17           3 :     return true;
      18             : }
      19             : 
      20       11763 : void ModeQAcro::update()
      21             : {
      22             :     // get nav_roll and nav_pitch from multicopter attitude controller
      23       11763 :     Vector3f att_target = plane.quadplane.attitude_control->get_att_target_euler_cd();
      24       11763 :     plane.nav_pitch_cd = att_target.y;
      25       11763 :     plane.nav_roll_cd = att_target.x;
      26       11763 :     return;
      27             : }
      28             : 
      29             : 
      30             : /*
      31             :   control QACRO mode
      32             :  */
      33       11763 : void ModeQAcro::run()
      34             : {
      35       11763 :     const uint32_t now = AP_HAL::millis();
      36       11763 :     if (quadplane.tailsitter.in_vtol_transition(now)) {
      37             :         // Tailsitters in FW pull up phase of VTOL transition run FW controllers
      38           0 :         Mode::run();
      39           0 :         return;
      40             :     }
      41             : 
      42       11763 :     if (quadplane.throttle_wait) {
      43        1769 :         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      44        1769 :         attitude_control->set_throttle_out(0, true, 0);
      45        1769 :         quadplane.relax_attitude_control();
      46             :     } else {
      47        9994 :         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      48             : 
      49             :         // convert the input to the desired body frame rate
      50        9994 :         float target_roll = 0;
      51        9994 :         float target_pitch = plane.channel_pitch->norm_input() * quadplane.acro_pitch_rate * 100.0f;
      52        9994 :         float target_yaw = 0;
      53        9994 :         if (quadplane.tailsitter.enabled()) {
      54             :             // Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw
      55           0 :             target_roll =  plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0f;
      56           0 :             target_yaw  = -plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
      57             :         } else {
      58        9994 :             target_roll = plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
      59        9994 :             target_yaw  = plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0;
      60             :         }
      61             : 
      62        9994 :         float throttle_out = quadplane.get_pilot_throttle();
      63             : 
      64             :         // run attitude controller
      65        9994 :         if (plane.g.acro_locking) {
      66           0 :             attitude_control->input_rate_bf_roll_pitch_yaw_3_cds(target_roll, target_pitch, target_yaw);
      67             :         } else {
      68        9994 :             attitude_control->input_rate_bf_roll_pitch_yaw_2_cds(target_roll, target_pitch, target_yaw);
      69             :         }
      70             : 
      71             :         // output pilot's throttle without angle boost
      72        9994 :         attitude_control->set_throttle_out(throttle_out, false, 10.0f);
      73             :     }
      74             : 
      75             :     // Stabilize with fixed wing surfaces
      76       11763 :     plane.mode_acro.run();
      77             : }
      78             : 
      79             : #endif