diff --git a/boards/holybro/pix32v5/src/board_config.h b/boards/holybro/pix32v5/src/board_config.h index f2509946a5..98e5e6f9b1 100644 --- a/boards/holybro/pix32v5/src/board_config.h +++ b/boards/holybro/pix32v5/src/board_config.h @@ -233,7 +233,7 @@ #define GPIO_nARMED_INIT /* PI0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTI|GPIO_PIN0) #define GPIO_nARMED /* PI0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0) -#define BOARD_INDICATE_ARMED_STATE(on_armed) px4_arch_configgpio((on_armed) ? GPIO_nARMED : GPIO_nARMED_INIT) +#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) /* PWM */ diff --git a/boards/nxp/fmurt1062-v1/src/board_config.h b/boards/nxp/fmurt1062-v1/src/board_config.h index 8efe241136..4bedc009a0 100644 --- a/boards/nxp/fmurt1062-v1/src/board_config.h +++ b/boards/nxp/fmurt1062-v1/src/board_config.h @@ -231,7 +231,7 @@ #define GPIO_nARMED_INIT /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | GPIO_INPUT | nARMED_INPUT_IOMUX) #define GPIO_nARMED /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | nARMED_OUTPUT_IOMUX) -#define BOARD_INDICATE_ARMED_STATE(on_armed) px4_arch_configgpio((on_armed) ? GPIO_nARMED : GPIO_nARMED_INIT) +#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) /* PWM */ diff --git a/boards/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h index f2509946a5..98e5e6f9b1 100644 --- a/boards/px4/fmu-v5/src/board_config.h +++ b/boards/px4/fmu-v5/src/board_config.h @@ -233,7 +233,7 @@ #define GPIO_nARMED_INIT /* PI0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTI|GPIO_PIN0) #define GPIO_nARMED /* PI0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0) -#define BOARD_INDICATE_ARMED_STATE(on_armed) px4_arch_configgpio((on_armed) ? GPIO_nARMED : GPIO_nARMED_INIT) +#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) /* PWM */ diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 54705803a8..770a984945 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -213,7 +213,7 @@ #define GPIO_nARMED /* PC12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN12) #if !defined(TRACE_PINS) -# define BOARD_INDICATE_ARMED_STATE(on_armed) px4_arch_configgpio((on_armed) ? GPIO_nARMED : GPIO_nARMED_INIT) +# define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) #endif /* PWM */ diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index c398a2da61..8f654cccbc 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -249,7 +249,7 @@ #if !defined(TRACE_PINS) #define GPIO_nARMED_INIT /* PE6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN6) #define GPIO_nARMED /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6) -#define BOARD_INDICATE_ARMED_STATE(on_armed) px4_arch_configgpio((on_armed) ? GPIO_nARMED : GPIO_nARMED_INIT) +#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) #endif diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index 5391b0a756..be145bdbae 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -41,6 +41,7 @@ endif() add_library(px4_platform board_identity.c + external_reset_lockout.cpp i2c.cpp i2c_spi_buses.cpp module.cpp diff --git a/platforms/common/external_reset_lockout.cpp b/platforms/common/external_reset_lockout.cpp new file mode 100644 index 0000000000..63f88d2349 --- /dev/null +++ b/platforms/common/external_reset_lockout.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#if defined(BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE) + +#include + +static px4::atomic lockout_states {0}; + +void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled) +{ + const uint8_t component_mask = 1 << (uint8_t)component; + uint8_t current_state; + + if (enabled) { + current_state = lockout_states.fetch_or(component_mask) | component_mask; + + } else { + current_state = lockout_states.fetch_and(~component_mask) & ~component_mask; + } + + BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(current_state != 0); +} + +#else + +void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled) {} + +#endif /* BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE */ + diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index cad5195341..8999f88a80 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -285,14 +285,6 @@ # endif #endif // -/* Provide an overridable default nop - * for BOARD_INDICATE_ARMED_STATE - */ - -#if !defined(BOARD_INDICATE_ARMED_STATE) -# define BOARD_INDICATE_ARMED_STATE(on_armed) -#endif - /************************************************************************************ * Public Data ************************************************************************************/ diff --git a/platforms/common/include/px4_platform_common/external_reset_lockout.h b/platforms/common/include/px4_platform_common/external_reset_lockout.h new file mode 100644 index 0000000000..b4cdf8df6c --- /dev/null +++ b/platforms/common/include/px4_platform_common/external_reset_lockout.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +enum class LockoutComponent : uint8_t { + Commander = 0, + SystemShutdownLock = 1 +}; + +/** + * A board may provide a mechanism to signal that PX4 is in a critical state + * and should not be reset/powered off. + * The main use is when the system is armed, but also during parameter saving. + * + * This can be called from different threads. + * + * @param component calling component + * @param enabled true if compoment is in critical state + */ +void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled); + + + diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index 03c6b7e0c4..b140e5f1a4 100644 --- a/platforms/common/shutdown.cpp +++ b/platforms/common/shutdown.cpp @@ -47,6 +47,7 @@ #define MODULE_NAME "shutdown" #endif +#include #include #include @@ -69,6 +70,7 @@ int px4_shutdown_lock() if (ret == 0) { ++shutdown_lock_counter; + px4_indicate_external_reset_lockout(LockoutComponent::SystemShutdownLock, true); return pthread_mutex_unlock(&shutdown_mutex); } @@ -83,6 +85,10 @@ int px4_shutdown_unlock() if (shutdown_lock_counter > 0) { --shutdown_lock_counter; + if (shutdown_lock_counter == 0) { + px4_indicate_external_reset_lockout(LockoutComponent::SystemShutdownLock, false); + } + } else { PX4_ERR("unmatched number of px4_shutdown_unlock() calls"); } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 46c7336154..4d83c26c29 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -2502,6 +2503,8 @@ Commander::run() arm_auth_update(now, params_updated || param_init_forced); + px4_indicate_external_reset_lockout(LockoutComponent::Commander, armed.armed); + px4_usleep(COMMANDER_MONITORING_INTERVAL); } @@ -2636,10 +2639,6 @@ Commander::control_status_leds(vehicle_status_s *status_local, const actuator_ar _last_overload = overload; - /* board supports HW armed indicator */ - - BOARD_INDICATE_ARMED_STATE(actuator_armed->armed); - #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS) /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */