matternet · tridge · Mar 4, 2020 · Mar 4, 2020 · Mar 4, 2020 · Apr 7, 2020
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
@@ -78,6 +78,7 @@ enum aux_sw_func {
     AUXSW_USER_FUNC1 =          47, // user function #1
     AUXSW_USER_FUNC2 =          48, // user function #2
     AUXSW_USER_FUNC3 =          49, // user function #3
+    AUXSW_NOTCH_ENABLE =        50,
     AUXSW_SWITCH_MAX,
 };
 

diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
@@ -1565,15 +1565,29 @@ bool Copter::ModeAuto::verify_land()
                 land_start(dest);
 
                 // advance to next state
+                if (land_state != LandStateType_Descending) {
+                    gcs().send_text(MAV_SEVERITY_INFO,"Landing");
+                }
                 land_state = LandStateType_Descending;
-                gcs().send_text(MAV_SEVERITY_INFO,"Landing");
             }
             break;
 
         case LandStateType_Descending:
             // rely on THROTTLE_LAND mode to correctly update landing status
-            retval = ap.land_complete;
-            gcs().send_text(MAV_SEVERITY_INFO,"Landed");
+            retval = copter.ap.land_complete;
+            if (retval) {
+                gcs().send_text(MAV_SEVERITY_INFO,"Landed");
+            }
+            if (retval && !copter.mission.continue_after_land() && copter.motors->armed()) {
+                /*
+                  we want to stop mission processing on land
+                  completion. Disarm now, then return false. This
+                  leaves mission state machine in the current NAV_LAND
+                  mission item. After disarming the mission will reset
+                */
+                copter.init_disarm_motors();
+                retval = false;
+            }
             break;
 
         default:

diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp
@@ -235,6 +235,7 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
         case AUXSW_INVERTED:
         case AUXSW_WINCH_ENABLE:
         case AUXSW_RC_OVERRIDE_ENABLE:
+        case AUXSW_NOTCH_ENABLE:
             do_aux_switch_function(ch_option, ch_flag);
             break;
     }
@@ -761,6 +762,22 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
                 }
             }
             break;
+
+        case AUXSW_NOTCH_ENABLE:
+            // Allow or disallow notch filter
+            switch (ch_flag) {
+                case AUX_SWITCH_HIGH: {
+                    gcs().send_text(MAV_SEVERITY_INFO, "Notch enabled");
+                    ins.notch_enable(true);
+                    break;
+                }
+                case AUX_SWITCH_LOW: {
+                    gcs().send_text(MAV_SEVERITY_INFO, "Notch disabled");
+                    ins.notch_enable(false);
+                    break;
+                }
+            }
+            break;
 
 #ifdef USERHOOK_AUXSWITCH
         case AUXSW_USER_FUNC1:

diff --git a/Tools/scripts/configure-ci.sh b/Tools/scripts/configure-ci.sh
@@ -21,7 +21,7 @@ pushd $HOME
 # PX4 toolchain
 dir=$ARM_ROOT
 if [ ! -d "$HOME/opt/$dir" ]; then
-  wget http://firmware.ardupilot.org/Tools/PX4-tools/$ARM_TARBALL
+  wget http://firmware.ardupilot.org/Tools/STM32-tools/archived/$ARM_TARBALL
   tar -xf $ARM_TARBALL -C opt
 fi
 

diff --git a/Tools/scripts/install-prereqs-arch.sh b/Tools/scripts/install-prereqs-arch.sh
@@ -19,7 +19,7 @@ ARCH_AUR_PKGS="genromfs"
 # (see https://launchpad.net/gcc-arm-embedded/)
 ARM_ROOT="gcc-arm-none-eabi-4_9-2015q3"
 ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2"
-ARM_TARBALL_URL="http://firmware.ardupilot.org/Tools/PX4-tools/$ARM_TARBALL"
+ARM_TARBALL_URL="http://firmware.ardupilot.org/Tools/STM32-tools/archived/$ARM_TARBALL"
 
 # Ardupilot Tools
 ARDUPILOT_TOOLS="ardupilot/Tools/autotest"

