Plane: Soaring upgrades

ArduPlane/ArduPlane.cpp

@@ -577,13 +577,6 @@ void Plane::update_alt()
         if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
             distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
         }
-
-        bool soaring_active = false;
-#if SOARING_ENABLED == ENABLED
-        if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
-            soaring_active = true;
-        }
-#endif
 
         SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
                                                  target_airspeed_cm,
@@ -592,8 +585,7 @@ void Plane::update_alt()
                                                  get_takeoff_pitch_min_cd(),
                                                  throttle_nudge,
                                                  tecs_hgt_afe(),
-                                                 aerodynamic_load_factor,
-                                                 soaring_active);
+                                                 aerodynamic_load_factor);
     }
 }
 

ArduPlane/Plane.h

@@ -757,6 +757,9 @@ class Plane : public AP_Vehicle {
     // rudder mixing gain for differential thrust (0 - 1)
     float rudder_dt;
 
+    // soaring mode-change timer
+    uint32_t soaring_mode_timer;
+
     void adjust_nav_pitch_throttle(void);
     void update_load_factor(void);
     void send_fence_status(mavlink_channel_t chan);

ArduPlane/altitude.cpp

@@ -40,6 +40,12 @@ void Plane::adjust_altitude_target()
         landing.adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, auto_state.wp_distance, target_altitude.offset_cm);
     } else if (landing.get_target_altitude_location(target_location)) {
        set_target_altitude_location(target_location);
+#if SOARING_ENABLED == ENABLED
+    } else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
+       // Reset target alt to current alt, to prevent large altitude errors when gliding.
+       set_target_altitude_location(current_loc);
+       reset_offset_altitude();
+#endif
     } else if (reached_loiter_target()) {
         // once we reach a loiter target then lock to the final
         // altitude target

ArduPlane/commands_logic.cpp

@@ -1074,47 +1074,9 @@ bool Plane::verify_loiter_heading(bool init)
         return true;
     }
 
-    if (next_WP_loc.get_distance(next_nav_cmd.content.location) < abs(aparm.loiter_radius)) {
-        /* Whenever next waypoint is within the loiter radius,
-           maintaining loiter would prevent us from ever pointing toward the next waypoint.
-           Hence break out of loiter immediately
-         */
-        return true;
-    }
-
-    // Bearing in degrees
-    int32_t bearing_cd = current_loc.get_bearing_to(next_nav_cmd.content.location);
-
-    // get current heading.
-    int32_t heading_cd = gps.ground_course_cd();
-
-    int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
-
     if (init) {
         loiter.sum_cd = 0;
     }
 
-    /*
-      Check to see if the the plane is heading toward the land
-      waypoint. We use 20 degrees (+/-10 deg) of margin so that
-      we can handle 200 degrees/second of yaw.
-
-      After every full circle, extend acceptance criteria to ensure
-      aircraft will not loop forever in case high winds are forcing
-      it beyond 200 deg/sec when passing the desired exit course
-    */
-
-    // Use integer division to get discrete steps
-    int32_t expanded_acceptance = 1000 * (loiter.sum_cd / 36000);
-
-    if (labs(heading_err_cd) <= 1000 + expanded_acceptance) {
-        // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
-
-        // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
-        if (next_WP_loc.loiter_xtrack) {
-            next_WP_loc = current_loc;
-        }
-        return true;
-    }
-    return false;
+    return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location);
 }

ArduPlane/mode.h

@@ -3,6 +3,7 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Common/Location.h>
 #include <stdint.h>
+#include <AP_Common/Location.h>
 
 class Mode
 {
@@ -172,6 +173,9 @@ class ModeLoiter : public Mode
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
+    bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc);
+    bool isHeadingLinedUp_cd(const int32_t bearing_cd);
+
 protected:
 
     bool _enter() override;

ArduPlane/mode_loiter.cpp

@@ -7,9 +7,10 @@ bool ModeLoiter::_enter()
     plane.auto_throttle_mode = true;
     plane.auto_navigation_mode = true;
     plane.do_loiter_at_location();
+    plane.loiter_angle_reset();
 
 #if SOARING_ENABLED == ENABLED
-    if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) {
+    if (plane.g2.soaring_controller.is_active()) {
         plane.g2.soaring_controller.init_thermalling();
         plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path
     }
@@ -25,3 +26,57 @@ void ModeLoiter::update()
     plane.calc_throttle();
 }
 
+bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc)
+{
+    // Return true if current heading is aligned to vector to targetLoc.
+    // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
+
+    const uint16_t loiterRadius = abs(plane.aparm.loiter_radius);
+    if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) {
+        /* Whenever next waypoint is within the loiter radius plus 5%,
+           maintaining loiter would prevent us from ever pointing toward the next waypoint.
+           Hence break out of loiter immediately
+         */
+        return true;
+    }
+
+    // Bearing in centi-degrees
+    const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc);
+    return isHeadingLinedUp_cd(bearing_cd);
+}
+
+
+bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd)
+{
+    // Return true if current heading is aligned to bearing_cd.
+    // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
+
+    // get current heading.
+    const int32_t heading_cd = plane.gps.ground_course_cd();
+
+    const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
+
+    /*
+      Check to see if the the plane is heading toward the land
+      waypoint. We use 20 degrees (+/-10 deg) of margin so that
+      we can handle 200 degrees/second of yaw.
+
+      After every full circle, extend acceptance criteria to ensure
+      aircraft will not loop forever in case high winds are forcing
+      it beyond 200 deg/sec when passing the desired exit course
+    */
+
+    // Use integer division to get discrete steps
+    const int32_t expanded_acceptance = 1000 * (labs(plane.loiter.sum_cd) / 36000);
+
+    if (labs(heading_err_cd) <= 1000 + expanded_acceptance) {
+        // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
+
+        // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
+        if (plane.next_WP_loc.loiter_xtrack) {
+            plane.next_WP_loc = plane.current_loc;
+        }
+        return true;
+    }
+    return false;
+}

ArduPlane/navigation.cpp

@@ -318,6 +318,13 @@ void Plane::update_fbwb_speed_height(void)
             // the current altitude
             set_target_altitude_current();
         }
+
+#if SOARING_ENABLED == ENABLED
+        if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
+            // we're in soaring mode with throttle suppressed
+            set_target_altitude_current();;
+        }
+#endif
 
         target_altitude.last_elevator_input = elevator_input;
     }

ArduPlane/servos.cpp

@@ -470,16 +470,6 @@ void Plane::set_servos_controlled(void)
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 
             constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle));
     }
-
-#if SOARING_ENABLED == ENABLED
-    // suppress throttle when soaring is active
-    if ((control_mode == &mode_fbwb || control_mode == &mode_cruise ||
-        control_mode == &mode_auto || control_mode == &mode_loiter) &&
-        g2.soaring_controller.is_active() &&
-        g2.soaring_controller.get_throttle_suppressed()) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
-    }
-#endif
 }
 
 /*