Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : /*
       4             :  * Init and run calls for stabilize flight mode
       5             :  */
       6             : 
       7             : // stabilize_run - runs the main stabilize controller
       8             : // should be called at 100hz or more
       9     6542412 : void ModeStabilize::run()
      10             : {
      11             :     // convert pilot input to lean angles
      12             :     float target_roll_rad, target_pitch_rad;
      13     6542412 :     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->lean_angle_max_rad());
      14             : 
      15             :     // get pilot's desired yaw rate
      16     6542412 :     float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
      17             : 
      18             :     // Determine desired spool state based on pilot throttle input.
      19             :     // The setter enforces that disarmed aircraft are held at SHUT_DOWN until armed.
      20     6542412 :     if (copter.ap.throttle_zero) {
      21     5925156 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      22             :     } else {
      23      617256 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      24             :     }
      25             : 
      26     6542412 :     float pilot_desired_throttle = get_pilot_desired_throttle();
      27             : 
      28     6542412 :     switch (motors->get_spool_state()) {
      29     6306171 :     case AP_Motors::SpoolState::SHUT_DOWN:
      30             :         // Motors Stopped
      31     6306171 :         attitude_control->reset_yaw_target_and_rate();
      32     6306171 :         attitude_control->reset_rate_controller_I_terms();
      33     6306171 :         pilot_desired_throttle = 0.0f;
      34     6306171 :         break;
      35             : 
      36       47005 :     case AP_Motors::SpoolState::GROUND_IDLE:
      37             :         // Landed
      38       47005 :         attitude_control->reset_yaw_target_and_rate();
      39       47005 :         attitude_control->reset_rate_controller_I_terms_smoothly();
      40       47005 :         pilot_desired_throttle = 0.0f;
      41       47005 :         break;
      42             : 
      43      182173 :     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
      44             :         // clear landing flag above zero throttle
      45      182173 :         if (!motors->limit.throttle_lower) {
      46      168863 :             set_land_complete(false);
      47             :         }
      48      182173 :         break;
      49             : 
      50        7063 :     case AP_Motors::SpoolState::SPOOLING_UP:
      51             :     case AP_Motors::SpoolState::SPOOLING_DOWN:
      52             :         // do nothing
      53        7063 :         break;
      54             :     }
      55             : 
      56             :     // call attitude controller
      57     6542412 :     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(target_roll_rad, target_pitch_rad, target_yaw_rate_rads);
      58             : 
      59             :     // output pilot's throttle
      60     6542412 :     attitude_control->set_throttle_out(pilot_desired_throttle, true, g.throttle_filt);
      61     6542412 : }