Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4          39 : bool ModeLoiter::_enter()
       5             : {
       6          39 :     plane.do_loiter_at_location();
       7          39 :     plane.setup_terrain_target_alt(plane.next_WP_loc);
       8             : 
       9             :     // make sure the local target altitude is the same as the nav target used for loiter nav
      10             :     // this allows us to do FBWB style stick control
      11             :     /*IGNORE_RETURN(plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, plane.target_altitude.amsl_cm));*/
      12          39 :     if (plane.stick_mixing_enabled() && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) {
      13           0 :         plane.set_target_altitude_current();
      14             :     }
      15             : 
      16          39 :     plane.loiter_angle_reset();
      17             : 
      18          39 :     return true;
      19             : }
      20             : 
      21      198807 : void ModeLoiter::update()
      22             : {
      23      198807 :     plane.calc_nav_roll();
      24      198807 :     if (plane.stick_mixing_enabled() && plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
      25        8407 :         plane.update_fbwb_speed_height();
      26             :     } else {
      27      190400 :         plane.calc_nav_pitch();
      28      190400 :         plane.calc_throttle();
      29             :     }
      30             : 
      31             : #if AP_SCRIPTING_ENABLED
      32      198807 :     if (plane.nav_scripting_active()) {
      33             :         // while a trick is running we reset altitude
      34           0 :         plane.set_target_altitude_current();
      35           0 :         plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE);
      36             :     }
      37             : #endif
      38      198807 : }
      39             : 
      40        2880 : bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc)
      41             : {
      42             :     // Return true if current heading is aligned to vector to targetLoc.
      43             :     // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
      44             : 
      45             :     // Corrected radius for altitude
      46        2880 :     const float loiter_radius = plane.nav_controller->loiter_radius(fabsf(plane.loiter.radius));
      47        2880 :     if (!is_positive(loiter_radius)) {
      48             :         // Zero is invalid, protect against divide by zero for destination inside loiter radius case
      49           0 :         return true;
      50             :     }
      51             : 
      52             :     // Calculate relative position of the vehicle relative to loiter center projected onto the closest point of the loiter circle
      53             :     // This removes error due to radial position as the nav controller attempts to track the circle
      54        2880 :     const Vector2f projected_pos = loiterCenterLoc.get_distance_NE(plane.current_loc).normalized() * loiter_radius;
      55             : 
      56             :     // Target position relative to loiter center
      57        2880 :     const Vector2f target_pos = loiterCenterLoc.get_distance_NE(targetLoc);
      58             : 
      59             :     // Distance between loiter circle and target
      60        2880 :     const float target_dist = target_pos.length();
      61        2880 :     if (!is_positive(target_dist)) {
      62             :         // Target is coincident with loiter center, no heading will be closer than any other
      63           0 :         return true;
      64             :     }
      65             : 
      66             :     // Target bearing in centi-degrees
      67             :     int32_t target_bearing_cd;
      68             : 
      69        2880 :     if (target_dist >= loiter_radius) {
      70             :         // Destination outside loiter radius, heading will always line up with destination
      71             : 
      72             :         // Vector from between projected vehicle position and target position
      73        2652 :         const Vector2f pos_to_target = target_pos - projected_pos;
      74        2652 :         target_bearing_cd = degrees(pos_to_target.angle()) * 100;
      75             : 
      76             :     } else {
      77             :         // Destination is inside loiter, heading will never line up with destination
      78             : 
      79             :         // Advance turn point by the angle of a segment with chord "a"
      80             :         // This results in turning earlier as the target point approaches the center
      81             :         // If target is on radius angle of 0 and angle of 60 deg if target is on center 
      82         228 :         const float a = loiter_radius - target_dist;
      83         228 :         const float segment_angle = 2.0 * asinf(a / (2.0 * loiter_radius));
      84             : 
      85             :         // Pick the intersection point that will be hit first for the current loiter direction, add 90 deg to get the tangent angle
      86         228 :         target_bearing_cd = degrees(wrap_PI(target_pos.angle() + (M_PI_2 - segment_angle) * plane.loiter.direction)) * 100;
      87             : 
      88             :     }
      89             : 
      90             :     // Ideal heading in centi-degrees, +- 90 to get tangent to loiter circle at closest point
      91        2880 :     const int32_t current_heading_cd = degrees(wrap_PI(projected_pos.angle() + M_PI_2 * plane.loiter.direction)) * 100;
      92             : 
      93        2880 :     return isHeadingLinedUp_cd(target_bearing_cd, current_heading_cd);
      94             : }
      95             : 
      96             : 
      97           0 : bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) {
      98             : 
      99             :     // get current heading.
     100           0 :     const int32_t heading_cd = (wrap_360(degrees(ahrs.groundspeed_vector().angle())))*100;
     101             : 
     102           0 :     return isHeadingLinedUp_cd(bearing_cd, heading_cd);
     103             : }
     104             : 
     105             : 
     106        2880 : bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t heading_cd)
     107             : {
     108             :     // Return true if current heading is aligned to bearing_cd.
     109             :     // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
     110        2880 :     const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
     111             : 
     112             :     /*
     113             :       Check to see if the the plane is heading toward the land
     114             :       waypoint. We use 20 degrees (+/-10 deg) of margin so that
     115             :       we can handle 200 degrees/second of yaw.
     116             : 
     117             :       After every full circle, extend acceptance criteria to ensure
     118             :       aircraft will not loop forever in case high winds are forcing
     119             :       it beyond 200 deg/sec when passing the desired exit course
     120             :     */
     121             : 
     122             :     // Use integer division to get discrete steps
     123        2880 :     const int32_t expanded_acceptance = 1000 * (labs(plane.loiter.sum_cd) / 36000);
     124             : 
     125        2880 :     if (labs(heading_err_cd) <= 1000 + expanded_acceptance) {
     126             :         // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
     127             : 
     128             :         // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
     129          18 :         if (plane.next_WP_loc.loiter_xtrack) {
     130           2 :             plane.next_WP_loc = plane.current_loc;
     131             :         }
     132          18 :         return true;
     133             :     }
     134        2862 :     return false;
     135             : }
     136             : 
     137       21434 : void ModeLoiter::navigate()
     138             : {
     139       21434 :     if (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
     140             :         // update the WP alt from the global target adjusted by update_fbwb_speed_height
     141        1682 :         plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE);
     142             :     }
     143             : 
     144             : #if AP_SCRIPTING_ENABLED
     145       21434 :     if (plane.nav_scripting_active()) {
     146             :         // don't try to navigate while running trick
     147           0 :         return;
     148             :     }
     149             : #endif
     150             : 
     151             :     // Zero indicates to use WP_LOITER_RAD
     152       21434 :     plane.update_loiter(0);
     153             : }
     154             : 
     155       21432 : void ModeLoiter::update_target_altitude()
     156             : {
     157       21432 :     if (plane.stick_mixing_enabled() && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) {
     158        1682 :         return;
     159             :     }
     160       19750 :     Mode::update_target_altitude();
     161             : }