Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if FRAME_CONFIG == HELI_FRAME
       4             : /*
       5             :  * Init and run calls for stabilize flight mode for trad heli
       6             :  */
       7             : 
       8             : // stabilize_init - initialise stabilize controller
       9          15 : bool ModeStabilize_Heli::init(bool ignore_checks)
      10             : {
      11             :     // be aware that when adding code to this function that it is *NOT
      12             :     // RUN* at vehicle startup!
      13             : 
      14             :     // set stab collective true to use stabilize scaled collective pitch range
      15          15 :     copter.input_manager.set_use_stab_col(true);
      16             : 
      17          15 :     return true;
      18             : }
      19             : 
      20             : // stabilize_run - runs the main stabilize controller
      21             : // should be called at 100hz or more
      22     1057936 : void ModeStabilize_Heli::run()
      23             : {
      24             :     float target_roll_rad, target_pitch_rad;
      25             :     float pilot_throttle_scaled;
      26             : 
      27             :     // convert pilot input to lean angles
      28     1057936 :     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->lean_angle_max_rad());
      29             : 
      30             :     // get pilot's desired yaw rate
      31     1057936 :     float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
      32             : 
      33             :     // get pilot's desired throttle
      34     1057936 :     pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
      35             : 
      36             :     // Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because
      37             :     // we may be in autorotation flight.  This is so that the servos move in a realistic fashion while disarmed
      38             :     // for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero
      39             :     // so the swash servos move.
      40             : 
      41             :     // Request throttle unlimited. The setter enforces safety constraints:
      42             :     // - Disarmed: held at SHUT_DOWN until armed
      43             :     // - Armed without interlock: limited to GROUND_IDLE (swash can move, rotor stopped)
      44             :     // - Armed with interlock: THROTTLE_UNLIMITED granted
      45     1057936 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      46             : 
      47     1057936 :     switch (motors->get_spool_state()) {
      48      734293 :     case AP_Motors::SpoolState::SHUT_DOWN:
      49             :         // Motors Stopped
      50      734293 :         attitude_control->reset_yaw_target_and_rate(false);
      51      734293 :         attitude_control->reset_rate_controller_I_terms();
      52      734293 :         break;
      53       67005 :     case AP_Motors::SpoolState::GROUND_IDLE:
      54             :         // If aircraft is landed, set target heading to current and reset the integrator
      55             :         // Otherwise motors could be at ground idle for practice autorotation
      56       67005 :         if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
      57       47133 :             attitude_control->reset_yaw_target_and_rate(false);
      58       47133 :             attitude_control->reset_rate_controller_I_terms_smoothly();
      59             :         }
      60       67005 :         break;
      61      198128 :     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
      62      198128 :         if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
      63           0 :             attitude_control->reset_rate_controller_I_terms_smoothly();
      64             :         }
      65      198128 :         break;
      66       58510 :     case AP_Motors::SpoolState::SPOOLING_UP:
      67             :     case AP_Motors::SpoolState::SPOOLING_DOWN:
      68             :         // do nothing
      69       58510 :         break;
      70             :     }
      71             : 
      72             :     // call attitude controller
      73     1057936 :     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(target_roll_rad, target_pitch_rad, target_yaw_rate_rads);
      74             : 
      75             :     // output pilot's throttle
      76     1057936 :     attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
      77     1057936 : }
      78             : 
      79             : #endif  //HELI_FRAME