Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_LOITER_ENABLED
       4             : 
       5             : /*
       6             :  * Init and run calls for loiter flight mode
       7             :  */
       8             : 
       9             : // loiter_init - initialise loiter controller
      10         194 : bool ModeLoiter::init(bool ignore_checks)
      11             : {
      12             :     float target_roll_rad, target_pitch_rad;
      13             :     // convert pilot input to lean angles
      14         194 :     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, loiter_nav->get_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
      15             : 
      16             :     // process pilot's roll and pitch input
      17         194 :     loiter_nav->set_pilot_desired_acceleration_rad(target_roll_rad, target_pitch_rad);
      18             : 
      19         194 :     loiter_nav->init_target();
      20             : 
      21             :     // initialise the vertical position controller
      22         194 :     if (!pos_control->D_is_active()) {
      23         143 :         pos_control->D_init_controller();
      24             :     }
      25             : 
      26             :     // set vertical speed and acceleration limits
      27         194 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      28         194 :     pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      29             : 
      30             : #if AC_PRECLAND_ENABLED
      31         194 :     _precision_loiter_active = false;
      32             : #endif
      33             : 
      34         194 :     return true;
      35             : }
      36             : 
      37             : #if AC_PRECLAND_ENABLED
      38     2213396 : bool ModeLoiter::do_precision_loiter()
      39             : {
      40     2213396 :     if (!_precision_loiter_enabled) {
      41     2177267 :         return false;
      42             :     }
      43       36129 :     if (copter.ap.land_complete_maybe) {
      44           0 :         return false;        // don't move on the ground
      45             :     }
      46             :     // if the pilot *really* wants to move the vehicle, let them....
      47       36129 :     if (loiter_nav->get_pilot_desired_acceleration_NE_mss().length() > 0.5) {
      48           0 :         return false;
      49             :     }
      50       36129 :     if (!copter.precland.target_acquired()) {
      51       10740 :         return false; // we don't have a good vector
      52             :     }
      53       25389 :     return true;
      54             : }
      55             : 
      56       25389 : void ModeLoiter::precision_loiter_xy()
      57             : {
      58       25389 :     loiter_nav->clear_pilot_desired_acceleration();
      59       25389 :     Vector2p target_pos_ne_m;
      60       25389 :     Vector2f target_vel_ne_ms;
      61       25389 :     if (!copter.precland.get_target_position_m(target_pos_ne_m)) {
      62           0 :         target_pos_ne_m = pos_control->get_pos_estimate_NED_m().xy();
      63             :     }
      64             :     // get the velocity of the target
      65       25389 :     copter.precland.get_target_velocity_ms(pos_control->get_vel_estimate_NED_ms().xy(), target_vel_ne_ms);
      66             : 
      67       25389 :     Vector2f zero;
      68             :     // target vel will remain zero if landing target is stationary
      69       25389 :     pos_control->input_pos_vel_accel_NE_m(target_pos_ne_m, target_vel_ne_ms, zero);
      70             :     // run pos controller
      71       25389 :     pos_control->NE_update_controller();
      72       25389 : }
      73             : #endif
      74             : 
      75             : // loiter_run - runs the loiter controller
      76             : // should be called at 100hz or more
      77     3459501 : void ModeLoiter::run()
      78             : {
      79             :     float target_roll_rad, target_pitch_rad;
      80     3459501 :     float target_yaw_rate_rads = 0.0f;
      81     3459501 :     float target_climb_rate_ms = 0.0f;
      82             : 
      83             :     // set vertical speed and acceleration limits
      84     3459501 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
      85             : 
      86             :     // convert pilot input to lean angles
      87     3459501 :     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, loiter_nav->get_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
      88             : 
      89             :     // process pilot's roll and pitch input
      90     3459501 :     loiter_nav->set_pilot_desired_acceleration_rad(target_roll_rad, target_pitch_rad);
      91             : 
      92             :     // get pilot's desired yaw rate
      93     3459501 :     target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
      94             : 
      95             :     // get pilot desired climb rate
      96     3459501 :     target_climb_rate_ms = get_pilot_desired_climb_rate_ms();
      97             : 
      98             :     // relax loiter target if we might be landed
      99     3459501 :     if (copter.ap.land_complete_maybe) {
     100     1246616 :         loiter_nav->soften_for_landing();
     101             :     }
     102             : 
     103             :     // Loiter State Machine Determination
     104     3459501 :     AltHoldModeState loiter_state = get_alt_hold_state_D_ms(target_climb_rate_ms);
     105             : 
     106             :     // Loiter State Machine
     107     3459501 :     switch (loiter_state) {
     108             : 
     109     1058592 :     case AltHoldModeState::MotorStopped:
     110     1058592 :         attitude_control->reset_rate_controller_I_terms();
     111     1058592 :         attitude_control->reset_yaw_target_and_rate();
     112     1058592 :         pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
     113     1058592 :         loiter_nav->init_target();
     114     1058592 :         break;
     115             : 
     116       40371 :     case AltHoldModeState::Landed_Ground_Idle:
     117       40371 :         attitude_control->reset_yaw_target_and_rate();
     118             :         FALLTHROUGH;
     119             : 
     120      149634 :     case AltHoldModeState::Landed_Pre_Takeoff:
     121      149634 :         attitude_control->reset_rate_controller_I_terms_smoothly();
     122      149634 :         loiter_nav->init_target();
     123      149634 :         pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
     124      149634 :         break;
     125             : 
     126       37879 :     case AltHoldModeState::Takeoff:
     127             :         // initiate take-off
     128       37879 :         if (!takeoff.running()) {
     129          95 :             takeoff.start_m(constrain_float(g2.pilot_takeoff_alt_m, 0.0, 10.0));
     130             :         }
     131             : 
     132             :         // get avoidance adjusted climb rate
     133       37879 :         target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
     134             : 
     135             :         // set position controller targets adjusted for pilot input
     136       37879 :         takeoff.do_pilot_takeoff_ms(target_climb_rate_ms);
     137             : 
     138             :         // run loiter controller
     139       37879 :         loiter_nav->update();
     140       37879 :         break;
     141             : 
     142     2213396 :     case AltHoldModeState::Flying:
     143             : #if AC_PRECLAND_ENABLED
     144     2213396 :         bool precision_loiter_old_state = _precision_loiter_active;
     145     2213396 :         if (do_precision_loiter()) {
     146       25389 :             precision_loiter_xy();
     147       25389 :             _precision_loiter_active = true;
     148             :         } else {
     149     2188007 :             _precision_loiter_active = false;
     150             :         }
     151     2213396 :         if (precision_loiter_old_state && !_precision_loiter_active) {
     152             :             // prec loiter was active, not any more, let's init again as user takes control
     153           1 :             loiter_nav->init_target();
     154             :         }
     155             :         // run loiter controller if we are not doing prec loiter
     156     2213396 :         if (!_precision_loiter_active) {
     157     2188007 :             loiter_nav->update();
     158             :         }
     159             : #else
     160             :         loiter_nav->update();
     161             : #endif
     162             : 
     163             : 
     164             :         // get avoidance adjusted climb rate
     165     2213396 :         target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
     166             : 
     167             : #if AP_RANGEFINDER_ENABLED
     168             :         // update the vertical offset based on the surface measurement
     169     2213396 :         copter.surface_tracking.update_surface_offset();
     170             : #endif
     171             : 
     172             :         // Send the commanded climb rate to the position controller
     173     2213396 :         pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
     174     2213396 :         break;
     175             :     }
     176             : 
     177             :     // call attitude controller
     178     3459501 :     attitude_control->input_thrust_vector_rate_heading_rads(loiter_nav->get_thrust_vector(), target_yaw_rate_rads, false);
     179             :     // run the vertical position controller and set output throttle
     180     3459501 :     pos_control->D_update_controller();
     181     3459501 : }
     182             : 
     183       51926 : float ModeLoiter::wp_distance_m() const
     184             : {
     185       51926 :     return loiter_nav->get_distance_to_target_m();
     186             : }
     187             : 
     188       51926 : float ModeLoiter::wp_bearing_deg() const
     189             : {
     190       51926 :     return degrees(loiter_nav->get_bearing_to_target_rad());
     191             : }
     192             : 
     193             : #endif