AC2.0.24b

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2567 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/APM_Config.h

@@ -7,13 +7,17 @@
 //#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
 #define NAV_TEST 1			// 0 = traditional, 1 = rate controlled
-#define YAW_OPTION 0		// 0 = hybrid rate approach, 1 = offset Yaw approach
+#define YAW_OPTION 1		// 0 = hybrid rate approach, 1 = offset Yaw approach, 2 = simple hybrid
 #define AUTO_RESET_LOITER 1 // enables Loiter to reset it's current location based on stick input.
 #define CUT_MOTORS 1		// do we cut the motors with no throttle?
 
+#define DYNAMIC_DRIFT 0 	// careful!!! 0 = off, 1 = on
+
 // do we want to have camera stabilization?
 #define CAMERA_STABILIZER ENABLED
 
+#define BROKEN_SLIDER 0
+
 #define FRAME_CONFIG QUAD_FRAME
 	/*
 	options:
@@ -73,4 +77,4 @@
 #define FR_LED AN12  // Mega PE4 pin, OUT7
 #define RE_LED AN14  // Mega PE5 pin, OUT6
 #define RI_LED AN10  // Mega PH4 pin, OUT5
-#define LE_LED AN8  // Mega PH5 pin, OUT4
+#define LE_LED AN8  // Mega PH5 pin, OUT4

ArduCopterMega/ArduCopterMega.pde

@@ -107,7 +107,7 @@ GPS         *g_gps;
     AP_Compass_HMC5883L      compass(Parameters::k_param_compass);
   #else
     #error Unrecognised MAG_PROTOCOL setting.
-  #endif 
+  #endif
 
 	// real GPS selection
 	#if   GPS_PROTOCOL == GPS_PROTOCOL_AUTO
@@ -191,7 +191,15 @@ GPS         *g_gps;
 //#include <GCS_SIMPLE.h>
 //GCS_SIMPLE    gcs_simple(&Serial);
 
-AP_RangeFinder_MaxsonarXL sonar;
+////////////////////////////////////////////////////////////////////////////////
+// SONAR selection
+////////////////////////////////////////////////////////////////////////////////
+//
+#if SONAR_TYPE == MAX_SONAR_XL
+	AP_RangeFinder_MaxsonarXL sonar;//(SONAR_PORT, &adc);
+#elif SONAR_TYPE == MAX_SONAR_LV
+	AP_RangeFinder_MaxsonarLV sonar;//(SONAR_PORT, &adc);
+#endif
 
 ////////////////////////////////////////////////////////////////////////////////
 // Global variables
@@ -255,6 +263,7 @@ bool 	did_clear_yaw_control;
 boolean motor_light;						// status of the Motor safety
 boolean GPS_light;							// status of the GPS light
 boolean timer_light;						// status of the Motor safety
+byte	led_mode = NORMAL_LEDS;
 
 // GPS variables
 // -------------
@@ -321,9 +330,11 @@ float	current_total;
 
 // Barometer Sensor variables
 // --------------------------
-unsigned long 	abs_pressure;
+long 			abs_pressure;
-unsigned long 	ground_pressure;
+long 			ground_pressure;
 int 			ground_temperature;
+int32_t 		baro_filter[BARO_FILTER_SIZE];
+byte			baro_filter_index;
 
 // Altitude Sensor variables
 // ----------------------
@@ -340,13 +351,14 @@ boolean	land_complete;
 int		landing_distance;					// meters;
 long 	old_alt;							// used for managing altitude rates
 int		velocity_land;
-byte 	yaw_tracking = MAV_ROI_WPNEXT;			// no tracking, point at next wp, or at a target
+byte 	yaw_tracking = MAV_ROI_WPNEXT;		// no tracking, point at next wp, or at a target
+byte	brake_timer;
 
 // Loiter management
 // -----------------
 long 	saved_target_bearing;				// deg * 100
-long 	loiter_time;						// millis : when we started LOITER mode
+unsigned long 	loiter_time;				// millis : when we started LOITER mode
-long 	loiter_time_max;					// millis : how long to stay in LOITER mode
+unsigned long 	loiter_time_max;			// millis : how long to stay in LOITER mode
 
 // these are the values for navigation control functions
 // ----------------------------------------------------
@@ -364,14 +376,16 @@ bool 	invalid_nav;						// used to control when we calculate nav_throttle
 bool 	set_throttle_cruise_flag = false;	// used to track the throttle crouse value
 
 long 	command_yaw_start;					// what angle were we to begin with
-long 	command_yaw_start_time;				// when did we start turning
+unsigned long 	command_yaw_start_time;				// when did we start turning
-int		command_yaw_time;					// how long we are turning
+unsigned int	command_yaw_time;					// how long we are turning
 long 	command_yaw_end;					// what angle are we trying to be
 long 	command_yaw_delta;					// how many degrees will we turn
 int		command_yaw_speed;					// how fast to turn
 byte	command_yaw_dir;
 byte	command_yaw_relative;
 
+int 	auto_level_counter;
+
 // Waypoints
 // ---------
 long	wp_distance;						// meters - distance between plane and next waypoint
@@ -381,8 +395,8 @@ byte	next_wp_index;						// Current active command index
 // repeating event control
 // -----------------------
 byte 	event_id; 							// what to do - see defines
-long 	event_timer; 						// when the event was asked for in ms
+unsigned long 	event_timer; 						// when the event was asked for in ms
-int 	event_delay; 						// how long to delay the next firing of event in millis
+unsigned int 	event_delay; 						// how long to delay the next firing of event in millis
 int 	event_repeat;						// how many times to fire : 0 = forever, 1 = do once, 2 = do twice
 int 	event_value; 						// per command value, such as PWM for servos
 int 	event_undo_value;					// the value used to undo commands
@@ -458,6 +472,7 @@ float 			load;						// % MCU cycles used
 
 byte			counter_one_herz;
 bool			GPS_enabled 	= false;
+byte			loop_step;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
@@ -478,6 +493,9 @@ void loop()
 		G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
 		mainLoop_count++;
 
+		//if (delta_ms_fast_loop > 6)
+		//	Log_Write_Performance();
+
 		/*
 		if(delta_ms_fast_loop > 11){
 			update_timer_light(true);
@@ -500,6 +518,7 @@ void loop()
 
 		fifty_hz_loop();
 		counter_one_herz++;
+
 		if(counter_one_herz == 50){
 			super_slow_loop();
 			counter_one_herz = 0;
@@ -519,7 +538,7 @@ void loop()
 	}
 }
 
-// Main loop 160Hz
+// Main loop
 void fast_loop()
 {
 	// IMU DCM Algorithm
@@ -532,8 +551,7 @@ void fast_loop()
 
 	// Read radio
 	// ----------
-	if (APM_RC.GetState() == 1)
+	read_radio();			// read the radio first
-		read_radio();			// read the radio first
 
 	// custom code/exceptions for flight modes
 	// ---------------------------------------
@@ -566,6 +584,8 @@ void medium_loop()
 		// This case deals with the GPS and Compass
 		//-----------------------------------------
 		case 0:
+			loop_step = 1;
+
 			medium_loopCounter++;
 
 			if(GPS_enabled){
@@ -582,11 +602,15 @@ void medium_loop()
 					compass.null_offsets(dcm.get_dcm_matrix());
 				}
 			#endif
+
+			// auto_trim, uses an auto_level algorithm
+			auto_trim();
 			break;
 
 		// This case performs some navigation computations
 		//------------------------------------------------
 		case 1:
+			loop_step = 2;
 			medium_loopCounter++;
 
 			// hack to stop navigation in Simple mode
@@ -597,7 +621,10 @@ void medium_loop()
 			}
 
 			// Auto control modes:
-			if(g_gps->new_data){
+			if(g_gps->new_data && g_gps->fix){
+				loop_step = 11;
+
+				// invalidate GPS data
 				g_gps->new_data 	= false;
 
 				// we are not tracking I term on navigation, so this isn't needed
@@ -621,6 +648,7 @@ void medium_loop()
 		// command processing
 		//-------------------
 		case 2:
+			loop_step = 3;
 			medium_loopCounter++;
 
 			// Read altitude from sensors
@@ -636,6 +664,13 @@ void medium_loop()
 			// invalidate the throttle hold value
 			// ----------------------------------
 			invalid_throttle = true;
+			break;
+
+		// This case deals with sending high rate telemetry
+		//-------------------------------------------------
+		case 3:
+			loop_step = 4;
+			medium_loopCounter++;
 
 			// perform next command
 			// --------------------
@@ -644,12 +679,6 @@ void medium_loop()
 					update_commands();
 				//}
 			}
-			break;
-
-		// This case deals with sending high rate telemetry
-		//-------------------------------------------------
-		case 3:
-			medium_loopCounter++;
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
 				if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
@@ -672,11 +701,17 @@ void medium_loop()
 			#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
 				hil.data_stream_send(5,45);
 			#endif
+
+			if (g.log_bitmask & MASK_LOG_MOTORS)
+				Log_Write_Motors();
+
+
 			break;
 
 		// This case controls the slow loop
 		//---------------------------------
 		case 4:
+			loop_step = 5;
 			medium_loopCounter = 0;
 
 			delta_ms_medium_loop	= millis() - medium_loopTimer;
@@ -695,6 +730,10 @@ void medium_loop()
 			// -----------------------
 			arm_motors();
 
+			// should we be in DCM Fast recovery?
+			// ----------------------------------
+			check_DCM();
+
 			slow_loop();
 			break;
 
@@ -756,9 +795,10 @@ void fifty_hz_loop()
 	// XXX this should be absorbed into the above,
 	// or be a "GCS fast loop" interface
 
-	#if FRAME_CONFIG ==	TRI_FRAME
+	#if FRAME_CONFIG == TRI_FRAME
 		// Hack - had to move to 50hz loop to test a theory
 		// servo Yaw
+		g.rc_4.calc_pwm();
 		APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
 	#endif
 }
@@ -770,6 +810,7 @@ void slow_loop()
 	//----------------------------------------
 	switch (slow_loopCounter){
 		case 0:
+			loop_step = 6;
 			slow_loopCounter++;
 			superslow_loopCounter++;
 
@@ -784,6 +825,7 @@ void slow_loop()
 			break;
 
 		case 1:
+			loop_step = 7;
 			slow_loopCounter++;
 
 			// Read 3-position switch on radio
@@ -810,11 +852,12 @@ void slow_loop()
 			break;
 
 		case 2:
+			loop_step = 8;
 			slow_loopCounter = 0;
 			update_events();
 
 			// blink if we are armed
-			update_motor_light();
+			update_lights();
 
 			// XXX this should be a "GCS slow loop" interface
 			#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
@@ -836,8 +879,8 @@ void slow_loop()
 
 
 			// filter out the baro offset.
-			if(baro_alt_offset > 0) baro_alt_offset--;
+			//if(baro_alt_offset > 0) baro_alt_offset--;
-			if(baro_alt_offset < 0) baro_alt_offset++;
+			//if(baro_alt_offset < 0) baro_alt_offset++;
 
 
 			#if MOTOR_LEDS == 1
@@ -856,6 +899,7 @@ void slow_loop()
 // 1Hz loop
 void super_slow_loop()
 {
+	loop_step = 9;
 	if (g.log_bitmask & MASK_LOG_CURRENT)
 		Log_Write_Current();
 
@@ -883,6 +927,7 @@ void super_slow_loop()
 
 void update_GPS(void)
 {
+	loop_step = 10;
 	g_gps->update();
 	update_GPS_light();
 
@@ -1234,14 +1279,51 @@ void update_trig(void){
 	sin_roll_y 		= temp.c.y / cos_pitch_x;
 }
 
-
+// updated at 10hz
 void update_alt()
 {
 	altitude_sensor = BARO;
 
-#if HIL_MODE == HIL_MODE_ATTITUDE
+	#if HIL_MODE == HIL_MODE_ATTITUDE
 	current_loc.alt = g_gps->altitude;
-#else
+	#else
+
+	if(g.sonar_enabled){
+		// filter out offset
+		float scale;
+
+		// read barometer
+		baro_alt 			= read_barometer();
+		int temp_sonar 		= sonar.read();
+
+		// spike filter
+		//if((temp_sonar - sonar_alt) < 50){
+			sonar_alt = temp_sonar;
+		//}
+
+		scale = (sonar_alt - 300) / 300;
+		scale = constrain(scale, 0, 1);
+
+		current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
+
+	}else{
+		baro_alt 		= read_barometer();
+		// no sonar altitude
+		current_loc.alt = baro_alt + home.alt;
+	}
+
+	#endif
+}
+
+// updated at 10hz
+void update_alt2()
+{
+	altitude_sensor = BARO;
+
+	#if HIL_MODE == HIL_MODE_ATTITUDE
+	current_loc.alt = g_gps->altitude;
+	#else
+
 	if(g.sonar_enabled){
 		// filter out offset
 
@@ -1254,11 +1336,8 @@ void update_alt()
 			sonar_alt = temp_sonar;
 		}
 
-		//sonar_alt = sonar.read();
-
 		// decide if we're using sonar
 		if ((baro_alt < 1200) || sonar_alt < 300){
-		//if (baro_alt < 700){
 			// correct alt for angle of the sonar
 			float temp = cos_pitch_x * cos_roll_x;
 			temp = max(temp, 0.707);
@@ -1285,7 +1364,7 @@ void update_alt()
 	}
 
 	//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
-#endif
+	#endif
 }
 
 void
@@ -1337,19 +1416,25 @@ void tuning(){
 
 	#elif CHANNEL_6_TUNING == CH6_PMAX
 		g.pitch_max.set(g.rc_6.control_in * 2);  // 0 to 2000
-		
+
+	#elif CHANNEL_6_TUNING == CH6_DCM_RP
+		dcm.kp_roll_pitch((float)g.rc_6.control_in / 5000.0);
+
+	#elif CHANNEL_6_TUNING == CH6_DCM_Y
+		dcm.kp_yaw((float)g.rc_6.control_in / 1000.0);
+
 	#elif CHANNEL_6_TUNING == CH6_YAW_KP
 	    // yaw heading
 		g.pid_yaw.kP((float)g.rc_6.control_in / 200.0);  // range from 0.0 ~ 5.0
-		
+
 	#elif CHANNEL_6_TUNING == CH6_YAW_KD
 	    // yaw heading
 		g.pid_yaw.kD((float)g.rc_6.control_in / 1000.0);
-	
+
 	#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
 	    // yaw rate
 		g.pid_acro_rate_yaw.kP((float)g.rc_6.control_in / 1000.0);
-		
+
 	#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KD
 	    // yaw rate
 		g.pid_acro_rate_yaw.kD((float)g.rc_6.control_in / 1000.0);
@@ -1388,4 +1473,21 @@ void update_nav_yaw()
 	}else if(yaw_tracking == MAV_ROI_WPNEXT){
 		nav_yaw = target_bearing;
 	}
-}
+}
+
+void check_DCM()
+{
+	#if DYNAMIC_DRIFT == 1
+	if(g.rc_1.control_in == 0 && g.rc_2.control_in == 0){
+		if(g.rc_3.control_in < (g.throttle_cruise + 20)){
+			//dcm.kp_roll_pitch(dcm.kDCM_kp_rp_high);
+			dcm.kp_roll_pitch(0.15);
+		}
+	}else{
+		dcm.kp_roll_pitch(0.05967);
+	}
+	#endif
+}
+
+
+

ArduCopterMega/Attitude.pde

@@ -30,8 +30,7 @@ limit_nav_pitch_roll(long pmax)
 void
 output_stabilize_roll()
 {
-	float error;//, rate;
+	float error;
-	//int dampener;
 
 	error 		= g.rc_1.servo_out - dcm.roll_sensor;
 
@@ -52,8 +51,7 @@ output_stabilize_roll()
 void
 output_stabilize_pitch()
 {
-	float error, rate;
+	float error;
-	int dampener;
 
 	error		= g.rc_2.servo_out - dcm.pitch_sensor;
 
@@ -113,6 +111,7 @@ throttle control
 void output_manual_throttle()
 {
 	g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
+	g.rc_3.servo_out = max(g.rc_3.servo_out, 0);
 }
 
 // Autopilot
@@ -125,6 +124,25 @@ void output_auto_throttle()
 }
 
 void calc_nav_throttle()
+{
+	// limit error
+	nav_throttle = g.pid_baro_throttle.get_pid(altitude_error, delta_ms_medium_loop, 1.0);
+	nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 60);
+
+	// simple filtering
+	if(nav_throttle_old == 0)
+		nav_throttle_old = nav_throttle;
+
+	nav_throttle 		= (nav_throttle + nav_throttle_old) >> 1;
+	nav_throttle_old 	= nav_throttle;
+
+	// clear the new data flag
+	invalid_throttle = false;
+
+	//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
+}
+
+void calc_nav_throttle2()
 {
 	// limit error
 	long error = constrain(altitude_error, -400, 400);
@@ -155,7 +173,6 @@ void calc_nav_throttle()
 
 	//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
 }
-
 float angle_boost()
 {
 	float temp = cos_pitch_x * cos_roll_x;
@@ -205,7 +222,7 @@ output_yaw_with_hold(boolean hold)
 		// look to see if we have exited rate control properly - ie stopped turning
 		if(rate_yaw_flag){
 			// we are still in motion from rate control
-			if(fabs(omega.z) < .2){
+			if((fabs(omega.z) < .25) || (brake_timer < 2)){
 				clear_yaw_control();
 				hold = true;			// just to be explicit
 			}else{
@@ -223,12 +240,15 @@ output_yaw_with_hold(boolean hold)
 	}
 
 	if(hold){
+		brake_timer 			= 0;
 		// try and hold the current nav_yaw setting
 		yaw_error				= nav_yaw - dcm.yaw_sensor; 									// +- 60°
+
+		// we need to wrap our value so we can be -180 to 180 (*100)
 		yaw_error 				= wrap_180(yaw_error);
 
 		// limit the error we're feeding to the PID
-		yaw_error				= constrain(yaw_error,	 -4000, 4000);						// limit error to 40 degees
+		yaw_error				= constrain(yaw_error,	 -9000, 9000);						// limit error to 40 degees
 
 		// Apply PID and save the new angle back to RC_Channel
 		g.rc_4.servo_out 		= g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); 		// .4 * 4000 = 1600
@@ -241,25 +261,23 @@ output_yaw_with_hold(boolean hold)
 	}else{
 
 		if(g.rc_4.control_in == 0){
-
+			brake_timer--;
 			// adaptive braking
-			g.rc_4.servo_out 	= (int)(-1000.0 * omega.z);
+			g.rc_4.servo_out 	= (int)(-1200.0 * omega.z);
 
 			yaw_debug 			= YAW_BRAKE;  // 1
 
 		}else{
 			// RATE control
-																// Hein, 5/21/11
+			brake_timer 		= 100;
-			long error			= ((long)g.rc_4.control_in * 6) - (rate * 2);					// control is += 6000 * 6 = 36000
-			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);	// kP .07 * 36000 = 2520
 			yaw_debug 			= YAW_RATE;  // 2
-
+			long error			= ((long)g.rc_4.control_in * 6) - (degrees(omega.z) * 100);		// control is += 4500 * 6 = 36000
-			//nav_yaw 			= dcm.yaw_sensor;	// I think this caused the free rotation, dont know why.
+			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);	// kP .07 * 36000 = 2520
 		}
 	}
 
 	// Limit Output
-	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -2500, 2500);								// limit to 24°
+	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -3200, 3200);								// limit to 32°
 
 	//Serial.printf("%d\n",g.rc_4.servo_out);
 }
@@ -271,7 +289,8 @@ output_yaw_with_hold(boolean hold)
 	// re-define nav_yaw if we have stick input
 	if(g.rc_4.control_in != 0){
 		// set nav_yaw + or - the current location
-		nav_yaw 	= (long)g.rc_4.control_in + dcm.yaw_sensor;
+		//nav_yaw 	= (long)g.rc_4.control_in + dcm.yaw_sensor;
+		nav_yaw 	+= (long)(g.rc_4.control_in / 90);
 	}
 
 	// we need to wrap our value so we can be 0 to 360 (*100)
@@ -290,7 +309,40 @@ output_yaw_with_hold(boolean hold)
 	g.rc_4.servo_out 		= g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); 		// .4 * 4000 = 1600
 
 	// add in yaw dampener
-	g.rc_4.servo_out		-= degrees(omega.z) * 100 * g.pid_yaw.kD();
+	g.rc_4.servo_out		-= (degrees(omega.z) * 100) * g.pid_yaw.kD();
 	g.rc_4.servo_out		 = constrain(g.rc_4.servo_out,	 -2500, 2500);						// limit error to 60 degees
 }
+
+#elif YAW_OPTION == 2
+
+void
+output_yaw_with_hold(boolean hold)
+{
+	if(hold){
+		// try and hold the current nav_yaw setting
+		yaw_error				= nav_yaw - dcm.yaw_sensor; 									// +- 60°
+
+		// we need to wrap our value so we can be -180 to 180 (*100)
+		yaw_error 	= wrap_180(yaw_error);
+
+		// limit the error we're feeding to the PID
+		yaw_error				= constrain(yaw_error,	 -3500, 3500);						// limit error to 40 degees
+
+		// Apply PID and save the new angle back to RC_Channel
+		g.rc_4.servo_out 		= g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); 		// .4 * 4000 = 1600
+
+		// add in yaw dampener
+		g.rc_4.servo_out		-= (degrees(omega.z) * 100) * g.pid_yaw.kD();
+
+	}else{
+		// RATE control
+		long error				= ((long)g.rc_4.control_in * 6) - (degrees(omega.z) * 100);		// control is += 4500 * 6 = 36000
+		g.rc_4.servo_out 		= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);	// kP .07 * 36000 = 2520
+		nav_yaw 				= dcm.yaw_sensor;	// save our Yaw
+	}
+
+	// Limit Output
+	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -2500, 2500);								// limit to 24°
+}
+
 #endif

ArduCopterMega/Log.pde

@@ -3,9 +3,9 @@
 // Code to Write and Read packets from DataFlash log memory
 // Code to interact with the user to dump or erase logs
 
-#define HEAD_BYTE1 0xA3    // Decimal 163
+#define HEAD_BYTE1 	0xA3	// Decimal 163
-#define HEAD_BYTE2 0x95    // Decimal 149
+#define HEAD_BYTE2 	0x95	// Decimal 149
-#define END_BYTE 0xBA      // Decimal 186
+#define END_BYTE	0xBA	// Decimal 186
 
 
 // These are function definitions so the Menu can be constructed before the functions
@@ -65,6 +65,7 @@ print_log_menu(void)
 		if (g.log_bitmask & MASK_LOG_RAW)			Serial.printf_P(PSTR(" RAW"));
 		if (g.log_bitmask & MASK_LOG_CMD)			Serial.printf_P(PSTR(" CMD"));
 		if (g.log_bitmask & MASK_LOG_CURRENT)		Serial.printf_P(PSTR(" CURRENT"));
+		if (g.log_bitmask & MASK_LOG_MOTORS)		Serial.printf_P(PSTR(" MOTORS"));
 	}
 
 	Serial.println();
@@ -101,11 +102,10 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 		Log_Read(1, 4096);
 	}
 
-	if (/*(argc != 2) || */(dump_log < 1) || (dump_log > last_log_num)) {
+	if (/*(argc != 2) || */ (dump_log < 1) || (dump_log > last_log_num)) {
 		Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
+		Log_Read(1, 4095);
 		return(-1);
-		//Log_Read(1, 4095);
-		return (0);
 	}
 
 	get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
@@ -174,6 +174,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
 		TARG(RAW);
 		TARG(CMD);
 		TARG(CURRENT);
+		TARG(MOTORS);
 		#undef TARG
 	}
 
