diff --git a/src/drivers/px4io/CMakeLists.txt b/src/drivers/px4io/CMakeLists.txt index 5c180c6f82..b7250f1145 100644 --- a/src/drivers/px4io/CMakeLists.txt +++ b/src/drivers/px4io/CMakeLists.txt @@ -33,7 +33,7 @@ px4_add_module( MODULE drivers__px4io MAIN px4io - STACK 1200 + STACK 1800 COMPILE_FLAGS -Os SRCS diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8659f49d39..adcb4466be 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -273,7 +273,8 @@ private: /* cached IO state */ uint16_t _status; ///< Various IO status flags uint16_t _alarms; ///< Various IO alarms - uint16_t _last_written_arming; ///< the last written arming state reg + uint16_t _last_written_arming_s; ///< the last written arming state reg + uint16_t _last_written_arming_c; ///< the last written arming state reg /* subscribed topics */ int _t_actuator_controls_0; ///< actuator controls group 0 topic @@ -510,7 +511,8 @@ PX4IO::PX4IO(device::Device *interface) : _perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")), _status(0), _alarms(0), - _last_written_arming(0), + _last_written_arming_s(0), + _last_written_arming_c(0), _t_actuator_controls_0(-1), _t_actuator_controls_1(-1), _t_actuator_controls_2(-1), @@ -836,7 +838,8 @@ PX4IO::init() PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | + PX4IO_P_SETUP_ARMING_LOCKDOWN, 0); if (_rc_handling_disabled) { ret = io_disable_rc_handling(); @@ -1331,9 +1334,11 @@ PX4IO::io_set_arming_state() if (armed.lockdown && !_lockdown_override) { set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + _lockdown_override = true; - } else { + } else if (!armed.lockdown && _lockdown_override) { clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + _lockdown_override = false; } /* Do not set failsafe if circuit breaker is enabled */ @@ -1369,11 +1374,9 @@ PX4IO::io_set_arming_state() } } - uint16_t new_arming_state = set; - new_arming_state &= ~(clear); - - if (_last_written_arming != new_arming_state) { - _last_written_arming = new_arming_state; + if (_last_written_arming_s != set || _last_written_arming_c != clear) { + _last_written_arming_s = set; + _last_written_arming_c = clear; return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); }