Merge pull request #596 from PX4/lockdown_disable

Lockdown disable

4 files changed, 63 insertions, 18 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 33589940c..52cf25086 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -154,6 +154,16 @@ static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
 
+static struct vehicle_status_s status;
+
+/* armed topic */
+static struct actuator_armed_s armed;
+
+static struct safety_s safety;
+
+/* flags for control apps */
+struct vehicle_control_mode_s control_mode;
+
 /* tasks waiting for low prio thread */
 typedef enum {
 	LOW_PRIO_TASK_NONE = 0,
@@ -210,6 +220,9 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
+int arm();
+int disarm();
+
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
 /**
@@ -277,6 +290,16 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "arm")) {
+		arm();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "disarm")) {
+		disarm();
+		exit(0);
+	}
+
 	usage("unrecognized command");
 	exit(1);
 }
@@ -344,6 +367,30 @@ void print_status()
 static orb_advert_t control_mode_pub;
 static orb_advert_t status_pub;
 
+int arm()
+{
+	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+int disarm()
+{
+	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -537,16 +584,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 }
 
-static struct vehicle_status_s status;
-
-/* armed topic */
-static struct actuator_armed_s armed;
-
-static struct safety_s safety;
-
-/* flags for control apps */
-struct vehicle_control_mode_s control_mode;
-
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 2e79f0ac6..f39fcf7ec 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -223,14 +223,25 @@ mixer_tick(void)
 		}
 	}
 
-	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
+	/* set arming */
+	bool needs_to_arm = (should_arm || should_always_enable_pwm);
+
+	/* check any conditions that prevent arming */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
+		needs_to_arm = false;
+	}
+	if (!should_arm && !should_always_enable_pwm) {
+		needs_to_arm = false;
+	}
+
+	if (needs_to_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
 		isr_debug(5, "> PWM enabled");
 
-	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
+	} else if (!needs_to_arm && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index e3f499435..d48c6c529 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -177,6 +177,7 @@
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
+#define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 2d7a1fe0d..1335f52e1 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -185,7 +185,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
 					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
-					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
+					 PX4IO_P_SETUP_ARMING_LOCKDOWN)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -498,11 +499,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * lockup of the IO arming state.
 			 */
 
-			// XXX do not reset IO's safety state by FMU for now
-			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
-			// }
-
 			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}