IO driver: Fixx lockdown flag handling

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

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);
 	}