@@ -191,6 +192,7 @@ int8_t
 process_logs(uint8_t argc, const Menu::arg *argv)
 {
 	log_menu.run();
+	return 0;
 }
 
 
@@ -453,6 +455,30 @@ void Log_Read_Current()
 			DataFlash.ReadInt());
 }
 
+void Log_Write_Motors()
+{
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_MOTORS_MSG);
+
+	DataFlash.WriteInt(motor_out[CH_1]);
+	DataFlash.WriteInt(motor_out[CH_2]);
+	DataFlash.WriteInt(motor_out[CH_3]);
+	DataFlash.WriteInt(motor_out[CH_4]);
+
+	DataFlash.WriteByte(END_BYTE);
+}
+
+// Read a Current packet
+void Log_Read_Motors()
+{
+	Serial.printf_P(PSTR("MOT: %d, %d, %d, %d\n"),
+			DataFlash.ReadInt(),
+			DataFlash.ReadInt(),
+			DataFlash.ReadInt(),
+			DataFlash.ReadInt());
+}
+
 void Log_Write_Nav_Tuning()
 {
 	Matrix3f tempmat = dcm.get_dcm_matrix();
@@ -512,13 +538,13 @@ void Log_Write_Control_Tuning()
 	DataFlash.WriteInt((int)(omega.z * 100));
 
 	// Alt hold
-	DataFlash.WriteInt((int)(g.rc_3.servo_out));
+	DataFlash.WriteInt(g.rc_3.servo_out);
-	DataFlash.WriteInt((int)sonar_alt);					//
+	DataFlash.WriteInt(sonar_alt);					//
-	DataFlash.WriteInt((int)baro_alt);					//
+	DataFlash.WriteInt(baro_alt);					//
 
 	DataFlash.WriteInt((int)next_WP.alt);				//
 	DataFlash.WriteInt((int)altitude_error);			//
-	DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator());
+	DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator());
 
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -559,6 +585,11 @@ void Log_Write_Performance()
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
 
