Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_SPORT_ENABLED
       4             : 
       5             : /*
       6             :  * Init and run calls for sport flight mode
       7             :  */
       8             : 
       9             : // sport_init - initialise sport controller
      10           0 : bool ModeSport::init(bool ignore_checks)
      11             : {
      12             :     // set vertical speed and acceleration limits
      13           0 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      14           0 :     pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      15             : 
      16             :     // initialise the vertical position controller
      17           0 :     if (!pos_control->D_is_active()) {
      18           0 :         pos_control->D_init_controller();
      19             :     }
      20             : 
      21           0 :     return true;
      22             : }
      23             : 
      24             : // sport_run - runs the sport controller
      25             : // should be called at 100hz or more
      26           0 : void ModeSport::run()
      27             : {
      28             :     // set vertical speed and acceleration limits
      29           0 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      30             : 
      31             :     // apply SIMPLE mode transform
      32           0 :     float roll_in_norm = channel_roll->norm_input_dz();
      33           0 :     float pitch_in_norm = channel_pitch->norm_input_dz();
      34           0 :     apply_simple_mode(roll_in_norm, pitch_in_norm);
      35             : 
      36             :     // get pilot's desired roll and pitch rates
      37           0 :     float target_roll_rads = roll_in_norm * radians(g2.command_model_acro_rp.get_rate());
      38           0 :     float target_pitch_rads = pitch_in_norm * radians(g2.command_model_acro_rp.get_rate());
      39             : 
      40             :     // get pilot's desired yaw rate
      41           0 :     float target_yaw_rads = get_pilot_desired_yaw_rate_rads();
      42             : 
      43             :     // get attitude targets
      44           0 :     const Vector3f att_target_euler_rad = attitude_control->get_att_target_euler_rad();
      45             : 
      46             :     // Calculate trainer mode earth frame rate command for roll
      47           0 :     float roll_angle_rad = wrap_PI(att_target_euler_rad.x);
      48           0 :     target_roll_rads -= constrain_float(roll_angle_rad, -ACRO_LEVEL_MAX_ANGLE_RAD, ACRO_LEVEL_MAX_ANGLE_RAD) * g.acro_balance_roll;
      49             : 
      50             :     // Calculate trainer mode earth frame rate command for pitch
      51           0 :     float pitch_angle_rad = wrap_PI(att_target_euler_rad.y);
      52           0 :     target_pitch_rads -= constrain_float(pitch_angle_rad, -ACRO_LEVEL_MAX_ANGLE_RAD, ACRO_LEVEL_MAX_ANGLE_RAD) * g.acro_balance_pitch;
      53             : 
      54           0 :     const float angle_max_rad = attitude_control->lean_angle_max_rad();
      55             : 
      56           0 :     if (roll_angle_rad > angle_max_rad){
      57           0 :         target_roll_rads +=  sqrt_controller(angle_max_rad - roll_angle_rad, radians(g2.command_model_acro_rp.get_rate()) / ACRO_LEVEL_MAX_OVERSHOOT_RAD, attitude_control->get_accel_roll_max_radss(), G_Dt);
      58           0 :     }else if (roll_angle_rad < -angle_max_rad) {
      59           0 :         target_roll_rads +=  sqrt_controller(-angle_max_rad - roll_angle_rad, radians(g2.command_model_acro_rp.get_rate()) / ACRO_LEVEL_MAX_OVERSHOOT_RAD, attitude_control->get_accel_roll_max_radss(), G_Dt);
      60             :     }
      61             : 
      62           0 :     if (pitch_angle_rad > angle_max_rad){
      63           0 :         target_pitch_rads +=  sqrt_controller(angle_max_rad - pitch_angle_rad, radians(g2.command_model_acro_rp.get_rate()) / ACRO_LEVEL_MAX_OVERSHOOT_RAD, attitude_control->get_accel_pitch_max_radss(), G_Dt);
      64           0 :     }else if (pitch_angle_rad < -angle_max_rad) {
      65           0 :         target_pitch_rads +=  sqrt_controller(-angle_max_rad - pitch_angle_rad, radians(g2.command_model_acro_rp.get_rate()) / ACRO_LEVEL_MAX_OVERSHOOT_RAD, attitude_control->get_accel_pitch_max_radss(), G_Dt);
      66             :     }
      67             : 
      68             :     // get pilot desired climb rate
      69           0 :     float target_climb_rate_ms = get_pilot_desired_climb_rate_ms();
      70             : 
      71             :     // Sport State Machine Determination
      72           0 :     AltHoldModeState sport_state = get_alt_hold_state_D_ms(target_climb_rate_ms);
      73             : 
      74             :     // State Machine
      75           0 :     switch (sport_state) {
      76             : 
      77           0 :     case AltHoldModeState::MotorStopped:
      78           0 :         attitude_control->reset_rate_controller_I_terms();
      79           0 :         attitude_control->reset_yaw_target_and_rate(false);
      80           0 :         pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
      81           0 :         break;
      82             : 
      83           0 :     case AltHoldModeState::Landed_Ground_Idle:
      84           0 :         attitude_control->reset_yaw_target_and_rate();
      85             :         FALLTHROUGH;
      86             : 
      87           0 :     case AltHoldModeState::Landed_Pre_Takeoff:
      88           0 :         attitude_control->reset_rate_controller_I_terms_smoothly();
      89           0 :         pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
      90           0 :         break;
      91             : 
      92           0 :     case AltHoldModeState::Takeoff:
      93             :         // initiate take-off
      94           0 :         if (!takeoff.running()) {
      95           0 :             takeoff.start_m(constrain_float(g2.pilot_takeoff_alt_m, 0.0, 10.0));
      96             :         }
      97             : 
      98             :         // get avoidance adjusted climb rate
      99           0 :         target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
     100             : 
     101             :         // set position controller targets adjusted for pilot input
     102           0 :         takeoff.do_pilot_takeoff_ms(target_climb_rate_ms);
     103           0 :         break;
     104             : 
     105           0 :     case AltHoldModeState::Flying:
     106             :         // get avoidance adjusted climb rate
     107           0 :         target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
     108             : 
     109             : #if AP_RANGEFINDER_ENABLED
     110             :         // update the vertical offset based on the surface measurement
     111           0 :         copter.surface_tracking.update_surface_offset();
     112             : #endif
     113             : 
     114             :         // Send the commanded climb rate to the position controller
     115           0 :         pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
     116           0 :         break;
     117             :     }
     118             : 
     119             :     // call attitude controller
     120           0 :     attitude_control->input_euler_rate_roll_pitch_yaw_rads(target_roll_rads, target_pitch_rads, target_yaw_rads);
     121             : 
     122             :     // run the vertical position controller and set output throttle
     123           0 :     pos_control->D_update_controller();
     124           0 : }
     125             : 
     126             : #endif