From 74f76006029ad6b209c1a409e8e35044b1c53bbb Mon Sep 17 00:00:00 2001
From: Julian Oes <joes@student.ethz.ch>
Date: Tue, 6 Nov 2012 19:03:08 -0800
Subject: Again some hacking to make arming/disarming work for now

---
 apps/px4io/comms.c  | 10 ++++++++--
 apps/px4io/mixer.c  | 14 +++++++-------
 apps/px4io/px4io.c  |  3 ++-
 apps/px4io/px4io.h  |  2 +-
 apps/px4io/safety.c |  4 ++--
 5 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 1edff25b1..0a0453b55 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -148,13 +148,19 @@ comms_handle_command(const void *buffer, size_t length)
 	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
 		system_state.fmu_channel_data[i] = cmd->servo_command[i];
 
+	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
+	if(system_state.arm_ok && !cmd->arm_ok) {
+		system_state.armed = false;
+	}
+
 	system_state.arm_ok = cmd->arm_ok;
 	system_state.mixer_use_fmu = true;
 	system_state.fmu_data_received = true;
 
+
 	/* handle changes signalled by FMU */
-	if (!system_state.arm_ok && system_state.armed)
-		system_state.armed = false;
+//	if (!system_state.arm_ok && system_state.armed)
+//		system_state.armed = false;
 
 	/* XXX do relay changes here */	
 	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 471965fd7..28edb2357 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -93,7 +93,7 @@ static void	mixer_get_rc_input(void);
 static void	mixer_update(int mixer, uint16_t *inputs, int input_count);
 
 /* current servo arm/disarm state */
-bool mixer_servos_armed;
+bool mixer_servos_armed = false;
 
 /*
  * Each mixer consumes a set of inputs and produces a single output.
@@ -182,10 +182,10 @@ mixer_tick(void *arg)
 			system_state.fmu_data_received = false;
 		}
 
-	} else if (system_state.rc_channels > 0) {
-		/* we have control data from an R/C input */
-		control_count = system_state.rc_channels;
-		control_values = &system_state.rc_channel_data[0];
+//	} else if (system_state.rc_channels > 0) {
+//		/* we have control data from an R/C input */
+//		control_count = system_state.rc_channels;
+//		control_values = &system_state.rc_channel_data[0];
 
 	} else {
 		/* we have no control input */
@@ -202,7 +202,7 @@ mixer_tick(void *arg)
 			/*
 			 * If we are armed, update the servo output.
 			 */
-			if (system_state.armed)
+			if (system_state.armed && system_state.arm_ok)
 				up_pwm_servo_set(i, mixers[i].current_value);
 		}
 	}
@@ -210,7 +210,7 @@ mixer_tick(void *arg)
 	/*
 	 * Decide whether the servos should be armed right now.
 	 */
-	should_arm = system_state.armed && (control_count > 0);
+	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 7039e5d58..7240a80ef 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -133,8 +133,9 @@ int user_start(int argc, char *argv[])
 		/* print some simple status */
 		if (timers[TIMER_STATUS_PRINT] == 0) {
 			timers[TIMER_STATUS_PRINT] = 1000;
-			lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d   \r",
+			lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d   \r",
 				cursor[cycle++ & 3],
+				(system_state.arm_ok        ? "FMU_ARMED"  : "FMU_SAFE"),
 				(system_state.armed         ? "ARMED"  : "SAFE"),
 				(system_state.rc_channels   ? "RC OK"  : "NO RC"),
 				(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 274b27ff3..8eb0c53b2 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -68,7 +68,7 @@
 struct sys_state_s 
 {
 
-	bool		armed;		/* actually armed */
+	bool		armed;		/* IO armed */
 	bool		arm_ok;		/* FMU says OK to arm */
 
 	/*
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index f27432664..0ad636c0b 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -76,9 +76,9 @@ safety_check_button(void *arg)
 	/* 
 	 * Debounce the safety button, change state if it has been held for long enough.
 	 *
-	 * Ignore the button if FMU has not said it's OK to arm yet.
 	 */
-	if (BUTTON_SAFETY && system_state.arm_ok) {
+
+	if (BUTTON_SAFETY) {
 		if (arm_counter < ARM_COUNTER_THRESHOLD) {
 			arm_counter++;
 		} else if (arm_counter == ARM_COUNTER_THRESHOLD) {
-- 
cgit v1.2.3