+	//DataFlash.WriteByte(	delta_ms_fast_loop);
+	//DataFlash.WriteByte(	loop_step);
+
+
+	//*
 	DataFlash.WriteLong(	millis()- perf_mon_timer);
 	DataFlash.WriteInt(		mainLoop_count);
 	DataFlash.WriteInt(		G_Dt_max);
@@ -568,6 +599,7 @@ void Log_Write_Performance()
 	DataFlash.WriteByte(	dcm.renorm_blowup_count);
 	DataFlash.WriteByte(	gps_fix_count);
 	DataFlash.WriteInt(		(int)(dcm.get_health() * 1000));
+	//*/
 	//PM,	20005,	3742, 10,0,0,0,0,89,1000,
 
 	DataFlash.WriteByte(END_BYTE);
@@ -576,6 +608,7 @@ void Log_Write_Performance()
 // Read a performance packet
 void Log_Read_Performance()
 {
+	//*
 	long pm_time;
 	int logvar;
 
@@ -594,6 +627,13 @@ void Log_Read_Performance()
 		Serial.print(comma);
 	}
 	Serial.println(" ");
+	//*/
+
+	/*
+	Serial.printf_P(PSTR("PM, %d, %d\n"),
+			DataFlash.ReadByte(),
+			DataFlash.ReadByte());
+	//*/
 }
 
 // Write a command processing packet.
@@ -668,7 +708,7 @@ void Log_Write_Mode(byte mode)
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_MODE_MSG);
 	DataFlash.WriteByte(mode);
-	DataFlash.WriteInt((int)g.throttle_cruise);
+	DataFlash.WriteInt(g.throttle_cruise);
 	DataFlash.WriteByte(END_BYTE);
 }
 
@@ -693,6 +733,22 @@ void Log_Read_Raw()
 	Serial.println(" ");
 }
 
+
+void Log_Write_Startup()
+{
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_STARTUP_MSG);
+	DataFlash.WriteByte(END_BYTE);
+}
+
+// Read a mode packet
+void Log_Read_Startup()
+{
+	Serial.printf_P(PSTR("START UP\n"));
+}
+
+
 // Read the DataFlash log memory : Packet Parser
 void Log_Read(int start_page, int end_page)
 {
@@ -722,64 +778,56 @@ void Log_Read(int start_page, int end_page)
 				break;
 
 			case 2:
-				if(data == LOG_ATTITUDE_MSG){
+				log_step = 0;
-					Log_Read_Attitude();
+				switch(data){
-					log_step++;
+					case LOG_ATTITUDE_MSG:
+						Log_Read_Attitude();
+						break;
 
-				}else if(data == LOG_MODE_MSG){
+					case LOG_MODE_MSG:
-					Log_Read_Mode();
+						Log_Read_Mode();
-					log_step++;
+						break;
 
-				}else if(data == LOG_CONTROL_TUNING_MSG){
+					case LOG_CONTROL_TUNING_MSG:
-					Log_Read_Control_Tuning();
+						Log_Read_Control_Tuning();
-					log_step++;
+						break;
 
-				}else if(data == LOG_NAV_TUNING_MSG){
+					case LOG_NAV_TUNING_MSG:
-					Log_Read_Nav_Tuning();
+						Log_Read_Nav_Tuning();
-					log_step++;
+						break;
 
-				}else if(data == LOG_PERFORMANCE_MSG){
+					case LOG_PERFORMANCE_MSG:
-					Log_Read_Performance();
+						Log_Read_Performance();
-					log_step++;
+						break;
 
-				}else if(data == LOG_RAW_MSG){
+					case LOG_RAW_MSG:
-					Log_Read_Raw();
+						Log_Read_Raw();
-					log_step++;
+						break;
 
-				}else if(data == LOG_CMD_MSG){
+					case LOG_CMD_MSG:
-					Log_Read_Cmd();
+						Log_Read_Cmd();
-					log_step++;
+						break;
 
-				}else if(data == LOG_CURRENT_MSG){
+					case LOG_CURRENT_MSG:
-					Log_Read_Current();
+						Log_Read_Current();
-					log_step++;
+						break;
 
-				}else if(data == LOG_STARTUP_MSG){
+					case LOG_STARTUP_MSG:
-					//Log_Read_Startup();
+						Log_Read_Startup();
-					log_step++;
+						break;
-				}else {
+
-					if(data == LOG_GPS_MSG){
+					case LOG_MOTORS_MSG:
+						Log_Read_Motors();
+						break;
+
+					case LOG_GPS_MSG:
 						Log_Read_GPS();
-						log_step++;
+						break;
-					}else{
-						Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
-						log_step = 0;	 // Restart, we have a problem...
-					}
 				}
 				break;
-			case 3:
-				if(data == END_BYTE){
-					 packet_count++;
-				}else{
-					Serial.printf_P(PSTR("Error EB: %d\n"),data);
-				}
-				log_step = 0;			// Restart sequence: new packet...
-				break;
 		}
-
 		page = DataFlash.GetPage();
 	}
