ArduPlane/mode_qstabilize.cpp

   1 #include "mode.h"
   2 #include "Plane.h"
   3
   4 #if HAL_QUADPLANE_ENABLED
   5
   6 bool ModeQStabilize::_enter()
   7 {
   8     quadplane.throttle_wait = false;
   9     return true;
  10 }
  11
  12 void ModeQStabilize::update()
  13 {
  14     // set nav_roll and nav_pitch using sticks
  15     // Beware that QuadPlane::tailsitter_check_input (called from Plane::read_radio)
  16     // may alter the control_in values for roll and yaw, but not the corresponding
  17     // radio_in values. This means that the results for norm_input would not necessarily
  18     // be correct for tailsitters, so get_control_in() must be used instead.
  19     // normalize control_input to [-1,1]
  20     const float roll_input = (float)plane.channel_roll->get_control_in() / plane.channel_roll->get_range();
  21     const float pitch_input = (float)plane.channel_pitch->get_control_in() / plane.channel_pitch->get_range();
  22
  23     // then scale to target angles in centidegrees
  24     if (plane.quadplane.tailsitter.active()) {
  25         // tailsitters are different
  26         set_tailsitter_roll_pitch(roll_input, pitch_input);
  27         return;
  28     }
  29
  30     if (!plane.quadplane.option_is_set(QuadPlane::OPTION::INGORE_FW_ANGLE_LIMITS_IN_Q_MODES)) {
  31         // by default angles are also constrained by forward flight limits
  32         set_limited_roll_pitch(roll_input, pitch_input);
  33     } else {
  34         // use angle max for both roll and pitch
  35         plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max;
  36         plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
  37     }
  38 }
  39
  40 // quadplane stabilize mode
  41 void ModeQStabilize::run()
  42 {
  43     const uint32_t now = AP_HAL::millis();
  44     if (quadplane.tailsitter.in_vtol_transition(now)) {
  45         // Tailsitters in FW pull up phase of VTOL transition run FW controllers
  46         Mode::run();
  47         return;
  48     }
  49
  50     plane.quadplane.assign_tilt_to_fwd_thr();
  51
  52     // special check for ESC calibration in QSTABILIZE
  53     if (quadplane.esc_calibration != 0) {
  54         quadplane.run_esc_calibration();
  55         plane.stabilize_roll();
  56         plane.stabilize_pitch();
  57         return;
  58     }
  59
  60     // normal QSTABILIZE mode
  61     float pilot_throttle_scaled = quadplane.get_pilot_throttle();
  62     quadplane.hold_stabilize(pilot_throttle_scaled);
  63
  64     // Stabilize with fixed wing surfaces
  65     plane.stabilize_roll();
  66     plane.stabilize_pitch();
  67 }
  68
  69 // set the desired roll and pitch for a tailsitter
  70 void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const float pitch_input)
  71 {
  72     // separate limit for roll, if set
  73     if (plane.quadplane.tailsitter.max_roll_angle > 0) {
  74         // roll param is in degrees not centidegrees
  75         plane.nav_roll_cd = plane.quadplane.tailsitter.max_roll_angle * 100.0f * roll_input;
  76     } else {
  77         plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max;
  78     }
  79
  80     // angle max for tailsitter pitch
  81     plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
  82
  83     plane.quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd);
  84 }
  85
  86 // set the desired roll and pitch for normal quadplanes, also limited by forward flight limits
  87 void ModeQStabilize::set_limited_roll_pitch(const float roll_input, const float pitch_input)
  88 {
  89     plane.nav_roll_cd = roll_input * MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max);
  90     // pitch is further constrained by PTCH_LIM_MIN/MAX which may impose
  91     // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX
  92     if (pitch_input > 0) {
  93         plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max*100, plane.quadplane.aparm.angle_max);
  94     } else {
  95         plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min*100, plane.quadplane.aparm.angle_max);
  96     }
  97 }
  98
  99 #endif