ShipOps: Peters Suggestions

ArduCopter/Parameters.cpp

@@ -1182,9 +1182,9 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
     // @User: Standard
     AP_GROUPINFO("PLDP_THRESH", 1, ParametersG2, pldp_thrust_placed_fraction, 0.9),
 
-    // @Param: PLDP_RNG_MIN
-    // @DisplayName: Payload Place minimum range finder altitude
-    // @Description: Minimum range finder altitude in m to trigger payload touchdown, set to zero to disable.
+    // @Param: PLDP_RNG_MAX
+    // @DisplayName: Payload Place maximum range finder altitude
+    // @Description: Maximum range finder altitude in m to trigger payload touchdown, set to zero to disable.
     // @Units: m
     // @Range: 0 100
     // @User: Standard
@@ -1215,8 +1215,8 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
     AP_GROUPINFO("SURFTRAK_TC", 5, ParametersG2, surftrak_tc, 1.0),
 
     // @Param: PLDP_RNG_DRP
-    // @DisplayName: Payload Place range finder altitude in m that will trigger a drop, after 1 second, without touchdown detection.
-    // @Description: Payload Place range finder altitude in m that will trigger a drop, after 1 second, without touchdown detection.
+    // @DisplayName: Payload Place range finder altitude that will trigger a drop
+    // @Description: If the rangefinder returns a value less than this value a 1 second counter is started.  A drop will be performed once that timer has elapsed.  A value of zero disables this behaviour.
     // @Units: m
     // @Range: 0 100
     // @User: Standard

ArduCopter/RC_Channel.cpp

@@ -492,8 +492,8 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
 #endif
             break;
 
-        case AUX_FUNC::SHIP_OPS_MODE:
 #if MODE_SHIP_OPS_ENABLED == ENABLED
+        case AUX_FUNC::SHIP_OPS_MODE:
             switch (ch_flag) {
                 case AuxSwitchPos::LOW:
                     copter.mode_ship_ops.set_approach_mode(ModeShipOperation::ApproachMode::PAYLOAD_PLACE);
@@ -505,8 +505,8 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
                     copter.mode_ship_ops.set_approach_mode(ModeShipOperation::ApproachMode::LAUNCH_RECOVERY);
                     break;
             }
-#endif
             break;
+#endif
 
         case AUX_FUNC::STABILIZE:
             do_aux_function_change_mode(Mode::Number::STABILIZE, ch_flag);
@@ -633,14 +633,14 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
             switch (ch_flag) {
             case AuxSwitchPos::LOW:
                 // release tie down
-                gcs().send_text(MAV_SEVERITY_INFO, "Release tie down");
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Release tie down");
                 SRV_Channels::set_output_scaled(SRV_Channel::k_tie_down_release, 1000);
                 break;
             case AuxSwitchPos::MIDDLE:
                 break;
             case AuxSwitchPos::HIGH:
                 // secure tie down
-                gcs().send_text(MAV_SEVERITY_INFO, "Secure tie down");
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Secure tie down");
                 SRV_Channels::set_output_scaled(SRV_Channel::k_tie_down_release, 0);
                 break;
             }

ArduCopter/events.cpp

@@ -425,14 +425,16 @@ void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
 // this is always called from a failsafe so we trigger notification to pilot
 void Copter::set_mode_Ship_Op_or_RTL_or_land_with_pause(ModeReason reason)
 {
-    // attempt to switch to SmartRTL, if this failed then attempt to RTL
+    // attempt to switch to ShipLanding, if this fails then attempt to RTL
     // if that fails, then land
-    if (!set_mode(Mode::Number::SHIP_OPS, reason)) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Ship Operations Unavailable, Trying RTL Mode");
-        set_mode_RTL_or_land_with_pause(reason);
-    } else {
+#if MODE_SHIP_OPS_ENABLED
+    if (set_mode(Mode::Number::SHIP_OPS, reason)) {
         AP_Notify::events.failsafe_mode_change = 1;
+        return;
     }
+#endif
+    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Ship Operations Unavailable, Trying RTL Mode");
+    set_mode_RTL_or_land_with_pause(reason);
 }
 
 // Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param

ArduCopter/mode.h

