From 1468ec497dd83425fb8f09263cfc379006af5eb0 Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sun, 23 Jan 2011 02:36:22 +0000
Subject: [PATCH] Added Arming light. LED A solid = safe, blinking = armed.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1529 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/ArduCopterMega.pde |  2 ++
 ArduCopterMega/system.pde         | 20 +++++++++++++++-----
 ArduCopterMega/test.pde           | 25 ++++---------------------
 3 files changed, 21 insertions(+), 26 deletions(-)

diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 8eeccf4d32..e48d939128 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -163,6 +163,8 @@ PID pid_nav_lon				(EE_GAIN_8);
 PID pid_baro_throttle		(EE_GAIN_9);
 PID pid_sonar_throttle		(EE_GAIN_10);
 
+boolean motor_light;
+
 // GPS variables
 // -------------
 byte 	ground_start_count	= 5;			// have we achieved first lock and set Home?
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 4f2cfc26e7..88f4833d38 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -346,19 +346,29 @@ void update_GPS_light(void)
 		GPS_light = !GPS_light;
 		if(GPS_light){
 			digitalWrite(C_LED_PIN, HIGH);
-			digitalWrite(A_LED_PIN, HIGH);
-			digitalWrite(B_LED_PIN, HIGH);
 		}else{
 			digitalWrite(C_LED_PIN, LOW);
-			digitalWrite(A_LED_PIN, LOW);
-			digitalWrite(B_LED_PIN, LOW);
 		}		
 	}else{
 		if(!GPS_light){
 			GPS_light = true;
 			digitalWrite(C_LED_PIN, HIGH);
+		}
+	}
+	
+	if(motor_armed == true){
+		motor_light = !motor_light;
+
+		// blink
+		if(motor_light){
+			digitalWrite(A_LED_PIN, HIGH);
+		}else{
+			digitalWrite(A_LED_PIN, LOW);
+		}		
+	}else{
+		if(!motor_light){
+			motor_light = true;
 			digitalWrite(A_LED_PIN, HIGH);
-			digitalWrite(B_LED_PIN, HIGH);
 		}
 	}
 }
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 58842acebe..09c982587b 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -259,31 +259,14 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
 			ts_num++;
 			if (ts_num > 10){
 				ts_num = 0;
-				/*Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, rc2:%d, rc4 %d, ny:%ld, ys:%ld, ye:%ld,  R: %d,  L: %d  F: %d  B: %d\n"),
+				Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int%4.4f, "),
 					(int)(roll_sensor/100),
 					(int)(pitch_sensor/100),
 					rc_1.pwm_out,
-					rc_2.pwm_out,
-					rc_4.pwm_out,
-					nav_yaw,
-					dcm.yaw_sensor,
-					yaw_error,
-					motor_out[RIGHT],
-					motor_out[LEFT],
-					motor_out[FRONT],
-					motor_out[BACK]);*/
+					pid_stabilize_roll.get_integrator());
 
-				Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int%4.4f,  R: %d,  L: %d  F: %d  B: %d\n"),
-					(int)(roll_sensor/100),
-					(int)(pitch_sensor/100),
-					rc_1.pwm_out,
-					pid_stabilize_roll.get_integrator(),
-					motor_out[RIGHT],
-					motor_out[LEFT],
-					motor_out[FRONT],
-					motor_out[BACK]);
+				print_motor_out();
 			}
-
 			// R: 1417,  L: 1453  F: 1453  B: 1417
 			
 			//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
@@ -846,7 +829,7 @@ void print_hit_enter()
 
 
 void print_motor_out(){
-	Serial.printf("out: %d %d %d %d\n", 
+	Serial.printf("out: R: %d,  L: %d  F: %d  B: %d\n", 
 				(motor_out[RIGHT] 	- rc_3.radio_min),
 				(motor_out[LEFT] 	- rc_3.radio_min),
 				(motor_out[FRONT] 	- rc_3.radio_min),