Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_ALTHOLD_ENABLED
       4             : 
       5             : /*
       6             :  * Init and run calls for althold, flight mode
       7             :  */
       8             : 
       9             : // althold_init - initialise althold controller
      10         164 : bool ModeAltHold::init(bool ignore_checks)
      11             : {
      12             : 
      13             :     // initialise the vertical position controller
      14         164 :     if (!pos_control->D_is_active()) {
      15         100 :         pos_control->D_init_controller();
      16             :     }
      17             : 
      18             :     // set vertical speed and acceleration limits
      19         164 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      20         164 :     pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      21             : 
      22         164 :     return true;
      23             : }
      24             : 
      25             : // althold_run - runs the althold controller
      26             : // should be called at 100hz or more
      27     2430229 : void ModeAltHold::run()
      28             : {
      29             :     // set vertical speed and acceleration limits
      30     2430229 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      31             : 
      32             :     // get pilot desired lean angles
      33             :     float target_roll_rad, target_pitch_rad;
      34     2430229 :     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
      35             : 
      36             :     // get pilot's desired yaw rate
      37     2430229 :     float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
      38             : 
      39             :     // get pilot desired climb rate
      40     2430229 :     float target_climb_rate_ms = get_pilot_desired_climb_rate_ms();
      41             : 
      42             :     // Alt Hold State Machine Determination
      43     2430229 :     AltHoldModeState althold_state = get_alt_hold_state_D_ms(target_climb_rate_ms);
      44             : 
      45             :     // Alt Hold State Machine
      46     2430229 :     switch (althold_state) {
      47             : 
      48      318615 :     case AltHoldModeState::MotorStopped:
      49      318615 :         attitude_control->reset_rate_controller_I_terms();
      50      318615 :         attitude_control->reset_yaw_target_and_rate(false);
      51      318615 :         pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
      52      318615 :         break;
      53             : 
      54       25136 :     case AltHoldModeState::Landed_Ground_Idle:
      55       25136 :         attitude_control->reset_yaw_target_and_rate();
      56             :         FALLTHROUGH;
      57             : 
      58      176585 :     case AltHoldModeState::Landed_Pre_Takeoff:
      59      176585 :         attitude_control->reset_rate_controller_I_terms_smoothly();
      60      176585 :         pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
      61      176585 :         break;
      62             : 
      63       25335 :     case AltHoldModeState::Takeoff:
      64             :         // initiate take-off
      65       25335 :         if (!takeoff.running()) {
      66          62 :             takeoff.start_m(constrain_float(g2.pilot_takeoff_alt_m, 0.0, 10.0));
      67             :         }
      68             : 
      69             :         // get avoidance adjusted climb rate
      70       25335 :         target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
      71             : 
      72             :         // set position controller targets adjusted for pilot input
      73       25335 :         takeoff.do_pilot_takeoff_ms(target_climb_rate_ms);
      74       25335 :         break;
      75             : 
      76     1909694 :     case AltHoldModeState::Flying:
      77             : 
      78             : #if AP_AVOIDANCE_ALTHOLD_ENABLED
      79             :         // apply avoidance
      80             :         copter.avoid.adjust_roll_pitch_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad());
      81             : #endif
      82             : 
      83             :         // get avoidance adjusted climb rate
      84     1909694 :         target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
      85             : 
      86             : #if AP_RANGEFINDER_ENABLED
      87             :         // update the vertical offset based on the surface measurement
      88     1909694 :         copter.surface_tracking.update_surface_offset();
      89             : #endif
      90             : 
      91             :         // Send the commanded climb rate to the position controller
      92     1909694 :         pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
      93     1909694 :         break;
      94             :     }
      95             : 
      96             :     // call attitude controller
      97     2430229 :     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(target_roll_rad, target_pitch_rad, target_yaw_rate_rads);
      98             : 
      99             :     // run the vertical position controller and set output throttle
     100     2430229 :     pos_control->D_update_controller();
     101     2430229 : }
     102             : 
     103             : #endif // MODE_ALTHOLD_ENABLED