-	//Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
 }
 
 

ArduCopterMega/Mavlink_Common.h

@@ -221,7 +221,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
 					(float)airspeed / 100.0,
 					(float)g_gps->ground_speed / 100.0,
 					(dcm.yaw_sensor / 100) % 360,
-					(int)g.rc_3.servo_out,
+					g.rc_3.servo_out,
 					current_loc.alt / 100.0,
 					climb_rate);
 			break;

ArduCopterMega/Parameters.h

@@ -17,7 +17,14 @@ public:
     // The increment will prevent old parameters from being used incorrectly
     // by newer code.
     //
-    static const uint16_t k_format_version = 9;
+    static const uint16_t k_format_version = 101;
+
+	// The parameter software_type is set up solely for ground station use
+	// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
+	// GCS will interpret values 0-9 as ArduPilotMega.  Developers may use
+	// values within that range to identify different branches.
+	//
+    static const uint16_t k_software_type = 10;		// 0 for APM trunk
 
     //
     // Parameter identities.
@@ -43,6 +50,7 @@ public:
         // Layout version number, always key zero.
         //
         k_param_format_version = 0,
+		k_param_software_type,
 
 
         // Misc
@@ -53,13 +61,13 @@ public:
 		//
 		k_param_streamrates_port0 = 110,
 		k_param_streamrates_port3,
+		k_param_sysid_this_mav,
+		k_param_sysid_my_gcs,
 
         //
         // 140: Sensor parameters
         //
         k_param_IMU_calibration = 140,
-        k_param_ground_temperature,
-        k_param_ground_pressure,
 		k_param_battery_monitoring,
 		k_param_pack_capacity,
 		k_param_compass_enabled,
@@ -97,6 +105,7 @@ public:
         k_param_flight_modes,
         k_param_esc_calibrate,
 
+ 	   #if FRAME_CONFIG ==	HELI_FRAME
 		//
 		// 200: Heli
 		//
@@ -114,6 +123,7 @@ public:
 		k_param_heli_collective_mid,
 		k_param_heli_ext_gyro_enabled,
 		k_param_heli_ext_gyro_gain,  // 213
+		#endif
 
         //
         // 220: Waypoint data
@@ -149,6 +159,12 @@ public:
     };
 
     AP_Int16    format_version;
+	AP_Int8		software_type;
+
+	// Telemetry control
+	//
+	AP_Int16		sysid_this_mav;
+	AP_Int16		sysid_my_gcs;
 
 
     // Crosstrack navigation
@@ -190,8 +206,6 @@ public:
     // Misc
     //
     AP_Int16    log_bitmask;
-    AP_Int16    ground_temperature;
-    AP_Int16    ground_pressure;
     AP_Int16    RTL_altitude;
 
     AP_Int8		sonar_enabled;
@@ -201,6 +215,7 @@ public:
 	AP_Int8		esc_calibrate;
 	AP_Int8		frame_orientation;
 
+    #if FRAME_CONFIG ==	HELI_FRAME
 	// Heli
 	RC_Channel	heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4;  // servos for swash plate and tail
 	AP_Int16	heli_servo1_pos, heli_servo2_pos, heli_servo3_pos;       // servo positions (3 because we don't need pos for tail servo)
@@ -208,7 +223,8 @@ public:
 	AP_Int16	heli_coll_min, heli_coll_max, heli_coll_mid;    // min and max collective.  mid = main blades at zero pitch
 	AP_Int8		heli_ext_gyro_enabled;   // 0 = no external tail gyro, 1 = external tail gyro
 	AP_Int16	heli_ext_gyro_gain;      // radio output 1000~2000 (value output on CH_7)
-	
+	#endif
+
     // RC channels
 	RC_Channel	rc_1;
 	RC_Channel	rc_2;
@@ -236,10 +252,15 @@ public:
 
     uint8_t     junk;
 
+    // Note: keep initializers here in the same order as they are declared above.
     Parameters() :
         // variable             default                     key										name
         //-------------------------------------------------------------------------------------------------------------------
-        format_version          (k_format_version,          k_param_format_version,        			NULL),
+        format_version          (k_format_version,          k_param_format_version,         		PSTR("SYSID_SW_MREV")),
+        software_type			(k_software_type,			k_param_software_type,         			PSTR("SYSID_SW_TYPE")),
+
+        sysid_this_mav			(MAV_SYSTEM_ID,				k_param_sysid_this_mav,					PSTR("SYSID_THISMAV")),
+        sysid_my_gcs			(255,		                k_param_sysid_my_gcs,					PSTR("SYSID_MYGCS")),
 
         crosstrack_gain         (XTRACK_GAIN * 100,			k_param_crosstrack_gain,        		PSTR("XTRK_GAIN")),
         crosstrack_entry_angle  (XTRACK_ENTRY_ANGLE * 100,	k_param_crosstrack_entry_angle, 		PSTR("XTRACK_ANGLE")),
@@ -268,20 +289,19 @@ public:
         pitch_max         		(PITCH_MAX * 100,			k_param_pitch_max,		       			PSTR("PITCH_MAX")),
 
         log_bitmask             (MASK_LOG_SET_DEFAULTS,		k_param_log_bitmask,            		PSTR("LOG_BITMASK")),
-        ground_temperature      (0,                         k_param_ground_temperature,    			PSTR("GND_TEMP")),
-        ground_pressure         (0,                         k_param_ground_pressure,       			PSTR("GND_ABS_PRESS")),
         RTL_altitude            (ALT_HOLD_HOME * 100,		k_param_RTL_altitude,          			PSTR("ALT_HOLD_RTL")),
         esc_calibrate 			(0, 						k_param_esc_calibrate, 					PSTR("ESC")),
 
         frame_orientation 		(FRAME_ORIENTATION, 		k_param_frame_orientation, 				PSTR("FRAME")),
 
+	    #if FRAME_CONFIG ==	HELI_FRAME
 		heli_servo_1			(k_param_heli_servo_1,		PSTR("HS1_")),
 		heli_servo_2			(k_param_heli_servo_2,		PSTR("HS2_")),
 		heli_servo_3			(k_param_heli_servo_3,		PSTR("HS3_")),
 		heli_servo_4			(k_param_heli_servo_4,		PSTR("HS4_")),
 		heli_servo1_pos			(-60,						k_param_heli_servo1_pos,				PSTR("SV1_POS_")),
 		heli_servo2_pos			(60,						k_param_heli_servo2_pos,				PSTR("SV2_POS_")),
-		heli_servo3_pos			(180,						k_param_heli_servo3_pos,				PSTR("SV3_POS_")),		
+		heli_servo3_pos			(180,						k_param_heli_servo3_pos,				PSTR("SV3_POS_")),
 		heli_roll_max			(4500,						k_param_heli_roll_max,					PSTR("ROL_MAX_")),
 		heli_pitch_max			(4500,						k_param_heli_pitch_max,					PSTR("PIT_MAX_")),
 		heli_coll_min			(1000,						k_param_heli_collective_min,			PSTR("COL_MIN_")),
@@ -289,7 +309,8 @@ public:
 		heli_coll_mid			(1500,						k_param_heli_collective_mid,			PSTR("COL_MID_")),
 		heli_ext_gyro_enabled	(0,							k_param_heli_ext_gyro_enabled,			PSTR("GYR_ENABLE_")),
 		heli_ext_gyro_gain		(1000,						k_param_heli_ext_gyro_gain,				PSTR("GYR_GAIN_")),
-		
+		#endif
+
         // RC channel           group key                   name
         //----------------------------------------------------------------------
         rc_1					(k_param_rc_1,		PSTR("RC1_")),

ArduCopterMega/commands.pde

@@ -224,7 +224,8 @@ void init_home()
 	home.id 	= MAV_CMD_NAV_WAYPOINT;
 	home.lng 	= g_gps->longitude;				// Lon * 10**7
 	home.lat 	= g_gps->latitude;				// Lat * 10**7
-	home.alt 	= max(g_gps->altitude, 0);		// we sometimes get negatives from GPS, not valid
+	//home.alt 	= max(g_gps->altitude, 0);		// we sometimes get negatives from GPS, not valid
+	home.alt 	= 0;							// this is a test
 	home_is_set = true;
 
 	// to point yaw towards home until we set it with Mavlink

ArduCopterMega/config.h

@@ -49,10 +49,15 @@
 //////////////////////////////////////////////////////////////////////////////
 // Sonar
 //
+
 #ifndef SONAR_PIN
 # define SONAR_PIN		AP_RANGEFINDER_PITOT_TUBE
 #endif
 
+#ifndef SONAR_TYPE
+# define SONAR_TYPE		MAX_SONAR_XL
+#endif
+
 
 //////////////////////////////////////////////////////////////////////////////
 // AIRSPEED_SENSOR
@@ -352,7 +357,7 @@
 # define YAW_P					0.4			// increase for more aggressive Yaw Hold, decrease if it's bouncy
 #endif
 #ifndef  YAW_I
-# define YAW_I					0.01		// set to .0001 to try and get over user's steady state error caused by poor balance
+# define YAW_I					0.005		// set to .0001 to try and get over user's steady state error caused by poor balance
 #endif
 #ifndef  YAW_D
 # define YAW_D					0.08		// .7 = almost no yaw	Trying a lower value to prevent odd behavior
@@ -366,7 +371,7 @@
 //
 // how much to we pitch towards the target
 #ifndef PITCH_MAX
-# define PITCH_MAX				24			// degrees
+# define PITCH_MAX				16			// degrees
 #endif
 
 
@@ -404,7 +409,7 @@
 	# define NAV_WP_P				4.0
 	#endif
 	#ifndef NAV_WP_I
-	# define NAV_WP_I				0.15			// leave 0
+	# define NAV_WP_I				0.15		//
 	#endif
 	#ifndef NAV_WP_D
 	# define NAV_WP_D				10			// not sure about at all
@@ -419,13 +424,13 @@
 // Throttle control gains
 //
 #ifndef THROTTLE_BARO_P
