Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : /*
       4             :   reset the total loiter angle
       5             :  */
       6        4118 : void Plane::loiter_angle_reset(void)
       7             : {
       8        4118 :     loiter.sum_cd = 0;
       9        4118 :     loiter.total_cd = 0;
      10        4118 :     loiter.reached_target_alt = false;
      11        4118 :     loiter.unable_to_achieve_target_alt = false;
      12        4118 : }
      13             : 
      14             : /*
      15             :   update the total angle we have covered in a loiter. Used to support
      16             :   commands to do N circles of loiter
      17             :  */
      18      511367 : void Plane::loiter_angle_update(void)
      19             : {
      20             :     static const int32_t lap_check_interval_cd = 3*36000;
      21             : 
      22      511367 :     const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
      23             :     int32_t loiter_delta_cd;
      24      511367 :     const bool reached_target = reached_loiter_target();
      25             : 
      26      511367 :     if (loiter.sum_cd == 0 && !reached_target) {
      27             :         // we don't start summing until we are doing the real loiter
      28      373248 :         loiter_delta_cd = 0;
      29      138119 :     } else if (loiter.sum_cd == 0) {
      30             :         // use 1 cd for initial delta
      31       18219 :         loiter_delta_cd = 1;
      32       18219 :         loiter.start_lap_alt_cm = current_loc.alt;
      33       18219 :         loiter.next_sum_lap_cd = lap_check_interval_cd;
      34             :     } else {
      35      119900 :         loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
      36             :     }
      37             : 
      38      511367 :     loiter.old_target_bearing_cd = target_bearing_cd;
      39      511367 :     loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
      40      511367 :     loiter.sum_cd += loiter_delta_cd * loiter.direction;
      41             : 
      42      511367 :     bool reached_target_alt = false;
      43             : 
      44      511367 :     if (reached_target) {
      45             :         // once we reach the position target we start checking the
      46             :         // altitude target
      47      107708 :         bool terrain_status_ok = false;
      48             : #if AP_TERRAIN_AVAILABLE
      49             :         /*
      50             :           if doing terrain following then we check against terrain
      51             :           target, fetch the terrain information
      52             :         */
      53      107708 :         float altitude_agl = 0;
      54      107708 :         if (target_altitude.terrain_following) {
      55       24260 :             if (terrain.status() == AP_Terrain::TerrainStatusOK &&
      56       12130 :                 terrain.height_above_terrain(altitude_agl, true)) {
      57       12130 :                 terrain_status_ok = true;
      58             :             }
      59             :         }
      60      119838 :         if (terrain_status_ok &&
      61       12130 :             fabsF(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) {
      62        9503 :             reached_target_alt = true;
      63             :         } else
      64             : #endif
      65       98205 :         if (!terrain_status_ok && labs(current_loc.alt - target_altitude.amsl_cm) < 500) {
      66       48906 :             reached_target_alt = true;
      67             :         }
      68             :     }
      69             : 
      70      511367 :     if (reached_target_alt) {
      71       58409 :         loiter.reached_target_alt = true;
      72       58409 :         loiter.unable_to_achieve_target_alt = false;
      73       58409 :         loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;
      74             : 
      75      452958 :     } else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) {
      76             :         // check every few laps for scenario where up/downward inhibit you from loitering up/down for too long
      77         351 :         loiter.unable_to_achieve_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500;
      78         351 :         loiter.start_lap_alt_cm = current_loc.alt;
      79         351 :         loiter.next_sum_lap_cd += lap_check_interval_cd;
      80             :     }
      81      511367 : }
      82             : 
      83             : //****************************************************************
      84             : // Function that will calculate the desired direction to fly and distance
      85             : //****************************************************************
      86      547097 : void Plane::navigate()
      87             : {
      88             :     // do not navigate with corrupt data
      89             :     // ---------------------------------
      90      547097 :     if (!have_position) {
      91       31931 :         return;
      92             :     }
      93             : 
      94      515166 :     if (next_WP_loc.lat == 0 && next_WP_loc.lng == 0) {
      95        3799 :         return;
      96             :     }
      97             : 
      98      511367 :     check_home_alt_change();
      99             : 
     100             :     // waypoint distance from plane
     101             :     // ----------------------------
     102      511367 :     auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
     103      511367 :     auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc);
     104      511367 :     TECS_controller.set_path_proportion(auto_state.wp_proportion);
     105             : 
     106             :     // update total loiter angle
     107      511367 :     loiter_angle_update();
     108             : 
     109             :     // control mode specific updates to navigation demands
     110             :     // ---------------------------------------------------
     111      511367 :     control_mode->navigate();
     112             : }
     113             : 
     114             : // method intended for use in calc_airspeed_errors only
     115      205761 : float Plane::mode_auto_target_airspeed_cm()
     116             : {
     117             : #if HAL_QUADPLANE_ENABLED
     118      208357 :     if (quadplane.landing_with_fixed_wing_spiral_approach() &&
     119        2596 :         ((vtol_approach_s.approach_stage == VTOLApproach::Stage::APPROACH_LINE) ||
     120        2540 :          (vtol_approach_s.approach_stage == VTOLApproach::Stage::VTOL_LANDING))) {
     121        1624 :         const float land_airspeed = TECS_controller.get_land_airspeed();
     122        1624 :         if (is_positive(land_airspeed)) {
     123           0 :             return land_airspeed * 100;
     124             :         }
     125             :         // fallover to normal airspeed
     126        1624 :         return aparm.airspeed_cruise*100;
     127             :     }
     128      204137 :     if (quadplane.in_vtol_land_approach()) {
     129        5900 :         return quadplane.get_land_airspeed_ms() * 100;
     130             :     }
     131             : #endif
     132             : 
     133             :     // normal AUTO mode and new_airspeed variable was set by
     134             :     // DO_CHANGE_SPEED command while in AUTO mode
     135      198237 :     if (new_airspeed_cm > 0) {
     136       17023 :         return new_airspeed_cm;
     137             :     }
     138             : 
     139             :     // fallover to normal airspeed
     140      181214 :     return aparm.airspeed_cruise*100;
     141             : }
     142             : 
     143      547097 : void Plane::calc_airspeed_errors()
     144             : {
     145             :     // Get the airspeed_estimate, update smoothed airspeed estimate
     146             :     // NOTE:  we use the airspeed estimate function not direct sensor
     147             :     //        as TECS may be using synthetic airspeed
     148      547097 :     float airspeed_measured = 0.1;
     149      547097 :     if (ahrs.airspeed_EAS(airspeed_measured)) {
     150      542890 :         smoothed_airspeed = MAX(0.1, smoothed_airspeed * 0.8f + airspeed_measured * 0.2f);
     151             :     }
     152             : 
     153             :     // low pass filter speed scaler, with 1Hz cutoff, at 10Hz
     154      547097 :     const float speed_scaler = calc_speed_scaler();
     155      547097 :     const float cutoff_Hz = 2.0;
     156      547097 :     const float dt = 0.1;
     157      547097 :     surface_speed_scaler += calc_lowpass_alpha_dt(dt, cutoff_Hz) * (speed_scaler - surface_speed_scaler);
     158             : 
     159             : 
     160             :     // FBW_B/cruise airspeed target
     161      547097 :     if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) {
     162       13043 :         if (flight_option_enabled(FlightOptions::CRUISE_TRIM_AIRSPEED)) {
     163           0 :             target_airspeed_cm = aparm.airspeed_cruise*100;
     164       13043 :         } else if (flight_option_enabled(FlightOptions::CRUISE_TRIM_THROTTLE)) {
     165        1855 :             float control_min = 0.0f;
     166        1855 :             float control_mid = 0.0f;
     167        1855 :             const float control_max = channel_throttle->get_range();
     168        1855 :             const float control_in = get_throttle_input();
     169        1855 :             switch (channel_throttle->get_type()) {
     170           0 :             case RC_Channel::ControlType::ANGLE:
     171           0 :                     control_min = -control_max;
     172           0 :                     break;
     173        1855 :             case RC_Channel::ControlType::RANGE:
     174        1855 :                     control_mid = channel_throttle->get_control_mid();
     175        1855 :                     break;
     176             :             }
     177        1855 :             if (control_in <= control_mid) {
     178        1855 :                 target_airspeed_cm = linear_interpolate(aparm.airspeed_min * 100, aparm.airspeed_cruise*100,
     179             :                                                         control_in,
     180             :                                                         control_min, control_mid);
     181             :             } else {
     182           0 :                 target_airspeed_cm = linear_interpolate(aparm.airspeed_cruise*100, aparm.airspeed_max * 100,
     183             :                                                         control_in,
     184             :                                                         control_mid, control_max);
     185             :             }
     186             :         } else {
     187       11188 :             target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) *
     188       11188 :                                   get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
     189             :         }
     190             : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     191      547097 :     } else if (control_mode == &mode_guided && is_positive(guided_state.target_airspeed_cm)) { // if offboard guided speed change cmd not set, then this section is skipped
     192             :         // offboard airspeed demanded
     193         396 :         uint32_t now = AP_HAL::millis();
     194         396 :         float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms);
     195         396 :         guided_state.target_airspeed_time_ms = now;
     196         396 :         float delta_amt = 100 * delta * guided_state.target_airspeed_accel;
     197         396 :         target_airspeed_cm += delta_amt;
     198             : 
     199             :         //target_airspeed_cm recalculated then clamped to between MIN airspeed and MAX airspeed by constrain_float
     200         396 :         if (is_positive(guided_state.target_airspeed_accel)) {
     201         194 :             target_airspeed_cm = constrain_float(MIN(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100);
     202             :         } else {
     203         202 :             target_airspeed_cm = constrain_float(MAX(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100);
     204             :         }
     205             : 
     206             : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     207             : 
     208             : #if HAL_SOARING_ENABLED
     209      533658 :     } else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
     210        8182 :         if (control_mode == &mode_thermal) {
     211        2724 :             float arspd = g2.soaring_controller.get_thermalling_target_airspeed();
     212             : 
     213        2724 :             if (arspd > 0) {
     214           0 :                 target_airspeed_cm = arspd * 100;
     215             :             } else {
     216        2724 :                 target_airspeed_cm = aparm.airspeed_cruise*100;
     217             :             }
     218        5458 :         } else if (control_mode == &mode_auto) {
     219        5458 :             float arspd = g2.soaring_controller.get_cruising_target_airspeed();
     220             : 
     221        5458 :             if (arspd > 0) {
     222         623 :                 target_airspeed_cm = arspd * 100;
     223             :             } else {
     224        4835 :                 target_airspeed_cm = aparm.airspeed_cruise*100;
     225             :             }
     226             :         }
     227             : #endif
     228             : 
     229      525476 :     } else if (flight_stage == AP_FixedWing::FlightStage::LAND) {
     230             :         // Landing airspeed target
     231       36337 :         target_airspeed_cm = landing.get_target_airspeed_ms() * 100.0f;  // m/s -> cm/s
     232      489139 :     } else if (control_mode == &mode_guided && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert
     233         792 :                        target_airspeed_cm = new_airspeed_cm;
     234      488347 :     } else if (control_mode == &mode_auto) {
     235      205761 :         target_airspeed_cm = mode_auto_target_airspeed_cm();
     236             : #if HAL_QUADPLANE_ENABLED
     237      282586 :     } else if (control_mode == &mode_qrtl && quadplane.in_vtol_land_approach()) {
     238        6406 :         target_airspeed_cm = quadplane.get_land_airspeed_ms() * 100;
     239             : #endif
     240             :     } else {
     241             :         // Normal airspeed target for all other cases
     242      276180 :         target_airspeed_cm = aparm.airspeed_cruise*100;
     243             :     }
     244             : 
     245             :     // Set target to current airspeed + ground speed undershoot,
     246             :     // but only when this is faster than the target airspeed commanded
     247             :     // above.
     248      547097 :     if (control_mode->does_auto_throttle() &&
     249      547965 :         groundspeed_undershoot_is_valid &&
     250         868 :         control_mode != &mode_circle) {
     251             :         /*
     252             :           calculate how much extra airspeed we need to target to
     253             :           achieve the desired ground speed in MIN_GROUNDSPEED
     254             : 
     255             :           we quantise the additional airspeed and apply a hysteresis
     256             :           in order to avoid triggering an oscillation in TECS
     257             :          */
     258         868 :         float target_airspeed = target_airspeed_cm*0.01;
     259         868 :         float EAS_undershoot = (groundspeed_undershoot*0.01) / ahrs.get_EAS2TAS();
     260         868 :         float min_gnd_target_airspeed = airspeed_measured + EAS_undershoot;
     261         868 :         float airspeed_target_offset = min_gnd_target_airspeed > target_airspeed? (min_gnd_target_airspeed - target_airspeed) : 0;
     262             : 
     263             :         // round up to nearest m/s
     264         868 :         airspeed_target_offset = int(airspeed_target_offset + 0.5);
     265             : 
     266             :         // apply some hysteresis
     267         868 :         if (airspeed_target_offset < last_groundspeed_undershoot_offset &&
     268         420 :             last_groundspeed_undershoot_offset - airspeed_target_offset < 1.2) {
     269         389 :             airspeed_target_offset = last_groundspeed_undershoot_offset;
     270             :         }
     271         868 :         last_groundspeed_undershoot_offset = airspeed_target_offset;
     272             : 
     273         868 :         target_airspeed_cm += airspeed_target_offset * 100;
     274             :     }
     275             : 
     276             :     // when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely.
     277             : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     278      547097 :     if (control_mode == &mode_guided && is_positive(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
     279          84 :         airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero
     280             :     }
     281             : #endif
     282             : 
     283             :     // Bump up the target airspeed based on throttle nudging
     284      547097 :     if (control_mode->allows_throttle_nudging() && airspeed_nudge_cm > 0) {
     285       31297 :         target_airspeed_cm += airspeed_nudge_cm;
     286             :     }
     287             : 
     288      547097 :     float airspeed_lower_bound = is_positive(aparm.airspeed_stall)
     289     1094194 :                                      ? aparm.airspeed_stall
     290      547097 :                                      : aparm.airspeed_min;
     291             : 
     292             :     // Apply airspeed limit
     293      547097 :     target_airspeed_cm = constrain_int32(target_airspeed_cm, airspeed_lower_bound*100, aparm.airspeed_max*100);
     294             : 
     295             :     // use the TECS view of the target airspeed for reporting, to take
     296             :     // account of the landing speed
     297      547097 :     airspeed_error = TECS_controller.get_target_airspeed() - airspeed_measured;
     298      547097 : }
     299             : 
     300      547097 : void Plane::calc_gndspeed_undershoot()
     301             : {
     302             :     // Use the component of ground speed in the forward direction
     303             :     // This prevents flyaway if wind takes plane backwards
     304      547097 :     Vector3f velNED;
     305      547097 :     if (ahrs.have_inertial_nav() && ahrs.get_velocity_NED(velNED)) {
     306      471548 :         const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
     307      471548 :         Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
     308      471548 :         if (!yawVect.is_zero()) {
     309      471548 :             yawVect.normalize();
     310      471548 :             float gndSpdFwd = yawVect * velNED.xy();
     311      471548 :             groundspeed_undershoot_is_valid = aparm.min_groundspeed > 0;
     312      471548 :             groundspeed_undershoot = groundspeed_undershoot_is_valid ? (aparm.min_groundspeed*100 - gndSpdFwd*100) : 0;
     313             :         }
     314             :     } else {
     315       75549 :         groundspeed_undershoot_is_valid = false;
     316       75549 :         groundspeed_undershoot = 0;
     317             :     }
     318      547097 : }
     319             : 
     320             : // method intended to be used by update_loiter
     321      108655 : void Plane::update_loiter_update_nav(uint16_t radius)
     322             : {
     323             : #if HAL_QUADPLANE_ENABLED
     324      186822 :     if (loiter.start_time_ms != 0 &&
     325       78167 :         quadplane.guided_mode_enabled()) {
     326        8780 :         if (!auto_state.vtol_loiter) {
     327          28 :             auto_state.vtol_loiter = true;
     328             :             // reset loiter start time, so we don't consider the point
     329             :             // reached till we get much closer
     330          28 :             loiter.start_time_ms = 0;
     331          28 :             quadplane.guided_start();
     332             :         }
     333        8780 :         return;
     334             :     }
     335             : #endif
     336             : 
     337             : #if HAL_QUADPLANE_ENABLED
     338       99875 :     const bool quadplane_qrtl_switch = (control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL);
     339             : #else
     340             :     const bool quadplane_qrtl_switch = false;
     341             : #endif
     342             : 
     343      230238 :     if ((loiter.start_time_ms == 0 &&
     344       30488 :          (control_mode == &mode_auto || control_mode == &mode_guided) &&
     345       20078 :          auto_state.crosstrack &&
     346      130363 :          current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) ||
     347             :         quadplane_qrtl_switch) {
     348             :         /*
     349             :           if never reached loiter point and using crosstrack and somewhat far away from loiter point
     350             :           navigate to it like in auto-mode for normal crosstrack behavior
     351             : 
     352             :           we also use direct waypoint navigation if we are a quadplane
     353             :           that is going to be switching to QRTL when it gets within
     354             :           RTL_RADIUS
     355             :         */
     356        3093 :         nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
     357        3093 :         return;
     358             :     }
     359       96782 :     nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
     360             : }
     361             : 
     362      108655 : void Plane::update_loiter(uint16_t radius)
     363             : {
     364      108655 :     if (radius <= 1) {
     365             :         // if radius is <=1 then use the general loiter radius. if it's small, use default
     366       95162 :         radius = (abs(aparm.loiter_radius) <= 1) ? LOITER_RADIUS_DEFAULT : abs(aparm.loiter_radius);
     367       95162 :         if (next_WP_loc.loiter_ccw == 1) {
     368           0 :             loiter.direction = -1;
     369             :         } else {
     370       95162 :             loiter.direction = (aparm.loiter_radius < 0) ? -1 : 1;
     371             :         }
     372             :     }
     373             : 
     374             :     // the radius actually being used by the controller is required by other functions
     375      108655 :     loiter.radius = (float)radius;
     376             : 
     377      108655 :     update_loiter_update_nav(radius);
     378             : 
     379      108655 :     if (loiter.start_time_ms == 0) {
     380       60745 :         if (reached_loiter_target() ||
     381       30229 :             auto_state.wp_proportion > 1) {
     382             :             // we've reached the target, start the timer
     383         287 :             loiter.start_time_ms = millis();
     384         287 :             if (control_mode->is_guided_mode()) {
     385             :                 // starting a loiter in GUIDED means we just reached the target point
     386         128 :                 gcs().send_mission_item_reached_message(0);
     387             :             }
     388             : #if HAL_QUADPLANE_ENABLED
     389         287 :             if (quadplane.guided_mode_enabled()) {
     390          56 :                 quadplane.guided_start();
     391             :             }
     392             : #endif
     393             :         }
     394             :     }
     395      108655 : }
     396             : 
     397             : /*
     398             :   handle speed and height control in FBWB, CRUISE, and optionally, LOITER mode.
     399             :   In this mode the elevator is used to change target altitude. The
     400             :   throttle is used to change target airspeed or throttle
     401             :  */
     402      195631 : void Plane::update_fbwb_speed_height(void)
     403             : {
     404      195631 :     uint32_t now = micros();
     405      195631 :     if (now - target_altitude.last_elev_check_us >= 100000) {
     406             :         // we don't run this on every loop as it would give too small granularity on quadplanes at 300Hz, and
     407             :         // give below 1cm altitude change, which would result in no climb or descent
     408       13030 :         float dt = (now - target_altitude.last_elev_check_us) * 1.0e-6;
     409       13030 :         dt = constrain_float(dt, 0.1, 0.15);
     410             : 
     411       13030 :         target_altitude.last_elev_check_us = now;
     412             : 
     413       13030 :         float elevator_input = channel_pitch->get_control_in() * (1/4500.0);
     414             : 
     415       13030 :         if (g.flybywire_elev_reverse) {
     416           0 :             elevator_input = -elevator_input;
     417             :         }
     418             : 
     419       13030 :         bool input_stop_climb = !is_positive(elevator_input) && is_positive(target_altitude.last_elevator_input);
     420       13030 :         bool input_stop_descent = !is_negative(elevator_input) && is_negative(target_altitude.last_elevator_input);
     421       13030 :         if (input_stop_climb || input_stop_descent) {
     422             :             // user elevator input reached or passed zero, lock in the current altitude
     423          45 :             set_target_altitude_current();
     424             :         }
     425             : 
     426       13030 :         float climb_rate = g.flybywire_climb_rate * elevator_input;
     427       13030 :         climb_rate = constrain_float(climb_rate, -TECS_controller.get_max_sinkrate(), TECS_controller.get_max_climbrate());
     428             : 
     429       13030 :         int32_t alt_change_cm = climb_rate * dt * 100;
     430       13030 :         change_target_altitude(alt_change_cm);
     431             : 
     432             : #if HAL_SOARING_ENABLED
     433       13030 :         if (g2.soaring_controller.is_active()) {
     434        1472 :             if (g2.soaring_controller.get_throttle_suppressed()) {
     435             :                 // we're in soaring mode with throttle suppressed
     436        1151 :                 set_target_altitude_current();
     437             :             } else {
     438             :                 // we're in soaring mode climbing back to altitude. Set target to SOAR_ALT_CUTOFF plus 10m to ensure we positively climb
     439             :                 // through SOAR_ALT_CUTOFF, thus triggering throttle suppression and return to glide.
     440         321 :                 target_altitude.amsl_cm = 100*plane.g2.soaring_controller.get_alt_cutoff() + 1000 + AP::ahrs().get_home().alt;
     441             :             }
     442             :         }
     443             : #endif
     444             : 
     445       13030 :         target_altitude.last_elevator_input = elevator_input;
     446             :     }
     447             : 
     448      195631 :     check_fbwb_altitude();
     449             : 
     450      195631 :     calc_throttle();
     451      195631 :     calc_nav_pitch();
     452      195631 : }
     453             : 
     454             : /*
     455             :   calculate the turn angle for the next leg of the mission
     456             :  */
     457        4165 : void Plane::setup_turn_angle(void)
     458             : {
     459        4165 :     int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
     460        4165 :     if (next_ground_course_cd == -1) {
     461             :         // the mission library can't determine a turn angle, assume 90 degrees
     462         252 :         auto_state.next_turn_angle = 90.0f;
     463             :     } else {
     464             :         // get the heading of the current leg
     465        3913 :         int32_t ground_course_cd = prev_WP_loc.get_bearing_to(next_WP_loc);
     466             : 
     467             :         // work out the angle we need to turn through
     468        3913 :         auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f;
     469             :     }
     470        4165 : }
     471             : 
     472             : /*
     473             :   see if we have reached our loiter target
     474             :  */
     475     1097478 : bool Plane::reached_loiter_target(void)
     476             : {
     477             : #if HAL_QUADPLANE_ENABLED
     478     1097478 :     if (quadplane.in_vtol_auto()) {
     479       66459 :         return auto_state.wp_distance < 3;
     480             :     }
     481             : #endif
     482     1031019 :     return nav_controller->reached_loiter_target();
     483             : }