Plane: Soaring - better reporting of exit due to drift.

ArduPlane/soaring.cpp

@@ -74,6 +74,7 @@ void Plane::update_soaring() {
         const char* strTooHigh = "Soaring: Too high, restoring ";
         const char* strTooLow =  "Soaring: Too low, restoring ";
         const char* strTooWeak = "Soaring: Thermal ended, restoring ";
+        const char* strTooFar  = "Soaring: Drifted too far, restoring ";
 
         // Exit as soon as thermal state estimate deteriorates and we're lined up to next target
         switch (previous_mode->mode_number()){
@@ -97,6 +98,10 @@ void Plane::update_soaring() {
                     gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_fbwb.name());
                     set_mode(mode_fbwb, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
                     break;
+                case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_fbwb.name());
+                    set_mode(mode_fbwb, ModeReason::SOARING_DRIFT_EXCEEDED);
+                    break;
                 } // switch louterStatus
             break;
             }
@@ -122,6 +127,10 @@ void Plane::update_soaring() {
                     gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_cruise.name());
                     set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
                     break;
+                case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_cruise.name());
+                    set_mode(mode_cruise, ModeReason::SOARING_DRIFT_EXCEEDED);
+                    break;
             } // switch loiterStatus
             cruise_state = cruise;
             set_target_altitude_current();
@@ -131,7 +140,7 @@ void Plane::update_soaring() {
         case Mode::Number::AUTO: {
             //Get the lat/lon of next Nav waypoint after this one:
             AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd();;
-            const bool headingLinedupToWP = isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
+            const bool headingLinedupToWP = plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
             if (!headingLinedupToWP) {
                 break;
             }
@@ -149,6 +158,10 @@ void Plane::update_soaring() {
                         gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name());
                         set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
                     break;
+                case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_auto.name());
+                    set_mode(mode_auto, ModeReason::SOARING_DRIFT_EXCEEDED);
+                    break;
                 } // switch loiterStatus
             break;
             } // case AUTO

libraries/AP_Soaring/AP_Soaring.cpp

@@ -121,7 +121,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ENABLE_CH", 15, SoaringController, soar_active_ch, 0),
 
-    // @Param: MAX_DEV
+    // @Param: MAX_DRIFT
     // @DisplayName: (Optional) Maximum drift distance to allow when thermalling.
     // @Description: The previous mode will be restored if the horizontal distance to the thermalling start location exceeds this value. 0 to disable.
     // @Range: 0 1000
@@ -216,8 +216,8 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
             if (result != _cruise_criteria_msg_last) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Not climbing");
             }
-        } else if (max_drift && (powf(position.x - _thermal_start_pos.x, 2) + powf(position.y - _thermal_start_pos.y, 2)) > powf(max_drift,2)) {
-            result = DEVIATION_EXCEEDED;
+        } else if (!(max_drift<0) && (powf(position.x - _thermal_start_pos.x, 2) + powf(position.y - _thermal_start_pos.y, 2)) > powf(max_drift,2)) {
+            result = DRIFT_EXCEEDED;
             if (result != _cruise_criteria_msg_last) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Drifted too far");
             }

libraries/AP_Soaring/AP_Soaring.h

@@ -78,7 +78,7 @@ public:
         ALT_TOO_LOW,
         THERMAL_WEAK,
         ALT_LOST,
-        DEVIATION_EXCEEDED,
+        DRIFT_EXCEEDED,
         THERMAL_GOOD_TO_KEEP_LOITERING
     } LoiterStatus;
 

libraries/AP_Vehicle/ModeReason.h

@@ -55,4 +55,5 @@ enum class ModeReason : uint8_t {
   AUTOROTATION_BAILOUT,
   SOARING_ALT_TOO_HIGH,
   SOARING_ALT_TOO_LOW,
+  SOARING_DRIFT_EXCEEDED,
 };