-# define THROTTLE_BARO_P		0.2			// trying a lower val
+# define THROTTLE_BARO_P		0.35			// trying a lower val
 #endif
 #ifndef THROTTLE_BARO_I
-# define THROTTLE_BARO_I		0.02		//with 4m error, 12.5s windup
+# define THROTTLE_BARO_I		0.05		//with 4m error, 12.5s windup
 #endif
 #ifndef THROTTLE_BARO_D
-# define THROTTLE_BARO_D		0.06		//
+# define THROTTLE_BARO_D		0.3		// upped with filter
 #endif
 #ifndef THROTTLE_BARO_IMAX
 # define THROTTLE_BARO_IMAX		30

ArduCopterMega/control_modes.pde

@@ -51,37 +51,45 @@ void read_trim_switch()
 {
 	// switch is engaged
 	if (g.rc_7.control_in > 800){
-		if(trim_flag == false){
-			// called once
-			trim_timer = millis();
-		}
-
 		trim_flag = true;
-		//trim_accel();
 
 	}else{ // switch is disengaged
 
 		if(trim_flag){
-			// switch was just released
+			// set the throttle nominal
-			if((millis() - trim_timer) > 2000){
+			if(g.rc_3.control_in > 150){
-				#if HIL_MODE != HIL_MODE_ATTITUDE
+				g.throttle_cruise.set_and_save(g.rc_3.control_in);
-				imu.save();
-				#endif
-			}else{
-				// set the throttle nominal
-				if(g.rc_3.control_in > 150){
-					g.throttle_cruise.set_and_save(g.rc_3.control_in);
 					//Serial.printf("tnom %d\n", g.throttle_cruise.get());
-				}
 			}
 			trim_flag = false;
 		}
 	}
 }
 
+void auto_trim()
+{
+	if(auto_level_counter > 0){
+		auto_level_counter--;
+		trim_accel();
+		led_mode = AUTO_TRIM_LEDS;
+
+		if(auto_level_counter == 1){
+			led_mode = NORMAL_LEDS;
+			clear_leds();
+			imu.save();
+			Serial.println("Done");
+			auto_level_counter = 0;
+		}
+	}
+}
+
+
 
 void trim_accel()
 {
+	g.pid_stabilize_roll.reset_I();
+	g.pid_stabilize_pitch.reset_I();
+
 	if(g.rc_1.control_in > 0){
 		imu.ay(imu.ay() + 1);
 	}else if (g.rc_1.control_in < 0){
@@ -94,11 +102,13 @@ void trim_accel()
 		imu.ax(imu.ax() - 1);
 	}
 
-	/*Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"),
+	/*
+	Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"),
 							dcm.roll_sensor,
 							dcm.pitch_sensor,
-							(double)imu.ax(),
+							(float)imu.ax(),
-							(double)imu.ay(),
+							(float)imu.ay(),
-							(double)imu.az());*/
+							(float)imu.az());
+	//*/
 }
 

ArduCopterMega/defines.h

@@ -25,6 +25,11 @@
 #define PLUS_FRAME 0
 #define X_FRAME 1
 
+
+// LED output
+#define NORMAL_LEDS 0
+#define AUTO_TRIM_LEDS 1
+
 // Internal defines, don't edit and expect things to work
 // -------------------------------------------------------
 
@@ -50,6 +55,11 @@
 #define GPS_PROTOCOL_MTK16	6
 #define GPS_PROTOCOL_AUTO	7
 
+// SONAR types:
+
+#define MAX_SONAR_XL 0
+#define MAX_SONAR_LV 1
+
 // Radio channels
 // Note channels are from 0!
 //
@@ -121,10 +131,12 @@
 #define CH6_SONAR_KD 6
 #define CH6_Y6_SCALING 7
 #define CH6_PMAX 8
-#define	CH6_YAW_KP 9
+#define CH6_DCM_RP 9
-#define	CH6_YAW_KD 10
+#define CH6_DCM_Y 10
-#define	CH6_YAW_RATE_KP 11
+#define	CH6_YAW_KP 11
-#define	CH6_YAW_RATE_KD 12
+#define	CH6_YAW_KD 12
+#define	CH6_YAW_RATE_KP 13
+#define	CH6_YAW_RATE_KD 14
 
 // nav byte mask
 // -------------
@@ -225,7 +237,8 @@
 #define SEVERITY_CRITICAL 4
 
 //  Logging parameters
-#define LOG_INDEX_MSG			0xF0
+#define TYPE_AIRSTART_MSG		0x00
+#define TYPE_GROUNDSTART_MSG	0x01
 #define LOG_ATTITUDE_MSG		0x01
 #define LOG_GPS_MSG				0x02
 #define LOG_MODE_MSG			0X03
@@ -236,8 +249,8 @@
 #define LOG_CMD_MSG				0x08
 #define LOG_CURRENT_MSG 		0x09
 #define LOG_STARTUP_MSG 		0x0A
-#define TYPE_AIRSTART_MSG		0x00
+#define LOG_MOTORS_MSG 			0x0B
-#define TYPE_GROUNDSTART_MSG	0x01
+#define LOG_INDEX_MSG			0xF0
 #define MAX_NUM_LOGS			50
 
 #define MASK_LOG_ATTITUDE_FAST 	(1<<0)
@@ -250,6 +263,7 @@
 #define MASK_LOG_RAW			(1<<7)
 #define MASK_LOG_CMD			(1<<8)
 #define MASK_LOG_CURRENT		(1<<9)
+#define MASK_LOG_MOTORS			(1<<10)
 #define MASK_LOG_SET_DEFAULTS	(1<<15)
 
 // Waypoint Modes
@@ -276,6 +290,7 @@
 
 #define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
 #define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT
+#define BARO_FILTER_SIZE 6
 
 /* ************************************************************** */
 /* Expansion PIN's that people can use for various things. */
@@ -319,6 +334,11 @@
 #define PIEZO_PIN AN5           //Last pin on the back ADC connector
 
 
+#define SONAR_PORT AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL
+
+// sonar
+#define SonarToCm(x) (x*1.26)   // Sonar raw value to centimeters
+
 // Hardware Parameters
 #define SLIDE_SWITCH_PIN 40
 #define PUSHBUTTON_PIN 41

ArduCopterMega/leds.pde

@@ -0,0 +1,110 @@
+void update_lights()
+{
+	switch(led_mode){
+		case NORMAL_LEDS:
+			update_motor_light();
+			update_GPS_light();
+			break;
+
+		case AUTO_TRIM_LEDS:
+			dancing_light();
+			break;
+	}
+}
+
+void update_GPS_light(void)
+{
+	// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
+	// ---------------------------------------------------------------------
+	switch (g_gps->status()){
+
+		case(2):
+			digitalWrite(C_LED_PIN, HIGH);  //Turn LED C on when gps has valid fix.
+			break;
+
+		case(1):
+			if (g_gps->valid_read == true){
+				GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
+				if (GPS_light){
+					digitalWrite(C_LED_PIN, LOW);
+				}else{
+					digitalWrite(C_LED_PIN, HIGH);
+				}
+				g_gps->valid_read = false;
+			}
+			break;
+
+		default:
+			digitalWrite(C_LED_PIN, LOW);
+			break;
+	}
+}
+
+void update_motor_light(void)
+{
+	if(motor_armed == false){
+		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);
+		}
+	}
+}
+
+void dancing_light()
+{
+	static byte step;
+
+	if (step++ == 3)
+		step = 0;
+
+	switch(step)
+	{
+		case 0:
+			digitalWrite(C_LED_PIN, LOW);
+			digitalWrite(A_LED_PIN, HIGH);
+			break;
+
+		case 1:
+			digitalWrite(A_LED_PIN, LOW);
+			digitalWrite(B_LED_PIN, HIGH);
+			break;
+
+		case 2:
+			digitalWrite(B_LED_PIN, LOW);
+			digitalWrite(C_LED_PIN, HIGH);
+			break;
+	}
+}
+void clear_leds()
+{
+	digitalWrite(A_LED_PIN, LOW);
+	digitalWrite(B_LED_PIN, LOW);
+	digitalWrite(C_LED_PIN, LOW);
+}
+
+#if MOTOR_LEDS == 1
+void update_motor_leds(void)
+{
+	// blink rear
+	static bool blink;
+
+	if (blink){
+		blink = false;
+		digitalWrite(RE_LED, LOW);
+
+	}else{
+		blink = true;
+		digitalWrite(RE_LED, HIGH);
+	}
+}
+#endif
+

ArduCopterMega/motors.pde

@@ -1,17 +1,27 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define ARM_DELAY 10
+#define ARM_DELAY 10	// one secon
-#define DISARM_DELAY 10
+#define DISARM_DELAY 10	// one secon
+#define LEVEL_DELAY 120 // twelve seconds
+#define AUTO_LEVEL_DELAY 250 // twentyfive seconds
 