diff --git a/Tools/scripts/install-prereqs-ubuntu.sh b/Tools/scripts/install-prereqs-ubuntu.sh
@@ -26,7 +26,7 @@ fi
 # (see https://launchpad.net/gcc-arm-embedded/)
 ARM_ROOT="gcc-arm-none-eabi-4_9-2015q3"
 ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2"
-ARM_TARBALL_URL="http://firmware.ardupilot.org/Tools/PX4-tools/$ARM_TARBALL"
+ARM_TARBALL_URL="http://firmware.ardupilot.org/Tools/STM32-tools/archived/$ARM_TARBALL"
 
 # Ardupilot Tools
 ARDUPILOT_TOOLS="Tools/autotest"

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -275,6 +275,10 @@ class AP_InertialSensor : AP_AccelCal_Client
     // return time in microseconds of last update() call
     uint32_t get_last_update_usec(void) const { return _last_update_usec; }
 
+    void notch_enable(bool enable) {
+        _notch_filter.force_enable(enable);
+    }
+
     enum IMU_SENSOR_TYPE {
         IMU_SENSOR_TYPE_ACCEL = 0,
         IMU_SENSOR_TYPE_GYRO = 1,

diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
@@ -25,7 +25,7 @@ const AP_Param::GroupInfo AP_Mission::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Mission options bitmask
     // @Description: Bitmask of what options to use in missions.
-    // @Bitmask: 0:Clear Mission on reboot
+    // @Bitmask: 0:Clear Mission on reboot, 2:ContinueAfterLand
     // @User: Advanced
     AP_GROUPINFO("OPTIONS",  2, AP_Mission, _options, AP_MISSION_OPTIONS_DEFAULT),
 

diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h
@@ -39,6 +39,7 @@
 
 #define AP_MISSION_OPTIONS_DEFAULT          0       // Do not clear the mission when rebooting
 #define AP_MISSION_MASK_MISSION_CLEAR       (1<<0)  // If set then Clear the mission on boot
+#define AP_MISSION_MASK_CONTINUE_AFTER_LAND (1<<2)  // Allow mission to continue after land
 
 /// @class    AP_Mission
 /// @brief    Object managing Mission
@@ -460,6 +461,15 @@ class AP_Mission {
     // available.
     bool jump_to_landing_sequence(void);
 
+    /*
+      return true if MIS_OPTIONS is set to allow continue of mission
+      logic after a land. If this is false then after a landing is
+      complete the vehicle should disarm and mission logic should stop
+     */
+    bool continue_after_land(void) const {
+        return (_options.get() & AP_MISSION_MASK_CONTINUE_AFTER_LAND) != 0;
+    }
+
     // user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 

diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
@@ -118,7 +118,7 @@ void NotchFilterVector3fParam::init(float _sample_freq_hz)
  */
 Vector3f NotchFilterVector3fParam::apply(const Vector3f &sample)
 {
-    if (!enable) {
+    if (!enable || rc_disabled) {
         // when not enabled it is a simple pass-through
         return sample;
     }
@@ -135,6 +135,17 @@ Vector3f NotchFilterVector3fParam::apply(const Vector3f &sample)
     return filter.apply(sample);
 }
 
+void NotchFilterVector3fParam::force_enable(bool _enable)
+{
+    if (!enable) {
+        return;
+    }
+    if (rc_disabled && _enable) {
+        init(sample_freq_hz);
+    }
+    rc_disabled = !_enable;
+}
+
 /* 
    instantiate template classes
  */

diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
@@ -49,7 +49,9 @@ class NotchFilterVector3fParam {
     Vector3f apply(const Vector3f &sample);
 
     static const struct AP_Param::GroupInfo var_info[];
-
+
+    void force_enable(bool enable);
+
 private:
     AP_Int8 enable;
     AP_Float center_freq_hz;
@@ -61,6 +63,7 @@ class NotchFilterVector3fParam {
     float last_center_freq;
     float last_bandwidth;
     float last_attenuation;
+    bool rc_disabled;
 
     NotchFilter<Vector3f> filter;
 };