ArduPlane/mode_qacro.cpp

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