+
+// called at 10hz
 void arm_motors()
 {
-	static byte arming_counter;
+	static int arming_counter;
 
 	// Arm motor output : Throttle down and full yaw right for more than 2 seconds
 	if (g.rc_3.control_in == 0){
+
 		// full right
 		if (g.rc_4.control_in > 4000) {
-			if (arming_counter >= ARM_DELAY) {
+
+			if (arming_counter > AUTO_LEVEL_DELAY){
+				auto_level_counter = 255;
+				arming_counter = 0;
+
+			}else if (arming_counter == ARM_DELAY){
 				motor_armed 	= true;
 				arming_counter 	= ARM_DELAY;
 
@@ -19,16 +29,24 @@ void arm_motors()
 				// ---------------------------
 				init_simple_bearing();
 
+				arming_counter++;
 			} else{
 				arming_counter++;
 			}
 
 		// full left
 		}else if (g.rc_4.control_in < -4000) {
-			if (arming_counter >= DISARM_DELAY){
+			//Serial.print(arming_counter, DEC);
+			if (arming_counter > LEVEL_DELAY){
+				//Serial.print("init");
+				imu.init_accel();
+				arming_counter = 0;
+
+			}else if (arming_counter == DISARM_DELAY){
 				motor_armed 	= false;
 				arming_counter 	= DISARM_DELAY;
 				compass.save_offsets();
+				arming_counter++;
 
 			}else{
 				arming_counter++;

ArduCopterMega/radio.pde

@@ -22,7 +22,7 @@ void init_rc_in()
 	g.rc_1.dead_zone = 60;		// 60 = .6 degrees
 	g.rc_2.dead_zone = 60;
 	g.rc_3.dead_zone = 60;
-	g.rc_4.dead_zone = 300;
+	g.rc_4.dead_zone = 600;
 
 	//set auxiliary ranges
 	g.rc_5.set_range(0,1000);
@@ -106,29 +106,32 @@ void output_min()
 }
 void read_radio()
 {
-	g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
+	if (APM_RC.GetState() == 1){
-	g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
-	g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
-	g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
-	g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
-	g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
-	g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
-	g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
 
-#if FRAME_CONFIG != HELI_FRAME
+		g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
-	// limit our input to 800 so we can still pitch and roll
+		g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
-	g.rc_3.control_in = min(g.rc_3.control_in, 800);
+		g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
-#endif
+		g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
+		g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
+		g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
+		g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
+		g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
 
-	//throttle_failsafe(g.rc_3.radio_in);
+		#if FRAME_CONFIG != HELI_FRAME
+			// limit our input to 800 so we can still pitch and roll
+			g.rc_3.control_in = min(g.rc_3.control_in, 800);
+		#endif
 
-	/*
+		//throttle_failsafe(g.rc_3.radio_in);
-	Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
+
-				g.rc_1.control_in,
+		/*
-				g.rc_2.control_in,
+		Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
-				g.rc_3.control_in,
+					g.rc_1.control_in,
-				g.rc_4.control_in);
+					g.rc_2.control_in,
-	*/
+					g.rc_3.control_in,
+					g.rc_4.control_in);
+		*/
+	}
 }
 
 void throttle_failsafe(uint16_t pwm)

ArduCopterMega/sensors.pde

@@ -7,66 +7,65 @@ void ReadSCP1000(void) {}
 
 void init_barometer(void)
 {
-	int flashcount;
-
 	#if HIL_MODE == HIL_MODE_SENSORS
 		hil.update();					// look for inbound hil packets for initialization
 	#endif
 
-	ground_pressure = 0;
 
-	while(ground_pressure == 0){
+	// We take some readings...
-		barometer.Read(); 					// Get initial data from absolute pressure sensor
+	for(int i = 0; i < 200; i++){
-		ground_pressure 	= barometer.Press;
+		delay(25);
-		ground_temperature 	= barometer.Temp;
-		delay(20);
-		//Serial.printf("barometer.Press %ld\n", barometer.Press);
-	}
-
-	for(int i = 0; i < 30; i++){		// We take some readings...
 
 		#if HIL_MODE == HIL_MODE_SENSORS
 			hil.update(); 				// look for inbound hil packets
 		#endif
 
-		barometer.Read(); 				// Get initial data from absolute pressure sensor
+		// Get initial data from absolute pressure sensor
-		ground_pressure	= (ground_pressure * 9l   + barometer.Press) / 10l;
+		ground_pressure 	= read_baro_filtered();
 		ground_temperature	= (ground_temperature * 9 + barometer.Temp) / 10;
 
-		delay(20);
+	}
-		if(flashcount == 5) {
+	abs_pressure  		= ground_pressure;
-			digitalWrite(C_LED_PIN, LOW);
+	ground_temperature 	= barometer.Temp;
-			digitalWrite(A_LED_PIN, HIGH);
-			digitalWrite(B_LED_PIN, LOW);
-		}
 
-		if(flashcount >= 10) {
+	//Serial.printf("abs_pressure %ld\n", ground_temperature);
-			flashcount = 0;
+	//SendDebugln("barometer calibration complete.");
-			digitalWrite(C_LED_PIN, HIGH);
+}
-			digitalWrite(A_LED_PIN, LOW);
+
-			digitalWrite(B_LED_PIN, HIGH);
+long read_baro_filtered(void)
-		}
+{
-		flashcount++;
+	long pressure = 0;
+
+	// get new data from absolute pressure sensor
+	barometer.Read();
+
+	// add new data into our filter
+	baro_filter[baro_filter_index] = barometer.Press;
+	baro_filter_index++;
+
+	// loop our filter
+	if(baro_filter_index == BARO_FILTER_SIZE)
+		baro_filter_index = 0;
+
+	// zero out our accumulator
+
+	// sum our filter
+	for(byte i = 0; i < BARO_FILTER_SIZE; i++){
+		pressure += baro_filter[i];
 	}
 
-	// makes the filtering work later
+	// average our sampels
-	abs_pressure  = barometer.Press;
+	return pressure /= BARO_FILTER_SIZE;
-
-	// save home pressure
-	ground_pressure = abs_pressure;
-
-	//Serial.printf("abs_pressure %ld\n", abs_pressure);
-	//SendDebugln("barometer calibration complete.");
 }
 
 long read_barometer(void)
 {
  	float x, scaling, temp;
 
-	barometer.Read();		// Get new data from absolute pressure sensor
+	abs_pressure = read_baro_filtered();
+
+	//Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure);
 
-	//abs_pressure 			= (abs_pressure + barometer.Press) >> 1;		// Small filtering
-	abs_pressure 			= ((float)abs_pressure * .7) + ((float)barometer.Press * .3);		// large filtering
 	scaling 				= (float)ground_pressure / (float)abs_pressure;
 	temp 					= ((float)ground_temperature / 10.0f) + 273.15f;
 	x 						= log(scaling) * temp * 29271.267f;

ArduCopterMega/setup.pde

@@ -15,9 +15,10 @@ static int8_t	setup_mag_offset		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_declination		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_esc				(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_show				(uint8_t argc, const Menu::arg *argv);
+
 #if FRAME_CONFIG == HELI_FRAME
-static int8_t	setup_heli				(uint8_t argc, const Menu::arg *argv);
+	static int8_t	setup_heli				(uint8_t argc, const Menu::arg *argv);
-static int8_t	setup_gyro				(uint8_t argc, const Menu::arg *argv);
+	static int8_t	setup_gyro				(uint8_t argc, const Menu::arg *argv);
 #endif
 
 // Command/function table for the setup menu
@@ -37,7 +38,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"compass",			setup_compass},
 	{"offsets",			setup_mag_offset},
 	{"declination",		setup_declination},
-#if FRAME_CONFIG == HELI_FRAME	
+#if FRAME_CONFIG == HELI_FRAME
 	{"heli",			setup_heli},
 	{"gyro",			setup_gyro},
 #endif
@@ -239,7 +240,7 @@ init_esc()
 	while(1){
 		read_radio();
 		delay(100);
-		update_esc_light();
+		dancing_light();
 		APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
 		APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
 		APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
@@ -300,7 +301,9 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 {
-	byte switchPosition, _oldSwitchPosition, mode;
+	byte _switchPosition = 0;
+	byte _oldSwitchPosition = 0;
+	byte mode = 0;
 
 	Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
 	print_hit_enter();
@@ -308,20 +311,20 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 	while(1){
 		delay(20);
 		read_radio();
-		switchPosition = readSwitch();
+		_switchPosition = readSwitch();
 
 
 		// look for control switch change
-		if (_oldSwitchPosition != switchPosition){
+		if (_oldSwitchPosition != _switchPosition){
 
-			mode = g.flight_modes[switchPosition];
+			mode = g.flight_modes[_switchPosition];
 			mode = constrain(mode, 0, NUM_MODES-1);
 
 			// update the user
-			print_switch(switchPosition, mode);
+			print_switch(_switchPosition, mode);
 
 			// Remember switch position
-			_oldSwitchPosition = switchPosition;
+			_oldSwitchPosition = _switchPosition;
 		}
 
 		// look for stick input
@@ -331,10 +334,10 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 				mode = 0;
 
 			// save new mode
-			g.flight_modes[switchPosition] = mode;
+			g.flight_modes[_switchPosition] = mode;
 
 			// print new mode
-			print_switch(switchPosition, mode);
+			print_switch(_switchPosition, mode);
 		}
 
 		// escape hatch
@@ -352,6 +355,7 @@ setup_declination(uint8_t argc, const Menu::arg *argv)
 {
 	compass.set_declination(radians(argv[1].f));
 	report_compass();
+	return 0;
 }
 
 
@@ -436,13 +440,13 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 
 	// initialise swash plate
 	heli_init_swash();
-	
+
-	// source swash plate movements directly from 
+	// source swash plate movements directly from
 	heli_manual_override = true;
-	
+
 	// display initial settings
 	report_heli();
-	
+
 	// display help
 	Serial.printf_P(PSTR("Instructions:"));
 	print_divider();
@@ -455,14 +459,14 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 	Serial.printf_P(PSTR("\tr\t\treverse servo\n"));
 	Serial.printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
 	Serial.printf_P(PSTR("\tx\t\texit & save\n"));
-	
+
 	// start capturing
 	while( value != 'x' ) {
-	
+
 	    // read radio although we don't use it yet
 	    read_radio();
-		
+
-		// record min/max 
+		// record min/max
 		if( state == 1 ) {
 		    if( abs(g.rc_1.control_in) > max_roll )
 			    max_roll = abs(g.rc_1.control_in);
@@ -476,10 +480,10 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 			max_tail = max(g.rc_4.radio_in, max_tail);
 			//Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out);
 		}
-	
+
 	    if( Serial.available() ) {
 		    value = Serial.read();
-			
+
 			// process the user's input
 			switch( value ) {
 				case '1':
@@ -494,7 +498,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 				case '4':
 					active_servo = CH_4;
 					break;
-				case 'a':					
+				case 'a':
 				case 'A':
 					heli_get_servo(active_servo)->radio_trim += 10;
 					break;
@@ -515,14 +519,14 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 				    if( state == 0 ) {
 					    state = 1;  // switch to capture min/max mode
 						Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"),active_servo+1, temp);
-						
+
 						// reset servo ranges
 						g.heli_roll_max = g.heli_pitch_max = 4500;
 						g.heli_coll_min = 1000;
 						g.heli_coll_max = 2000;
 						g.heli_servo_4.radio_min = 1000;
 						g.heli_servo_4.radio_max = 2000;
-						
+
 						// set sensible values in temp variables
 						max_roll = abs(g.rc_1.control_in);
 						max_pitch = abs(g.rc_2.control_in);
@@ -541,10 +545,10 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 							g.heli_coll_max = max_coll;
 							g.heli_servo_4.radio_min = min_tail;
 							g.heli_servo_4.radio_max = max_tail;
-							
+
 							// reinitialise swash
 							heli_init_swash();
-							
+
 							// display settings
 							report_heli();
 						}
@@ -561,7 +565,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 						if( active_servo == CH_3 )
 							g.heli_servo3_pos = temp;
 						heli_init_swash();
-						Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp); 
+						Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
 					}
 					break;
 				case 'r':
@@ -576,25 +580,25 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 					if( temp > -500 && temp < 500 ) {
 					    heli_get_servo(active_servo)->radio_trim = 1500 + temp;
 						heli_init_swash();
-						Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp); 
+						Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
 					}
 					break;
-				case 'z':			
+				case 'z':
 				case 'Z':
 					heli_get_servo(active_servo)->radio_trim -= 10;
 					break;
 			}
 		}
-		
+
 		// allow swash plate to move
 		output_motors_armed();
-		
+
 		delay(20);
 	}
 
 	// display final settings
 	report_heli();
-	
+
 	// save to eeprom
 	g.heli_servo_1.save_eeprom();
 	g.heli_servo_2.save_eeprom();
@@ -608,7 +612,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 	g.heli_coll_min.save();
 	g.heli_coll_max.save();
 	g.heli_coll_mid.save();
-	
+
 	// return swash plate movements to attitude controller
 	heli_manual_override = false;
 
@@ -627,7 +631,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv)
 		    g.heli_ext_gyro_gain = argv[2].i;
 			g.heli_ext_gyro_gain.save();
 		}
-		
+
 	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
 		g.heli_ext_gyro_enabled.set_and_save(false);
 
@@ -646,6 +650,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv)
 }
 
 #endif // FRAME_CONFIG == HELI
+
 void clear_offsets()
 {
 	Vector3f _offsets;
@@ -710,7 +715,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
 			compass.set_offsets(_offsets);
 			//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
 			report_compass();
-			break;
+			return 0;
 		}
 	}
 }
