diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7079285db4..dec99e518a 100644 --- 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 index ea59b65fbf..0a8059ab47 100644 --- 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 index ec8e81d9d3..62b86a74c6 100644 --- 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 index b721a3c19f..c2f5aeff25 100755 --- 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 index f33922347c..e29c9fac77 100755 --- 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 index ed7ae60fae..bceedd2919 100755 --- 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 index b0b9f88a9b..1a0278e32f 100644 --- 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 index 90daefbd23..51b0b71284 100644 --- 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 index 155af31f3c..0879a71bd3 100644 --- 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 index b4c834f0cd..2bf9369c54 100644 --- 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 index fb61ef98f0..c9cfa8ca28 100644 --- 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 filter; };