Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4             : #if HAL_QUADPLANE_ENABLED
       5             : 
       6           3 : bool ModeQStabilize::_enter()
       7             : {
       8           3 :     quadplane.throttle_wait = false;
       9           3 :     return true;
      10             : }
      11             : 
      12     1616725 : 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     1616725 :     const float roll_input = (float)plane.channel_roll->get_control_in() / plane.channel_roll->get_range();
      21     1616725 :     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     1616725 :     if (plane.quadplane.tailsitter.active()) {
      25             :         // tailsitters are different
      26       47327 :         set_tailsitter_roll_pitch(roll_input, pitch_input);
      27       47327 :         return;
      28             :     }
      29             : 
      30     1569398 :     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     1569398 :         set_limited_roll_pitch(roll_input, pitch_input);
      33             :     } else {
      34             :         // use angle max for both roll and pitch
      35           0 :         const int16_t angle_max_cd = plane.quadplane.attitude_control->lean_angle_max_cd();
      36           0 :         plane.nav_roll_cd = roll_input * angle_max_cd;
      37           0 :         plane.nav_pitch_cd = pitch_input * angle_max_cd;
      38             :     }
      39             : }
      40             : 
      41             : // quadplane stabilize mode
      42       15235 : void ModeQStabilize::run()
      43             : {
      44       15235 :     const uint32_t now = AP_HAL::millis();
      45       15235 :     if (quadplane.tailsitter.in_vtol_transition(now)) {
      46             :         // Tailsitters in FW pull up phase of VTOL transition run FW controllers
      47           0 :         Mode::run();
      48           0 :         return;
      49             :     }
      50             : 
      51       15235 :     plane.quadplane.assign_tilt_to_fwd_thr();
      52             : 
      53             :     // special check for ESC calibration in QSTABILIZE
      54       15235 :     if (quadplane.esc_calibration != 0) {
      55           0 :         quadplane.run_esc_calibration();
      56           0 :         plane.stabilize_roll();
      57           0 :         plane.stabilize_pitch();
      58           0 :         return;
      59             :     }
      60             : 
      61             :     // normal QSTABILIZE mode
      62       15235 :     float pilot_throttle_scaled = quadplane.get_pilot_throttle();
      63       15235 :     quadplane.hold_stabilize(pilot_throttle_scaled);
      64             : 
      65             :     // Stabilize with fixed wing surfaces
      66       15235 :     plane.stabilize_roll();
      67       15235 :     plane.stabilize_pitch();
      68             : 
      69             :     // Center rudder
      70       15235 :     output_rudder_and_steering(0.0);
      71             : }
      72             : 
      73             : // set the desired roll and pitch for a tailsitter
      74       47327 : void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const float pitch_input)
      75             : {
      76             :     // separate limit for roll, if set
      77       47327 :     if (plane.quadplane.tailsitter.max_roll_angle > 0) {
      78             :         // roll param is in degrees not centidegrees
      79           0 :         plane.nav_roll_cd = plane.quadplane.tailsitter.max_roll_angle * 100.0f * roll_input;
      80             :     } else {
      81       47327 :         plane.nav_roll_cd = roll_input * plane.quadplane.attitude_control->lean_angle_max_cd();
      82             :     }
      83             : 
      84             :     // angle max for tailsitter pitch
      85       47327 :     plane.nav_pitch_cd = pitch_input * plane.quadplane.attitude_control->lean_angle_max_cd();
      86       47327 :     plane.quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd);
      87       47327 : }
      88             : 
      89             : // set the desired roll and pitch for normal quadplanes, also limited by forward flight limits
      90     1569398 : void ModeQStabilize::set_limited_roll_pitch(const float roll_input, const float pitch_input)
      91             : {
      92     1569398 :     const int16_t angle_max_cd = plane.quadplane.attitude_control->lean_angle_max_cd();
      93     1569398 :     plane.nav_roll_cd = roll_input * MIN(plane.roll_limit_cd, angle_max_cd);
      94             :     // pitch is further constrained by PTCH_LIM_MIN/MAX which may impose
      95             :     // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX
      96     1569398 :     if (pitch_input > 0) {
      97        1080 :         plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max*100, angle_max_cd);
      98             :     } else {
      99     1568318 :         plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min*100, angle_max_cd);
     100             :     }
     101     1569398 : }
     102             : 
     103             : #endif