@@ -959,18 +964,18 @@ void report_heli()
 {
 	Serial.printf_P(PSTR("Heli\n"));
 	print_divider();
-	
+
 	// main servo settings
 	Serial.printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
 	Serial.printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo1_pos, (int)g.heli_servo_1.radio_min, (int)g.heli_servo_1.radio_max, (int)g.heli_servo_1.get_reverse());
 	Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo2_pos, (int)g.heli_servo_2.radio_min, (int)g.heli_servo_2.radio_max, (int)g.heli_servo_2.get_reverse());
 	Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo3_pos, (int)g.heli_servo_3.radio_min, (int)g.heli_servo_3.radio_max, (int)g.heli_servo_3.get_reverse());
 	Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)g.heli_servo_4.radio_min, (int)g.heli_servo_4.radio_max, (int)g.heli_servo_4.get_reverse());
-		
+
 	Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max);
 	Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max);
 	Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max);
-		
+
 	print_blanks(2);
 }
 
@@ -983,7 +988,7 @@ void report_gyro()
 	print_enabled( g.heli_ext_gyro_enabled );
 	if( g.heli_ext_gyro_enabled )
 	    Serial.printf_P(PSTR("gain: %d"),(int)g.heli_ext_gyro_gain);
-		
+
 	print_blanks(2);
 }
 
@@ -1076,11 +1081,14 @@ radio_input_switch(void)
 
 void zero_eeprom(void)
 {
-	byte b;
+	byte b = 0;
+
 	Serial.printf_P(PSTR("\nErasing EEPROM\n"));
+
 	for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
 		eeprom_write_byte((uint8_t *) i, b);
 	}
+
 	Serial.printf_P(PSTR("done\n"));
 }
 
@@ -1111,6 +1119,8 @@ print_gyro_offsets(void)
 						(float)imu.gz());
 }
 
+#if FRAME_CONFIG == HELI_FRAME
+
 RC_Channel *
 heli_get_servo(int servo_num){
 	if( servo_num == CH_1 )
@@ -1140,6 +1150,7 @@ int read_num_from_serial() {
 			index++;
 		}
 	}while (timeout < 5 && index < 5);
-	
+
 	return atoi(data);
 }
+#endif

ArduCopterMega/system.pde

@@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.23 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.24 Beta", main_menu_commands);
 
 void init_ardupilot()
 {
@@ -87,6 +87,23 @@ void init_ardupilot()
 	//
 	report_version();
 
+
+	// setup IO pins
+	pinMode(C_LED_PIN, OUTPUT);				// GPS status LED
+	pinMode(A_LED_PIN, OUTPUT);				// GPS status LED
+	pinMode(B_LED_PIN, OUTPUT);				// GPS status LED
+	pinMode(SLIDE_SWITCH_PIN, INPUT);		// To enter interactive mode
+	pinMode(PUSHBUTTON_PIN, INPUT);			// unused
+	DDRL |= B00000100;						// Set Port L, pin 2 to output for the relay
+
+	#if MOTOR_LEDS == 1
+		pinMode(FR_LED, OUTPUT);			// GPS status LED
+		pinMode(RE_LED, OUTPUT);			// GPS status LED
+		pinMode(RI_LED, OUTPUT);			// GPS status LED
+		pinMode(LE_LED, OUTPUT);			// GPS status LED
+	#endif
+
+
 	if (!g.format_version.load() ||
 	     g.format_version != Parameters::k_format_version) {
 		//Serial.printf_P(PSTR("\n\nForcing complete parameter reset..."));
@@ -134,10 +151,10 @@ void init_ardupilot()
 		APM_RC.setHIL(rc_override);
 	}
 	#endif
-	
+
-    #if FRAME_CONFIG ==	HELI_FRAME 
+    #if FRAME_CONFIG ==	HELI_FRAME
-    heli_init_swash();  // heli initialisation
+		heli_init_swash();  // heli initialisation
-	#endif	
+	#endif
 
 	init_rc_in();		// sets up rc channels from radio
 	init_rc_out();		// sets up the timer libs
@@ -147,8 +164,8 @@ void init_ardupilot()
 	#endif
 
 	#if HIL_MODE != HIL_MODE_ATTITUDE
-	adc.Init();	 		// APM ADC library initialization
+		adc.Init();	 		// APM ADC library initialization
-	barometer.Init();	// APM Abs Pressure sensor initialization
+		barometer.Init();	// APM Abs Pressure sensor initialization
 	#endif
 
 	// Do GPS init
@@ -164,21 +181,20 @@ void init_ardupilot()
 	#endif
 
 	// init the HIL
-#if HIL_MODE != HIL_MODE_DISABLED
+	#if HIL_MODE != HIL_MODE_DISABLED
+		#if HIL_PORT == 3
+			hil.init(&Serial3);
+		#elif HIL_PORT == 1
+			hil.init(&Serial1);
+		#else
+			hil.init(&Serial);
+		#endif
+	#endif
 
-  #if HIL_PORT == 3
+	//  We may have a hil object instantiated just for mission planning
-	hil.init(&Serial3);
+	#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
-  #elif HIL_PORT == 1
+		hil.init(&Serial);
-	hil.init(&Serial1);
+	#endif
-  #else
-	hil.init(&Serial);
-  #endif
-#endif
-
-//  We may have a hil object instantiated just for mission planning
-#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
-	hil.init(&Serial);
-#endif
 
 	if(g.compass_enabled)
 		init_compass();
@@ -189,23 +205,15 @@ void init_ardupilot()
 	}
 	#endif
 
-	pinMode(C_LED_PIN, OUTPUT);			// GPS status LED
+	// setup DCM for copters:
-	pinMode(A_LED_PIN, OUTPUT);			// GPS status LED
+	dcm.kp_roll_pitch(0.12);		// higher for quads
-	pinMode(B_LED_PIN, OUTPUT);			// GPS status LED
+	dcm.ki_roll_pitch(0.00000319); 	// 1/4 of the normal rate
-	pinMode(SLIDE_SWITCH_PIN, INPUT);	// To enter interactive mode
-	pinMode(PUSHBUTTON_PIN, INPUT);		// unused
-	DDRL |= B00000100;					// Set Port L, pin 2 to output for the relay
-
-	#if MOTOR_LEDS == 1
-		pinMode(FR_LED, OUTPUT);			// GPS status LED
-		pinMode(RE_LED, OUTPUT);			// GPS status LED
-		pinMode(RI_LED, OUTPUT);			// GPS status LED
-		pinMode(LE_LED, OUTPUT);			// GPS status LED
-	#endif
 
 	// Logging:
 	// --------
-	DataFlash.Init(); 	// DataFlash log initialization
+	// DataFlash log initialization
+	DataFlash.Init();
+
 	// setup the log bitmask
 	if (g.log_bitmask & MASK_LOG_SET_DEFAULTS)
 		default_log_bitmask();
@@ -216,7 +224,7 @@ void init_ardupilot()
 	// the system in an odd state, we don't let the user exit the top
 	// menu; they must reset in order to fly.
 	//
-	if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
+	if (check_startup_for_CLI()) {
 		digitalWrite(A_LED_PIN,HIGH);		// turn on setup-mode LED
 		Serial.printf_P(PSTR("\n"
 							 "Entering interactive setup mode...\n"
@@ -233,6 +241,17 @@ void init_ardupilot()
 		}
 	}
 
+	// Logging:
+	// --------
+	if(g.log_bitmask != 0){
+		//	TODO - Here we will check  on the length of the last log
+		//  We don't want to create a bunch of little logs due to powering on and off
+		start_new_log();
+	}
+
+	//if (g.log_bitmask & MASK_LOG_MODE)
+	//	Log_Write_Mode(control_mode);
+
 	// All of the Gyro calibrations
 	// ----------------------------
 	startup_ground();
@@ -244,16 +263,9 @@ void init_ardupilot()
 	// init the Yaw Hold output
 	clear_yaw_control();
 
-	// Logging:
-	// --------
-	if(g.log_bitmask != 0){
-		//	TODO - Here we will check  on the length of the last log
-		//  We don't want to create a bunch of little logs due to powering on and off
-		start_new_log();
-	}
 
-	if (g.log_bitmask & MASK_LOG_MODE)
+	delay(100);
-		Log_Write_Mode(control_mode);
+
 }
 
 //********************************************************************************