@@ -35,6 +35,7 @@ class PayloadPlace {
     void run_horizontal_control();
     void init_vertical_control();
     void run_vertical_control();
+    bool within_rangefinder_release_range() const;
 
 private:
 
@@ -1484,15 +1485,15 @@ class ModeShipOperation : public Mode {
     const char *state_name(SubMode mode) const;
     void set_state(SubMode mode);
 
-    // SHIP_OPS states
+    // Keep-Out-Zone states
     enum class KeepOutZoneMode : uint8_t {
         NO_ACTION,
         AVOID_KOZ,
         EXIT_KOZ
     };
 
     void set_keep_out_zone_mode(KeepOutZoneMode keep_out_zone_mode);
-    KeepOutZoneMode keep_out_zone_mode() { return _keep_out_zone_mode; }
+    //KeepOutZoneMode keep_out_zone_mode() const { return new_keep_out_zone_mode; }
 
     // SHIP_OPS states
     enum class ApproachMode : uint8_t {
@@ -1501,7 +1502,7 @@ class ModeShipOperation : public Mode {
     };
 
     void set_approach_mode(ApproachMode approach_mode);
-    ApproachMode approach_mode() { return _approach_mode; }
+    //ApproachMode approach_mode() const { return new_approach_mode; }
 
     virtual bool is_landing() const override;
 
@@ -1516,23 +1517,25 @@ class ModeShipOperation : public Mode {
 private:
 
     SubMode _state = SubMode::CLIMB_TO_RTL;  // records state of rtl (initial climb, returning home, etc)
-    KeepOutZoneMode _keep_out_zone_mode = KeepOutZoneMode::NO_ACTION;  // records state of rtl (initial climb, returning home, etc)
-    ApproachMode _approach_mode = ApproachMode::LAUNCH_RECOVERY;  // records state of rtl (initial climb, returning home, etc)
+    KeepOutZoneMode keep_out_zone_mode = KeepOutZoneMode::NO_ACTION;  // records state of rtl (initial climb, returning home, etc)
+    ApproachMode approach_mode = ApproachMode::LAUNCH_RECOVERY;  // records state of rtl (initial climb, returning home, etc)
     uint32_t wp_distance() const override;
     int32_t wp_bearing() const override;
 
     uint32_t last_log_ms;   // system time of last time desired velocity was logging
     float target_climb_rate;   // system time of last time desired velocity was logging
     Vector3f offset;
-    Vector3p ship_pos_ned;
-    Vector3f ship_vel_ned;
-    Vector3f ship_accel_ned;
-    float ship_heading;
-    float ship_heading_rate;
-    float ship_heading_accel;
     bool ship_takeoff;
     bool pilot_correction_active;
-    bool ship_available;
+    struct {
+        Vector3p pos_ned;
+        Vector3f vel_ned;
+        Vector3f accel_ned;
+        float heading;
+        float heading_rate;
+        float heading_accel;
+        bool available;
+    } ship;
 
     // Ship Operations parameters
     AP_Float ship_perch_angle;

ArduCopter/mode_auto.cpp

@@ -1299,28 +1299,26 @@ void PayloadPlace::run_vertical_control()
     const auto &wp_nav = copter.wp_nav;
     const auto &pos_control = copter.pos_control;
 
-    if (copter.rangefinder_state.enabled && is_positive(g2.pldp_range_finder_drop_m)) {
-        if (!(copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0) && 
-        (copter.rangefinder_state.alt_cm > g2.pldp_range_finder_drop_m * 100.0 || copter.rangefinder_state.alt_cm < 0.5 * g2.pldp_range_finder_drop_m * 100.0)) ) {
-            rangefinder_drop_alt_time_ms = now_ms;
-        } else if (now_ms - rangefinder_drop_alt_time_ms < rangefinder_drop_check_duration_ms) {
-            switch (state) {
-            case State::Descent_Start:
-            case State::Descent_Measure:
-                // do nothing on this loop
-                break;
-            case State::Descent_Test:
-                gcs().send_text(MAV_SEVERITY_INFO, "%s landed", prefix_str);
-                state = State::Release;
-                break;
-            case State::Release:
-            case State::Releasing:
-            case State::Delay:
-            case State::Ascent_Start:
-            case State::Ascent:
-            case State::Done:
-                break;
-            }
+    if (!within_rangefinder_release_range()) {
+        // reset timer
+        rangefinder_drop_alt_time_ms = now_ms;
+    } else if (now_ms - rangefinder_drop_alt_time_ms < rangefinder_drop_check_duration_ms) {
+        switch (state) {
+        case State::Descent_Start:
+        case State::Descent_Measure:
+            // do nothing on this loop
+            break;
+        case State::Descent_Test:
+            gcs().send_text(MAV_SEVERITY_INFO, "%s rangefinder trigger", prefix_str);
+            state = State::Release;
+            break;
+        case State::Release:
+        case State::Releasing:
+        case State::Delay:
+        case State::Ascent_Start:
+        case State::Ascent:
+        case State::Done:
+            break;
         }
     }
 
@@ -1508,6 +1506,41 @@ void PayloadPlace::run_horizontal_control()
     copter.flightmode->land_run_horizontal_control();
 }
 
+// returns true if rangefinder returns valid range to trigger drop
+bool PayloadPlace::within_rangefinder_release_range() const
+{
+    auto &g2 = copter.g2;
+    if (!is_positive(g2.pldp_range_finder_drop_m)) {
+        // feature disabled
+        return false;
+    }
+    if (!copter.rangefinder_state.enabled) {
+        // no rangefinder!
+        return false;
+    }
+    if (!copter.rangefinder_alt_ok()) {
+        // no valid range from rangefinder
+        return false;
+    }
+    if (copter.rangefinder_state.glitch_count != 0) {
+        // do not accept reading when glitching is happening
+        return false;
+    }
+
+    const float rangefinder_range_m = copter.rangefinder_state.alt_cm * 0.01;
+    if (rangefinder_range_m > g2.pldp_range_finder_drop_m) {
+        // too high
+        return false;
+    }
+
+    if (rangefinder_range_m < 0.5 * g2.pldp_range_finder_drop_m) {
+        // too low
+        return false;
+    }
+
+    return true;
+}
+
 // sets the target_loc's alt to the vehicle's current alt but does not change target_loc's frame
 // in the case of terrain altitudes either the terrain database or the rangefinder may be used
 // returns true on success, false on failure