@@ -263,8 +275,6 @@ void startup_ground(void)
 {
 	gcs.send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
 
-	// make Motor light go dark
-	digitalWrite(A_LED_PIN, LOW);
 
 	#if(GROUND_START_DELAY > 0)
 		//gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay"));
@@ -310,7 +320,13 @@ void startup_ground(void)
 	    }
 	}
 
+	clear_leds();
+
 	report_gps();
+	g_gps->idleTimeout = 20000;
+
+	Log_Write_Startup();
+
 	SendDebug("\nReady to FLY ");
 	//gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
 }
@@ -336,6 +352,8 @@ void set_mode(byte mode)
 	//Serial.printf("set mode: %d\n",control_mode);
 	Serial.println(flight_mode_strings[control_mode]);
 
+	led_mode = NORMAL_LEDS;
+
 	switch(control_mode)
 	{
 		case ACRO:
@@ -402,95 +420,6 @@ void set_failsafe(boolean mode)
 	}
 }
 
-#if MOTOR_LEDS == 1
-void update_motor_leds(void)
-{
-	// blink rear
-	static bool blink;
-
-	if (blink){
-		blink = false;
-		digitalWrite(RE_LED, LOW);
-
-	}else{
-		blink = true;
-		digitalWrite(RE_LED, HIGH);
-	}
-}
-#endif
-
-void update_GPS_light(void)
-{
-	// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
-	// ---------------------------------------------------------------------
-	switch (g_gps->status()){
-
-		case(2):
-			digitalWrite(C_LED_PIN, HIGH);  //Turn LED C on when gps has valid fix.
-			break;
-
-		case(1):
-			if (g_gps->valid_read == true){
-				GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
-				if (GPS_light){
-					digitalWrite(C_LED_PIN, LOW);
-				}else{
-					digitalWrite(C_LED_PIN, HIGH);
-				}
-				g_gps->valid_read = false;
-			}
-			break;
-
-		default:
-			digitalWrite(C_LED_PIN, LOW);
-			break;
-	}
-}
-
-void update_motor_light(void)
-{
-	if(motor_armed == false){
-		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);
-		}
-	}
-}
-
-void update_esc_light()
-{
-	static byte step;
-
-	if (step++ == 3)
-		step = 0;
-
-	switch(step)
-	{
-		case 0:
-			digitalWrite(C_LED_PIN, LOW);
-			digitalWrite(A_LED_PIN, HIGH);
-			break;
-
-		case 1:
-			digitalWrite(A_LED_PIN, LOW);
-			digitalWrite(B_LED_PIN, HIGH);
-			break;
-
-		case 2:
-			digitalWrite(B_LED_PIN, LOW);
-			digitalWrite(C_LED_PIN, HIGH);
-			break;
-	}
-}
 
 
 void resetPerfData(void) {
@@ -541,3 +470,27 @@ init_throttle_cruise()
 	//}
 }
 
+#if BROKEN_SLIDER == 1
+
+boolean
+check_startup_for_CLI()
+{
+	if(abs(g.rc_4.control_in) > 3000){
+		// startup to fly
+		return true;
+	}else{
+		// CLI mode
+		return false;
+	}
+}
+
+#else
+
+boolean
+check_startup_for_CLI()
+{
+	return (digitalRead(SLIDE_SWITCH_PIN) == 0);
+}
+
+#endif
+

ArduCopterMega/test.pde

@@ -402,14 +402,12 @@ static int8_t
 test_adc(uint8_t argc, const Menu::arg *argv)
 {
 	print_hit_enter();
-	adc.Init();
-	delay(1000);
 	Serial.printf_P(PSTR("ADC\n"));
 	delay(1000);
 
 	while(1){
 		for(int i = 0; i < 9; i++){
-			Serial.printf_P(PSTR("i:%d\t"),adc.Ch(i));
+			Serial.printf_P(PSTR("%d,"),adc.Ch(i));
 		}
 		Serial.println();
 		delay(20);
@@ -433,29 +431,14 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 	delay(1000);
 
 	//float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
-
+	fast_loopTimer = millis();
 
 	while(1){
-		delay(20);
+		//delay(20);
-		if (millis() - fast_loopTimer > 19) {
+		if (millis() - fast_loopTimer >= 5) {
 			delta_ms_fast_loop 	= millis() - fast_loopTimer;
 			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
 			fast_loopTimer		= millis();
-			/*
-			Matrix3f temp 	= dcm.get_dcm_matrix();
-
-			sin_pitch 		= -temp.c.x;
-			cos_pitch 		= sqrt(1 - (temp.c.x * temp.c.x));
-
-			cos_roll 		= temp.c.z / cos_pitch;
-			sin_roll 		= temp.c.y / cos_pitch;
-
-			yawvector.x 	= temp.a.x; // sin
-			yawvector.y 	= temp.b.x;	// cos
-			yawvector.normalize();
-			cos_yaw 		= yawvector.y;	// 0 x = north
-			sin_yaw 		= yawvector.x;	// 1 y
-			*/
 
 			// IMU
 			// ---
@@ -464,12 +447,33 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 			Vector3f accels 	= imu.get_accel();
 			Vector3f gyros 		= imu.get_gyro();
 
-			if(g.compass_enabled){
+			medium_loopCounter++;
-				medium_loopCounter++;
+			if(medium_loopCounter == 4){
-				if(medium_loopCounter == 5){
+				update_trig();
+			}
+
+			if(medium_loopCounter == 20){
+				read_radio();
+				medium_loopCounter = 0;
+				//tuning();
+				//dcm.kp_roll_pitch((float)g.rc_6.control_in / 2000.0);
+
+				/*
+				Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld, kp:%1.4f, kp:%1.4f \n"),
+								dcm.roll_sensor,
+								dcm.pitch_sensor,
+								dcm.yaw_sensor,
+								dcm.kp_roll_pitch(),
+								(float)g.rc_6.control_in / 2000.0);
+				*/
+				Serial.printf_P(PSTR("%ld, %ld, %ld\n"),
+								dcm.roll_sensor,
+								dcm.pitch_sensor,
+								dcm.yaw_sensor);
+
+				if(g.compass_enabled){
 					compass.read();		 				// Read magnetometer
 					compass.calculate(dcm.roll, dcm.pitch);		// Calculate heading
-					medium_loopCounter = 0;
 				}
 			}
 
@@ -481,12 +485,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 								accels.x, accels.y, accels.z,
 								gyros.x,  gyros.y,  gyros.z);
 			*/
-			Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"),
-								dcm.roll_sensor,
-								dcm.pitch_sensor,
-								dcm.yaw_sensor);
 			/*
-			update_trig();
 			Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
 								cos_pitch_x,
 								sin_pitch_y,
@@ -699,23 +698,30 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
 
 		#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
 			Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
-			
+
+		#elif CHANNEL_6_TUNING == CH6_DCM_RP
+			Serial.printf_P(PSTR("DCM RP: %1.4f\n"), ((float)g.rc_6.control_in / 5000.0));
+
+		#elif CHANNEL_6_TUNING == CH6_DCM_Y
+			Serial.printf_P(PSTR("DCM Y: %1.4f\n"), ((float)g.rc_6.control_in / 1000.0));
+
 		#elif CHANNEL_6_TUNING == CH6_YAW_KP
 			// yaw heading
 			Serial.printf_P(PSTR("yaw kP: %1.3f\n"), ((float)g.rc_6.control_in / 200.0));  // range from 0 ~ 5.0
-			
+
 		#elif CHANNEL_6_TUNING == CH6_YAW_KD
 			// yaw heading
 			Serial.printf_P(PSTR("yaw kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
-		
+
 		#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
 			// yaw rate
 			Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
-			
+
 		#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KD
 			// yaw rate
 			Serial.printf_P(PSTR("yaw rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
 		#endif
+
 		if(Serial.available() > 0){
 			return (0);
 		}
@@ -796,23 +802,18 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_rawgps(uint8_t argc, const Menu::arg *argv)
 {
-  print_hit_enter();
+	print_hit_enter();
-  delay(1000);
+	delay(1000);
 
-  while(1){
+ 	while(1){
-          if (Serial3.available()){
+		if (Serial1.available()){
-                  digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS
+			digitalWrite(B_LED_PIN, HIGH); 		// Blink Yellow LED if we are sending data to GPS
-                  Serial1.write(Serial3.read());
+			Serial.write(Serial1.read());
-                  digitalWrite(B_LED_PIN, LOW);
+			digitalWrite(B_LED_PIN, LOW);
-          }
+		}
-          if (Serial1.available()){
+		if(Serial.available() > 0){
-                  digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS
+			return (0);
-                  Serial3.write(Serial1.read());
+		}
-                  digitalWrite(C_LED_PIN, LOW);
-          }
-          if(Serial.available() > 0){
-                  return (0);
-    }
   }
 }
 
@@ -893,6 +894,11 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_sonar(uint8_t argc, const Menu::arg *argv)
 {
+	if(g.sonar_enabled == false){
+		Serial.printf_P(PSTR("Sonar disabled\n"));
+		return (0);
+	}
+
 	print_hit_enter();
 	while(1) {
 		delay(100);
@@ -903,6 +909,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
 			return (0);
 		}
 	}
+
 	return (0);
 }
 
@@ -910,7 +917,7 @@ static int8_t
 test_mission(uint8_t argc, const Menu::arg *argv)
 {
 	//write out a basic mission to the EEPROM
-	Location t;
+
 /*{
 	uint8_t		id;					///< command id
 	uint8_t		options;			///< options bitmask (1<<0 = relative altitude)
@@ -919,7 +926,6 @@ test_mission(uint8_t argc, const Menu::arg *argv)
 	int32_t		lat;				///< param 3 - Lattitude * 10**7
 	int32_t		lng;				///< param 4 - Longitude * 10**7
 }*/
-	byte alt_rel = 1;
 
 	// clear home
 	{Location t = {0,   	0,      0, 		0, 		0, 			0};
@@ -963,7 +969,7 @@ test_mission(uint8_t argc, const Menu::arg *argv)
 	g.waypoint_radius.set_and_save(3);
 
 	test_wp(NULL, NULL);
-
+	return (0);
 }
 
 void print_hit_enter()