updated to AP_Var

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

ArduCopterMega/ArduCopterMega.pde

@@ -1,7 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
 /*
-ArduCopterMega Version 0.1 Experimental
+ArduCopterMega Version 0.1.3 Experimental
 Authors:	Jason Short
 Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
 Thanks to:	Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
@@ -216,8 +216,7 @@ byte	command_may_index;					// current command memory location
 byte	command_must_ID;					// current command ID
 byte	command_may_ID;						// current command ID
 
-float	altitude_gain;						// in nav
+//float	altitude_gain;						// in nav
-float	distance_gain;						// in nav
 
 float cos_roll_x;
 float sin_roll_y;
@@ -281,11 +280,9 @@ int 	milliamp_hours;
 
 // Barometer Sensor variables
 // --------------------------
-//int				baro_offset;				// used to correct drift of absolute pressue sensor
 unsigned long 	abs_pressure;
-unsigned long 	abs_pressure_ground;
+unsigned long 	ground_pressure;
 int 			ground_temperature;
-//int 			temp_unfilt;
 
 byte 	altitude_sensor = BARO;				// used to know whic sensor is active, BARO or SONAR
 

ArduCopterMega/EEPROM.pde

@@ -5,7 +5,7 @@
 
 // Macro functions
 // ---------------
-void read_EEPROM_startup(void)
+/*void read_EEPROM_startup(void)
 {
 	read_EEPROM_PID();
 	read_EEPROM_frame();
@@ -13,27 +13,17 @@ void read_EEPROM_startup(void)
 	read_EEPROM_logs();
 	read_EEPROM_flight_modes();
 	read_EEPROM_waypoint_info();
-	imu.load_gyro_eeprom();
+	//imu.load_gyro_eeprom();
-	imu.load_accel_eeprom();
+	//imu.load_accel_eeprom();
 	read_EEPROM_alt_RTL();
 	read_EEPROM_current();
 	read_EEPROM_nav();
 	// magnatometer
 	read_EEPROM_compass();
-	read_EEPROM_compass_declination();
+	//read_EEPROM_compass_declination();
 	read_EEPROM_compass_offset();
 }
-
+*/
-void read_EEPROM_airstart_critical(void)
-{	
-	// Read the home location
-	//-----------------------	
-	home = get_wp_with_index(0);
-		
-	// Read pressure sensor values
-	//----------------------------
-	read_EEPROM_pressure();
-}
 
 void save_EEPROM_groundstart(void)
 {
@@ -56,56 +46,45 @@ void save_EEPROM_groundstart(void)
 
 void save_EEPROM_alt_RTL(void)
 {
-	eeprom_write_dword((uint32_t *)EE_ALT_HOLD_HOME, alt_to_hold);
+	g.RTL_altitude.save();
+	
 }
 
 void read_EEPROM_alt_RTL(void)
 {
-	alt_to_hold = eeprom_read_dword((const uint32_t *)	EE_ALT_HOLD_HOME);
+	g.RTL_altitude.load();
 }
 
 /********************************************************************************/
 
 void read_EEPROM_waypoint_info(void)
 {
-	g.waypoint_total 		= eeprom_read_byte((uint8_t *) EE_WP_TOTAL);				
+	g.waypoint_total.load();
-	wp_radius 		= eeprom_read_byte((uint8_t *) EE_WP_RADIUS);
+	g.waypoint_radius.load();
-	loiter_radius 	= eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS);
+	g.loiter_radius.load();
 }
 
 void save_EEPROM_waypoint_info(void)
 {
-	eeprom_write_byte((uint8_t *)	EE_WP_TOTAL, 		g.waypoint_total);
+	g.waypoint_total.save();
-	eeprom_write_byte((uint8_t *)	EE_WP_RADIUS, 		wp_radius);
+	g.waypoint_radius.save();
-	eeprom_write_byte((uint8_t *)	EE_LOITER_RADIUS, 	loiter_radius);
+	g.loiter_radius.save();
 }
 
 /********************************************************************************/
 
 void read_EEPROM_nav(void)
 {
-	// for nav estimation
+	g.crosstrack_gain.load();
-	distance_gain 		= read_EE_compressed_float(EE_DISTANCE_GAIN, 4);
+	g.crosstrack_entry_angle.load();
-	altitude_gain 		= read_EE_compressed_float(EE_ALTITUDE_GAIN, 4);
+	g.pitch_max.load();
-
-	// stored as degree * 100
-	g.crosstrack_gain 			= read_EE_compressed_float(EE_XTRACK_GAIN, 4);
-	g.crosstrack_entry_angle 	= eeprom_read_word((uint16_t *)	EE_XTRACK_ANGLE) * 100;
-	pitch_max 			= eeprom_read_word((uint16_t *) EE_PITCH_MAX);	// stored as degress * 100
 }
 
 void save_EEPROM_nav(void)
 {
-	// for nav estimation
+	g.crosstrack_gain.save();
-	write_EE_compressed_float(altitude_gain, 	EE_ALTITUDE_GAIN, 	4);
+ 	g.crosstrack_entry_angle.save();
-	write_EE_compressed_float(distance_gain, 	EE_DISTANCE_GAIN, 	4);
+	g.pitch_max.save();
-	write_EE_compressed_float(crosstrack_gain, 	EE_XTRACK_GAIN, 	4);
-	
-	// stored as degrees
- 	eeprom_write_word((uint16_t *)	EE_XTRACK_ANGLE, g.crosstrack_entry_angle / 100);
-
-	// stored as degrees
-	eeprom_write_word((uint16_t *)	EE_PITCH_MAX, pitch_max);
 }
 
 /********************************************************************************/
@@ -126,10 +105,10 @@ void read_EEPROM_PID(void)
 	g.pid_sonar_throttle.load_gains();
 	
 	// roll pitch
-	stabilize_dampener 			= read_EE_compressed_float(EE_STAB_DAMPENER, 4);
+	g.stabilize_dampener.load();
 
 	// yaw
-	hold_yaw_dampener			= read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4);
+	g.hold_yaw_dampener.load();
  	init_pids();
 }
 
@@ -149,61 +128,47 @@ void save_EEPROM_PID(void)
 	g.pid_sonar_throttle.save_gains();
 
 	// roll pitch
-	write_EE_compressed_float(stabilize_dampener,	EE_STAB_DAMPENER, 4);
+	g.stabilize_dampener.save();
-
 	// yaw
-	write_EE_compressed_float(hold_yaw_dampener, 	EE_HOLD_YAW_DAMPENER, 4);
+	g.hold_yaw_dampener.save();
 }
 
 /********************************************************************************/
 
 void save_EEPROM_frame(void)
 {
-	eeprom_write_byte((uint8_t *)EE_FRAME, frame_type);
+	g.frame_type.save();
 }
 
 void read_EEPROM_frame(void)
 {
-	frame_type 	= eeprom_read_byte((uint8_t *)	EE_FRAME);
+	g.frame_type.load();
 }
 
 /********************************************************************************/
 
-void save_EEPROM_g.(void)
+void save_EEPROM_throttle_cruise (void)
 {
-	eeprom_write_word((uint16_t *)EE_THROTTLE_CRUISE, g.);
+	g.throttle_cruise.save();
 }
 
-void read_EEPROM_g.(void)
+void read_EEPROM_throttle_cruise(void)
 {
-	g.throttle_cruise 	= eeprom_read_word((uint16_t *)	EE_THROTTLE_CRUISE);
+	g.throttle_cruise.load();
-}
-
-/********************************************************************************/
-
-void save_EEPROM_mag_declination(void)
-{
-	write_EE_compressed_float(mag_declination, 	EE_MAG_DECLINATION, 1);
-}
-
-void read_EEPROM_compass_declination(void)
-{
-	mag_declination 	= read_EE_compressed_float(EE_MAG_DECLINATION, 1);
 }
 
 /********************************************************************************/
 
 void save_EEPROM_current(void)
 {
-	eeprom_write_byte((uint8_t *)	EE_CURRENT_SENSOR, 	current_enabled);
+	g.current_enabled.save();
-	eeprom_write_word((uint16_t *)	EE_CURRENT_MAH, 	milliamp_hours);
+	g.milliamp_hours.save();
-	
 }
 
 void read_EEPROM_current(void)
 {
-	current_enabled	= eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR);
+	g.current_enabled.load();
-	milliamp_hours	= eeprom_read_word((uint16_t *) EE_CURRENT_MAH);
+	g.milliamp_hours.load();
 }
 
 /********************************************************************************/
@@ -226,40 +191,42 @@ void read_EEPROM_compass_offset(void)
 
 void read_EEPROM_compass(void)
 {
-	compass_enabled	= eeprom_read_byte((uint8_t *) EE_COMPASS);
+	g.compass_enabled.load();
 }
 
 void save_EEPROM_mag(void)
 {
-	eeprom_write_byte((uint8_t *)	EE_COMPASS, 	compass_enabled);
+	g.compass_enabled.save();
 }
 
 /********************************************************************************/
 
 void save_command_index(void)
 {
-	eeprom_write_byte((uint8_t *) EE_WP_INDEX, command_must_index);
+	g.command_must_index.save();
 }
 
 void read_command_index(void)
 {
-	g.waypoint_index = command_must_index 	= eeprom_read_byte((uint8_t *) EE_WP_INDEX);
+	g.command_must_index.load();
 }
 
 /********************************************************************************/
 
 void save_EEPROM_pressure(void)
 {
-	eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, 	abs_pressure_ground);
+	g.ground_pressure.save();
-	eeprom_write_word((uint16_t *)EE_GND_TEMP, 			ground_temperature);
+	g.ground_temperature.save();
+
 }
 
 void read_EEPROM_pressure(void)
 {
-	abs_pressure_ground = eeprom_read_dword((uint32_t *) 	EE_ABS_PRESS_GND);
+	g.ground_pressure.load();
-	// Better than zero for an air start value
+	g.ground_temperature.load();	
-	abs_pressure	 	= abs_pressure_ground;
+
-	ground_temperature 	= eeprom_read_word((uint16_t *) 	EE_GND_TEMP);
+	// to prime the filter
+	abs_pressure	 	= g.ground_pressure;
 }
 
 /********************************************************************************/
@@ -292,52 +259,51 @@ void save_EEPROM_radio(void)
 // configs are the basics
 void read_EEPROM_throttle(void)
 {
-	throttle_min 				= eeprom_read_word((uint16_t *) 	EE_THROTTLE_MIN);
+	g.throttle_min.load();
-	throttle_max 				= eeprom_read_word((uint16_t *) 	EE_THROTTLE_MAX);
+	g.throttle_max.load();
-	read_EEPROM_g.();
+	g.throttle_cruise.load();
-	throttle_failsafe_enabled 	= eeprom_read_byte((byte *) 	EE_THROTTLE_FAILSAFE);
+	g.throttle_failsafe_enabled.load();
-	throttle_failsafe_action 	= eeprom_read_byte((byte *) 	EE_THROTTLE_FAILSAFE_ACTION);
+	g.throttle_failsafe_action.load();
-	throttle_failsafe_value 	= eeprom_read_word((uint16_t *) EE_THROTTLE_FS_VALUE);
+	g.throttle_failsafe_value.load();
 }
 
 void save_EEPROM_throttle(void)
 {
-	eeprom_write_word((uint16_t *) 	EE_THROTTLE_MIN, 			throttle_min);
+	g.throttle_min.load();
-	eeprom_write_word((uint16_t *) 	EE_THROTTLE_MAX, 			throttle_max);
+	g.throttle_max.load();
-	save_EEPROM_g.();
+	g.throttle_cruise.save();
-	eeprom_write_byte((byte *) 		EE_THROTTLE_FAILSAFE,		throttle_failsafe_enabled);
+	g.throttle_failsafe_enabled.load();
-	eeprom_write_byte((byte *) 		EE_THROTTLE_FAILSAFE_ACTION,throttle_failsafe_action);
+	g.throttle_failsafe_action.load();
-	eeprom_write_word((uint16_t *) 	EE_THROTTLE_FS_VALUE,		throttle_failsafe_value);
+	g.throttle_failsafe_value.load();
 }
 
 /********************************************************************************/
 
 void read_EEPROM_logs(void)
 {
-	g.log_bitmask 				= eeprom_read_word((uint16_t *) EE_LOG_BITMASK);
+	g.log_bitmask.load();
 }
 
 void save_EEPROM_logs(void)
 {
-	eeprom_write_word((uint16_t *) 	EE_LOG_BITMASK,				log_bitmask);
+	g.log_bitmask.save();
 }
 
 /********************************************************************************/
 
 void read_EEPROM_flight_modes(void)
 {
-	// Read Radio min/max settings
+	g.flight_modes.load();
-	eeprom_read_block((void*)&flight_modes, (const void*)EE_FLIGHT_MODES, sizeof(flight_modes));
 }
 
 void save_EEPROM_flight_modes(void)
 {
-	// Save Radio min/max settings
+	g.flight_modes.save();
- 	eeprom_write_block((const void *)&flight_modes, (void *)EE_FLIGHT_MODES, sizeof(flight_modes));
+	
 }
 
 /********************************************************************************/
-
+/*
 float
 read_EE_float(int address)
 {
@@ -362,9 +328,9 @@ void write_EE_float(float value, int address)
 	for (int i = 0; i < 4; i++) 
 		eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
 }
-
+*/
 /********************************************************************************/
-
+/*
 float
 read_EE_compressed_float(int address, byte places)
 {
@@ -380,3 +346,4 @@ void write_EE_compressed_float(float value, int address, byte places)
 	int temp 	= value * scale;
 	eeprom_write_word((uint16_t *) 	address, temp);
 }
+*/

ArduCopterMega/Parameters.h

@@ -55,11 +55,10 @@ public:
         //
         k_param_IMU_calibration = 140,
         k_param_ground_temperature,
-        k_param_ground_altitude,
         k_param_ground_pressure,
 		k_param_current,
+		k_param_milliamp_hours,
 		k_param_compass,
-		k_param_mag_declination,
 
         //
         // 160: Navigation parameters
@@ -97,6 +96,7 @@ public:
         k_param_waypoint_mode = 220,
         k_param_waypoint_total,
         k_param_waypoint_index,
+        k_param_command_must_index,        
         k_param_waypoint_radius,
         k_param_loiter_radius,
 
@@ -138,6 +138,7 @@ public:
     AP_Int8     waypoint_mode;
     AP_Int8     waypoint_total;
     AP_Int8     waypoint_index;
+    AP_Int8		command_must_index;
     AP_Int8     waypoint_radius;
     AP_Int8     loiter_radius;
 
@@ -168,14 +169,13 @@ public:
     //
     AP_Int16    log_bitmask;
     AP_Int16    ground_temperature;
-    AP_Int16    ground_altitude;
     AP_Int16    ground_pressure;
     AP_Int16    RTL_altitude;
     AP_Int8		frame_type;
 
     AP_Int8		current_enabled;
+    AP_Int16	milliamp_hours;
     AP_Int8		compass_enabled;
-    AP_Float	mag_declination;
 
 
     // RC channels
@@ -218,21 +218,22 @@ public:
         frame_type  			(FRAME_CONFIG,				k_param_frame_type,				PSTR("FRAME_CONFIG")),
 
         current_enabled  		(DISABLED,					k_param_current,				PSTR("CURRENT_ENABLE")),
+        milliamp_hours  		(2100,						k_param_milliamp_hours,			PSTR("MAH")),
         compass_enabled  		(DISABLED,					k_param_compass,				PSTR("COMPASS_ENABLE")),
-        mag_declination  		(0,							k_param_mag_declination,		PSTR("MAG_DEC")),
 
         waypoint_mode           (0,                         k_param_waypoint_mode,          PSTR("WP_MODE")),
         waypoint_total          (0,                         k_param_waypoint_total,         PSTR("WP_TOTAL")),
         waypoint_index          (0,                         k_param_waypoint_index,         PSTR("WP_INDEX")),
+        command_must_index      (0,                         k_param_command_must_index,     PSTR("WP_MUST_INDEX")),
         waypoint_radius         (WP_RADIUS_DEFAULT,         k_param_waypoint_radius,        PSTR("WP_RADIUS")),
         loiter_radius           (LOITER_RADIUS_DEFAULT,     k_param_loiter_radius,          PSTR("LOITER_RADIUS")),
 
-        throttle_min            (THROTTLE_MIN,              k_param_throttle_min,           PSTR("THR_MIN")),
+        throttle_min            	(THROTTLE_MIN,              k_param_throttle_min,				PSTR("THR_MIN")),
-        throttle_max            (THROTTLE_MAX,              k_param_throttle_max,           PSTR("THR_MAX")),
+        throttle_max            	(THROTTLE_MAX,              k_param_throttle_max,				PSTR("THR_MAX")),
         throttle_failsafe_enabled   (THROTTLE_FAILSAFE,         k_param_throttle_failsafe_enabled,  PSTR("THR_FAILSAFE")),
         throttle_failsafe_action	(THROTTLE_FAILSAFE_ACTION,  k_param_throttle_failsafe_action, 	PSTR("THR_FS_ACTION")),
         throttle_failsafe_value 	(THROTTLE_FS_VALUE,         k_param_throttle_failsafe_value, 	PSTR("THR_FS_VALUE")),
-        throttle_cruise         (THROTTLE_CRUISE,           k_param_throttle_cruise,        PSTR("TRIM_THROTTLE")),
+        throttle_cruise         	(THROTTLE_CRUISE,           k_param_throttle_cruise,        	PSTR("TRIM_THROTTLE")),
 
         flight_mode_channel     (FLIGHT_MODE_CHANNEL,       k_param_flight_mode_channel,    PSTR("FLIGHT_MODE_CH")),
         flight_modes            (k_param_flight_modes,                                      PSTR("FLIGHT_MODES")),
@@ -241,7 +242,6 @@ public:
 
         log_bitmask             (0,                         k_param_log_bitmask,            PSTR("LOG_BITMASK")),
         ground_temperature      (0,                         k_param_ground_temperature,     PSTR("GND_TEMP")),
-        ground_altitude         (0,                         k_param_ground_altitude,        PSTR("GND_ALT_CM")),
         ground_pressure         (0,                         k_param_ground_pressure,        PSTR("GND_ABS_PRESS")),
         RTL_altitude            (ALT_HOLD_HOME_CM,          k_param_RTL_altitude,           PSTR("ALT_HOLD_RTL")),
 

ArduCopterMega/commands.pde

@@ -155,7 +155,7 @@ void
 set_next_WP(struct Location *wp)
 {
 	Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), (int)g.waypoint_index);
-	send_message(MSG_COMMAND, g.waypoint_index);
+	send_message(MSG_COMMAND_LIST, g.waypoint_index);
 
 	// copy the current WP into the OldWP slot
 	// ---------------------------------------

ArduCopterMega/commands_process.pde

@@ -26,10 +26,10 @@ void load_next_command()
 	if(next_command.id == CMD_BLANK){
 		next_command = get_wp_with_index(g.waypoint_index + 1);
 		if(next_command.id != CMD_BLANK){
-			//Serial.print("MSG fetch found new cmd from list at index: ");
+			//SendDebug("MSG <load_next_command> fetch found new cmd from list at index ");
-			//Serial.println((g.waypoint_index + 1),DEC);
+			//SendDebug((g.waypoint_index + 1),DEC);
-			//Serial.print("MSG cmd id: ");
+			//SendDebug(" with cmd id ");
-			//Serial.println(next_command.id,DEC);	
+			//SendDebugln(next_command.id,DEC);
 		}
 	}
 
@@ -38,9 +38,9 @@ void load_next_command()
 	// --------------------------------------------
 	if(next_command.id == CMD_BLANK){
 		// we are out of commands!
-		//send_message(SEVERITY_LOW,"out of commands!");
+		gcs.send_text(SEVERITY_LOW,"out of commands!");
-		//Serial.print("MSG out of commands, wp_index: ");
+		//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
-		//Serial.println(g.waypoint_index,DEC);
+		//SendDebugln(g.waypoint_index,DEC);
 
 
 		switch (control_mode){
@@ -50,8 +50,8 @@ void load_next_command()
 
 			default:
 				next_command = get_LOITER_home_wp();
-				//Serial.print("MSG Preload RTL cmd id: ");
+				//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
-				//Serial.println(next_command.id,DEC);
+				//SendDebugln(next_command.id,DEC);
 				break;
 		}
 	}
@@ -62,15 +62,15 @@ void process_next_command()
 	// these are waypoint/Must commands
 	// ---------------------------------
 	if (command_must_index == 0){ // no current command loaded
-		if (next_command.id >= 0x10 && next_command.id <= 0x1F ){
+		if (next_command.id < MAV_CMD_NAV_LAST ){
 			increment_WP_index();
-			save_command_index();	// to Recover from in air Restart
+			//save_command_index();	// to Recover from in air Restart
 			command_must_index = g.waypoint_index;
 
-			//Serial.print("MSG new command_must_id ");
+			//SendDebug("MSG <process_next_command> new command_must_id ");
-			//Serial.print(next_command.id,DEC);
+			//SendDebug(next_command.id,DEC);
-			//Serial.print(" index:");
+			//SendDebug(" index:");
-			//Serial.println(command_must_index,DEC);
+			//SendDebugln(command_must_index,DEC);
 			if (g.log_bitmask & MASK_LOG_CMD)
 				Log_Write_Cmd(g.waypoint_index, &next_command);
 			process_must();
@@ -80,7 +80,7 @@ void process_next_command()
 	// these are May commands
 	// ----------------------
 	if (command_may_index == 0){
-		if (next_command.id >= 0x20 && next_command.id <= 0x2F ){
+		if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
 			increment_WP_index();// this command is from the command list in EEPROM
 			command_may_index = g.waypoint_index;
 			//Serial.print("new command_may_index ");
@@ -93,7 +93,7 @@ void process_next_command()
 
 	// these are do it now commands
 	// ---------------------------
-	if (next_command.id >= 0x30){
+	if (next_command.id > MAV_CMD_CONDITION_LAST){
 		increment_WP_index();// this command is from the command list in EEPROM
 		if (g.log_bitmask & MASK_LOG_CMD)
 			Log_Write_Cmd(g.waypoint_index, &next_command);
@@ -101,17 +101,16 @@ void process_next_command()
 	}
 
 }
-
 /*
 These functions implement the waypoint commands.
 */
 void process_must()
 {
-	//Serial.print("process must index: ");
+	//SendDebug("process must index: ");
-	//Serial.println(command_must_index,DEC);
+	//SendDebugln(command_must_index,DEC);
 
-	send_message(SEVERITY_LOW,"New cmd: ");
+	gcs.send_text(SEVERITY_LOW,"New cmd: <process_must>");
-	send_message(MSG_COMMAND, g.waypoint_index);
+	gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
 
 	// clear May indexes
 	command_may_index	= 0;
@@ -119,6 +118,10 @@ void process_must()
 
 	command_must_ID = next_command.id;
 
+	// loads the waypoint into Next_WP struct
+	// --------------------------------------
+	set_next_WP(&next_command);
+
 	// invalidate command so a new one is loaded
 	// -----------------------------------------
 	next_command.id = 0;
@@ -127,13 +130,10 @@ void process_must()
 	// -------------------------
 	reset_I();
 
-	// loads the waypoint into Next_WP struct
-	// --------------------------------------
-	set_next_WP(&next_command);
-		
 	switch(command_must_ID){
 
-		case CMD_TAKEOFF:	// TAKEOFF!
+		case MAV_CMD_NAV_TAKEOFF:	// TAKEOFF!
+			// pitch in deg, airspeed  m/s, throttle %, track WP 1 or 0
 			takeoff_altitude 	= (int)next_command.alt;
 			next_WP.lat 		= current_loc.lat + 10;	// so we don't have bad calcs
 			next_WP.lng 		= current_loc.lng + 10;	// so we don't have bad calcs
@@ -142,10 +142,10 @@ void process_must()
 			//set_next_WP(&next_WP);
 			break;
 
-		case CMD_WAYPOINT:	// Navigate to Waypoint
+		case MAV_CMD_NAV_WAYPOINT:	// Navigate to Waypoint
 			break;
 			
-		case CMD_R_WAYPOINT:	// Navigate to Waypoint
+		case MAV_CMD_NAV_R_WAYPOINT:	// Navigate to Waypoint
 			next_command.lat 	+= home.lat;	// offset from home location
 			next_command.lng 	+= home.lng;	// offset from home location
 
@@ -153,7 +153,7 @@ void process_must()
 			set_next_WP(&next_command);
 			break;
 
-		case CMD_LAND:	// LAND to Waypoint
+		case MAV_CMD_NAV_LAND:	// LAND to Waypoint
 			velocity_land		= 1000;
 			next_WP.lat 		= current_loc.lat;	
 			next_WP.lng 		= current_loc.lng;	
@@ -161,24 +161,25 @@ void process_must()
 			land_complete 		= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction 
 			break;
 			
-		case CMD_ALTITUDE:	// Loiter indefinitely
+		/*
+		case MAV_CMD_ALTITUDE:	//
 			next_WP.lat 		= current_loc.lat + 10;	// so we don't have bad calcs
 			next_WP.lng 		= current_loc.lng + 10;	// so we don't have bad calcs
 			break;
+		*/
 			
-		case CMD_LOITER:	// Loiter indefinitely
+		case MAV_CMD_NAV_LOITER_UNLIM:	// Loiter indefinitely
 			break;
 		
-		case CMD_LOITER_N_TURNS:	// Loiter N Times
+		case MAV_CMD_NAV_LOITER_TURNS:	// Loiter N Times
 			break;
 
-		case CMD_LOITER_TIME:
+		case MAV_CMD_NAV_LOITER_TIME:
 			break;
 
-		case CMD_RTL:
+		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
 			return_to_launch();		
 			break;
-
 	}
 }
 
@@ -192,22 +193,21 @@ void process_may()
 	// -----------------------------------------
 	next_command.id = 0;
 
-	send_message(SEVERITY_LOW,"New cmd: ");
+	gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
-	send_message(MSG_COMMAND, g.waypoint_index);
+	gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
-	
 	// do the command
 	// --------------
 	switch(command_may_ID){
 
-		case CMD_DELAY:	// Navigate to Waypoint
+		case MAV_CMD_CONDITION_DELAY:	// Navigate to Waypoint
 			delay_start = millis();
 			delay_timeout  = next_command.lat;
 			break;
 
-		case CMD_LAND_OPTIONS:	// Land this puppy
+		//case MAV_CMD_NAV_LAND_OPTIONS:	// Land this puppy
-			break;
+		//	break;
 
-		case CMD_ANGLE:
+		case MAV_CMD_CONDITION_ANGLE:
 			// p1:		bearing
 			// alt:		speed
 			// lat:		direction (-1,1), 
@@ -282,37 +282,15 @@ void process_now()
 	// -----------------------------------------
 	next_command.id = 0;
 
-	send_message(SEVERITY_LOW, "New cmd: ");
+	gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
-	send_message(MSG_COMMAND, g.waypoint_index);
+	gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
 
 	// do the command
 	// --------------
 	switch(id){
-	
+		/*
-		case CMD_RESET_INDEX:	
-			init_commands();
-			break;
-
-		case CMD_GETVAR_INDEX:
-			//
-			break;
-
-		case CMD_SENDVAR_INDEX:
-			//
-			break;
-
-		case CMD_TELEMETRY:	
-			//
-			break;
-
-		//case CMD_AIRSPEED_CRUISE:	
-			//airspeed_cruise = next_command.p1 * 100;
-			// todo save to EEPROM
-			//break;
-
 		case CMD_THROTTLE_CRUISE:	
 			g.throttle_cruise = next_command.p1;
-			// todo save to EEPROM
 			break;
 
 		case CMD_RESET_HOME:	
@@ -356,29 +334,33 @@ void process_now()
 		//	radio_min[next_command.p1] = next_command.alt;
 		//	save_EEPROM_radio_minmax();
 		//	break;
+		*/
 		
-		case CMD_REPEAT:	
+		case MAV_CMD_DO_SET_HOME:
+			init_home();
+			break;
+		
+		case MAV_CMD_DO_REPEAT_SERVO:	
 			new_event(&next_command);
 			break;
 
-		case CMD_SERVO:
+		case MAV_CMD_DO_SET_SERVO:
+			//Serial.print("MAV_CMD_DO_SET_SERVO ");
+			//Serial.print(next_command.p1,DEC);
+			//Serial.print(" PWM: ");
+			//Serial.println(next_command.alt,DEC);
 			APM_RC.OutputCh(next_command.p1, next_command.alt);
 			break;
 
-		case CMD_INDEX:
+		case MAV_CMD_DO_SET_RELAY:
-			command_must_index = 0;
+			if (next_command.p1 == 0) {
-			command_may_index = 0;
+				relay_on();
-			g.waypoint_index = next_command.p1 - 1;
+			} else if (next_command.p1 == 1) {
-			break;
+				relay_off();
-
-		case CMD_RELAY:
-			if(next_command.p1 = 0){
-				relay_A();
 			}else{
-				relay_B();
+				relay_toggle();
 			}
 			break;
-			
 	}
 }
 
@@ -386,24 +368,24 @@ void process_now()
 // ---------------
 void verify_must()
 {
-	float power;
 	
 	switch(command_must_ID) {
 	
-		case CMD_ALTITUDE:
+		/*case MAV_CMD_ALTITUDE:
 				if (abs(next_WP.alt - current_loc.alt) < 100){
 					command_must_index 	= 0;
 				}
 			break;
+		*/
 		
-		case CMD_TAKEOFF:	// Takeoff!
+		case MAV_CMD_NAV_TAKEOFF:	// Takeoff!
 			if (current_loc.alt > (next_WP.alt -100)){
 				command_must_index 	= 0;
 				takeoff_complete 	= true;
 			}
 			break;
 
-		case CMD_LAND:
+		case MAV_CMD_NAV_LAND:
 			// 10 - 9  = 1  
 			velocity_land  = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
 			old_alt = current_loc.alt;
@@ -411,12 +393,19 @@ void verify_must()
 				land_complete 		= true;
 				command_must_index 	= 0;
 			}
+			update_crosstrack();
+
 			break;
 
-		case CMD_WAYPOINT:	// reach a waypoint
+		case MAV_CMD_NAV_WAYPOINT:	// reach a waypoint
-			if ((wp_distance > 0) && (wp_distance <= waypoint_radius)) {
+			update_crosstrack();
-				Serial.print("MSG REACHED_WAYPOINT #");
+			if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
-				Serial.println(command_must_index,DEC);
+				//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
+				//SendDebugln(command_must_index,DEC);
+				char message[50];
+				sprintf(message,"Reached Waypoint #%i",command_must_index);
+				gcs.send_text(SEVERITY_LOW,message);
+
 				// clear the command queue;
 				command_must_index 	= 0;
 			}
@@ -428,23 +417,35 @@ void verify_must()
 			}
 			break;
 
-		case CMD_LOITER_N_TURNS:	// LOITER N times
+		case MAV_CMD_NAV_LOITER_TURNS:	// LOITER N times
 			break;
 
-		case CMD_LOITER_TIME:	// loiter N milliseconds
+		case MAV_CMD_NAV_LOITER_TIME:	// loiter N milliseconds
 			break;
 
-		case CMD_RTL:
+		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-			if (wp_distance <= waypoint_radius) {
+			if (wp_distance <= g.waypoint_radius) {
-				//Serial.println("REACHED_HOME");
+				gcs.send_text(SEVERITY_LOW,"Reached home");
 				command_must_index 	= 0;
 			}
 			break;
 			
-		case CMD_LOITER:	// Just plain LOITER
+		//case MAV_CMD_NAV_LOITER_UNLIM:	// Just plain LOITER
-			break;
+		//	break;
 
-		case CMD_ANGLE:
+
+		default:
+			gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
+			break;
+	}
+}
+
+void verify_may()
+{
+	float power;
+	switch(command_may_ID) {
+	
+		case MAV_CMD_CONDITION_ANGLE:
 			if((millis() - command_yaw_start_time) > command_yaw_time){
 				command_must_index 	= 0;
 				nav_yaw = command_yaw_end;
@@ -456,23 +457,14 @@ void verify_must()
 			}
 			break;
 
-		default:
+		case MAV_CMD_CONDITION_DELAY:
-			send_message(SEVERITY_HIGH,"No current Must commands");
-			break;
-	}	
-}
-
-void verify_may()
-{
-	switch(command_may_ID) {
-		case CMD_DELAY:
 			if ((millis() - delay_start) > delay_timeout){
 				command_may_index = 0;
 				delay_timeout = 0;
 			}
 			
-		case CMD_LAND_OPTIONS:
+		//case CMD_LAND_OPTIONS:
-			break;
+		//	break;
 	}
 }
 

ArduCopterMega/control_modes.pde

@@ -72,14 +72,14 @@ void read_trim_switch()
 		if(trim_flag){
 			// switch was just released
 			if((millis() - trim_timer) > 2000){
-				// not being used
+				imu.save();
 				
 			} else {
 				// set the throttle nominal
 				if(g.rc_3.control_in > 50){
-					g.throttle_cruise = g.rc_3.control_in;
+					g.throttle_cruise.set(g.rc_3.control_in);
-					Serial.printf("tnom %d\n", g.);
+					Serial.printf("tnom %d\n", g.throttle_cruise.get());
-					save_EEPROM_g.();
+					save_EEPROM_throttle_cruise();
 				}
 			}
 			trim_flag = false;

ArduCopterMega/defines.h

@@ -62,7 +62,8 @@
 #define SONAR 0
 #define BARO 1
 
-#define WP_START_BYTE 0x130	// where in memory home WP is stored + all other WP
+// parameters get the first 1KiB of EEPROM, remainder is for waypoints
+#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
 #define WP_SIZE 14
 
 // GCS enumeration
@@ -156,19 +157,60 @@
 #define MSG_PRESSURE 0x04
 #define MSG_STATUS_TEXT 0x05
 #define MSG_PERF_REPORT 0x06
-#define MSG_COMMAND 0x22
+#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
+#define MSG_VERSION_REQUEST 0x08
+#define MSG_VERSION 0x09
+#define MSG_EXTENDED_STATUS 0x0a
+#define MSG_CPU_LOAD 0x0b
+
+#define MSG_COMMAND_REQUEST 0x20
+#define MSG_COMMAND_UPLOAD 0x21
+#define MSG_COMMAND_LIST 0x22
+#define MSG_COMMAND_MODE_CHANGE 0x23
+#define MSG_CURRENT_WAYPOINT 0x24
+
+#define MSG_VALUE_REQUEST 0x30
+#define MSG_VALUE_SET 0x31
 #define MSG_VALUE 0x32
+
+#define MSG_PID_REQUEST 0x40
+#define MSG_PID_SET 0x41
 #define MSG_PID 0x42
-#define MSG_TRIMS 0x50
+#define MSG_VFR_HUD 0x4a
-#define MSG_MINS 0x51
+
-#define MSG_MAXS 0x52
+#define MSG_TRIM_STARTUP 0x50
-#define MSG_IMU_OUT 0x53
+#define MSG_TRIM_MIN 0x51
+#define MSG_TRIM_MAX 0x52
+#define MSG_RADIO_OUT 0x53
+#define MSG_RADIO_IN  0x54
+
+#define MSG_RAW_IMU 0x60
+#define MSG_GPS_STATUS 0x61
+#define MSG_GPS_RAW 0x62
+
+#define MSG_SERVO_OUT 0x70
+
+#define MSG_PIN_REQUEST 0x80
+#define MSG_PIN_SET 0x81
+
+#define MSG_DATAFLASH_REQUEST 0x90
+#define MSG_DATAFLASH_SET 0x91
+
+#define MSG_EEPROM_REQUEST 0xa0
+#define MSG_EEPROM_SET 0xa1
+
+#define MSG_POSITION_CORRECT 0xb0
+#define MSG_ATTITUDE_CORRECT 0xb1
+#define MSG_POSITION_SET 0xb2
+#define MSG_ATTITUDE_SET 0xb3
+#define MSG_LOCAL_LOCATION 0xb4
 
 #define SEVERITY_LOW 1
 #define SEVERITY_MEDIUM 2
 #define SEVERITY_HIGH 3
 #define SEVERITY_CRITICAL 4
 
+
 //  Logging parameters
 #define LOG_ATTITUDE_MSG		0x01
 #define LOG_GPS_MSG				0x02
@@ -269,8 +311,8 @@
 #define EE_XTRACK_GAIN 0x30
 #define EE_XTRACK_ANGLE 0x32
 #define EE_PITCH_MAX 0x34
-#define EE_DISTANCE_GAIN 0x36
+//#define EE_DISTANCE_GAIN 0x36
-#define EE_ALTITUDE_GAIN 0x38
+//#define EE_ALTITUDE_GAIN 0x38
 
 #define EE_GAIN_1 0x40	// all gains stored from here
 #define EE_GAIN_2 0x48	// all gains stored from here

ArduCopterMega/events.pde

@@ -1,3 +1,5 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
 /*
 	This event will be called when the failsafe changes
 	boolean failsafe reflects the current state
@@ -18,9 +20,9 @@ void failsafe_event()
 
 void low_battery_event(void)
 {
-	send_message(SEVERITY_HIGH,"Low Battery!");
+	gcs.send_text(SEVERITY_HIGH,"Low Battery!");
 	set_mode(RTL);
-	throttle_cruise = THROTTLE_CRUISE;
+	g.throttle_cruise.set(THROTTLE_CRUISE);
 }
 
 
@@ -42,12 +44,12 @@ undo_event
 
 void new_event(struct Location *cmd)
 {
-	Serial.print("New Event Loaded ");
+	SendDebug("New Event Loaded ");
-	Serial.println(cmd->p1,DEC);
+	SendDebugln(cmd->p1,DEC);
 
 
 	if(cmd->p1 == STOP_REPEAT){
-		Serial.println("STOP repeat ");
+		SendDebugln("STOP repeat ");
 		event_id 			= NO_REPEAT;
 		event_timer 		= -1;
 		undo_event 			= 0;
@@ -116,25 +118,26 @@ void perform_event()
 		case RELAY_TOGGLE:
 
 			event_undo_value = PORTL & B00000100 ? 1:0;
-			if(event_undo_value == 1){
+			relay_toggle();
-				relay_A();
+			SendDebug("toggle relay ");
-			}else{
+			SendDebugln(PORTL,BIN);
-				relay_B();
-			}
-			Serial.print("toggle relay ");
-			Serial.println(PORTL,BIN);
 			undo_event = 2;
 			break;
 
 	}
 }
 
-void relay_A()
+void relay_on()
 {
 	PORTL |= B00000100;
 }
 
-void relay_B()
+void relay_off()
+{
+	PORTL &= ~B00000100;
+}
+
+void relay_toggle()
 {
 	PORTL ^= B00000100;
 }
@@ -184,14 +187,9 @@ void perform_event_undo()
 			break;
 
 		case RELAY_TOGGLE:
-
+			relay_toggle();
-			if(event_undo_value == 1){
+			SendDebug("untoggle relay ");
-				relay_A();
+			SendDebugln(PORTL,BIN);
-			}else{
-				relay_B();
-			}
-			Serial.print("untoggle relay ");
-			Serial.println(PORTL,BIN);
 			break;
 	}
 }

ArduCopterMega/motors.pde

@@ -274,6 +274,6 @@ set_servos_4()
 		reset_I();
 
 		// Initialize yaw command to actual yaw when throttle is down...
-		g.rc_4.control_in = ToDeg(yaw);
+		g.rc_4.control_in = ToDeg(dcm.yaw);
 	}
  }

ArduCopterMega/read_commands.pde

@@ -79,30 +79,28 @@ void parseCommand(char *buffer)
 			case 'P':
 				g.pid_stabilize_roll.kP((float)value / 1000);
 				g.pid_stabilize_pitch.kP((float)value / 1000);
-				save_EEPROM_PID();
+				g.pid_stabilize_pitch.save_gains();
 				break;
 
 			case 'I':
 				g.pid_stabilize_roll.kI((float)value / 1000);
 				g.pid_stabilize_pitch.kI((float)value / 1000);
-				save_EEPROM_PID();
+				g.pid_stabilize_pitch.save_gains();
 				break;
 
 			case 'D':
 				//g.pid_stabilize_roll.kD((float)value / 1000);
 				//g.pid_stabilize_pitch.kD((float)value / 1000);
-				//save_EEPROM_PID();
 				break;
 	
 			case 'X':
 				g.pid_stabilize_roll.imax(value * 100);
 				g.pid_stabilize_pitch.imax(value * 100);
-				save_EEPROM_PID();
+				g.pid_stabilize_pitch.save_gains();
 				break;
 
 			case 'R':
-				stabilize_dampener = (float)value / 1000;
+				g.stabilize_dampener.set_and_save((float)value / 1000);
-				save_EEPROM_PID();
 				break;
 		}
 		init_pids();

ArduCopterMega/sensors.pde

@@ -6,7 +6,7 @@ void init_pressure_ground(void)
 	for(int i = 0; i < 300; i++){		// We take some readings...
 		delay(20);
 		APM_BMP085.Read(); 	// Get initial data from absolute pressure sensor
-		abs_pressure_ground = (abs_pressure_ground * 9l + APM_BMP085.Press) / 10l;
+		ground_pressure = (ground_pressure * 9l + APM_BMP085.Press) / 10l;
 		ground_temperature 	= (ground_temperature * 9 + APM_BMP085.Temp) / 10;		
 	}
 	abs_pressure  = APM_BMP085.Press;
@@ -19,7 +19,7 @@ void read_barometer(void){
 	
 	//abs_pressure 			= (abs_pressure + APM_BMP085.Press) >> 1;		// Small filtering
 	abs_pressure 			= ((float)abs_pressure * .7) + ((float)APM_BMP085.Press * .3);		// large filtering
-	scaling 				= (float)abs_pressure_ground / (float)abs_pressure;
+	scaling 				= (float)ground_pressure / (float)abs_pressure;
 	temp 					= ((float)ground_temperature / 10.f) + 273.15;
 	x 						= log(scaling) * temp * 29271.267f;
 	current_loc.alt 		= (long)(x / 10) + home.alt;// + baro_offset;		// Pressure altitude in centimeters

ArduCopterMega/setup.pde

@@ -224,7 +224,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 
 	
 		
-		if(frame_type == PLUS_FRAME){
+		if(g.frame_type == PLUS_FRAME){
 			if(g.rc_1.control_in > 0){
 				motor_out[CH_1] 	= out_min;
 				Serial.println("0");
@@ -243,7 +243,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 				Serial.println("2");
 			}
 
-		}else if(frame_type == X_FRAME){
+		}else if(g.frame_type == X_FRAME){
 
 			// lower right
 			if((g.rc_1.control_in > 0) 		&& (g.rc_2.control_in > 0)){
@@ -265,7 +265,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 				Serial.println("0");
 			}
 			
-		}else if(frame_type == TRI_FRAME){
+		}else if(g.frame_type == TRI_FRAME){
 
 			if(g.rc_1.control_in > 0){
 				motor_out[CH_1] 	= out_min;
@@ -293,7 +293,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 			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);
-			if(frame_type != TRI_FRAME)
+			if(g.frame_type != TRI_FRAME)
 				APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
 		}else{
 			APM_RC.OutputCh(CH_1, motor_out[CH_1]);
@@ -314,7 +314,7 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
 	Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n"));
 	
 	imu.init_accel();
-	imu.print_accel_offsets();
+	print_accel_offsets();
 
 	report_imu();
 	return(0);
@@ -332,7 +332,7 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
 		save_EEPROM_PID();
 	
 	}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) {
-		stabilize_dampener = argv[2].f;
+		g.stabilize_dampener = argv[2].f;
 		save_EEPROM_PID();
 
 	}else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) {
@@ -412,8 +412,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_declination(uint8_t argc, const Menu::arg *argv)
 {
-	mag_declination = argv[1].f;
+	compass.set_declination(radians(argv[1].f));
-	save_EEPROM_mag_declination();
 	report_compass();
 }
 
@@ -449,16 +448,16 @@ static int8_t
 setup_frame(uint8_t argc, const Menu::arg *argv)
 {
 	if (!strcmp_P(argv[1].str, PSTR("+"))) {
-		frame_type = PLUS_FRAME;
+		g.frame_type = PLUS_FRAME;
 
 	} else if (!strcmp_P(argv[1].str, PSTR("x"))) {
-		frame_type = X_FRAME;
+		g.frame_type = X_FRAME;
 	
 	} else if (!strcmp_P(argv[1].str, PSTR("tri"))) {
-		frame_type = TRI_FRAME;
+		g.frame_type = TRI_FRAME;
 
 	} else if (!strcmp_P(argv[1].str, PSTR("hexa"))) {
-		frame_type = HEXA_FRAME;
+		g.frame_type = HEXA_FRAME;
 
 	} else {
 		Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa]\n"));
@@ -475,11 +474,11 @@ static int8_t
 setup_current(uint8_t argc, const Menu::arg *argv)
 {
 	if (!strcmp_P(argv[1].str, PSTR("on"))) {
-		current_enabled = true;
+		g.current_enabled.set(true);
 		save_EEPROM_mag();
 	
 	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
-		current_enabled = false;
+		g.current_enabled.set(false);
 		save_EEPROM_mag();
 	
 	} else if(argv[1].i > 10){
@@ -599,14 +598,14 @@ default_nav()
 void
 default_alt_hold()
 {
-	alt_to_hold = -1;
+	g.RTL_altitude.set(-1);
 	save_EEPROM_alt_RTL();
 }
 
 void
 default_frame()
 {
-	frame_type = PLUS_FRAME;
+	g.frame_type = PLUS_FRAME;
 	save_EEPROM_frame();
 }
 
@@ -614,7 +613,7 @@ void
 default_current()
 {
 	milliamp_hours 		= 2000;
-	current_enabled 	= false;
+	g.current_enabled.set(false);
 	save_EEPROM_current();
 }
 
@@ -633,12 +632,12 @@ default_flight_modes()
 void
 default_throttle()
 {
-	g.throttle_min				= THROTTLE_MIN;
+	g.throttle_min					= THROTTLE_MIN;
-	g.throttle_max				= THROTTLE_MAX;
+	g.throttle_max					= THROTTLE_MAX;
-	g.throttle_cruise			= THROTTLE_CRUISE;
+	g.throttle_cruise				= THROTTLE_CRUISE;
-	throttle_failsafe_enabled	= THROTTLE_FAILSAFE;
+	g.throttle_failsafe_enabled		= THROTTLE_FAILSAFE;
-	throttle_failsafe_action	= THROTTLE_FAILSAFE_ACTION;
+	g.throttle_failsafe_action		= THROTTLE_FAILSAFE_ACTION;
-	throttle_failsafe_value		= THROTTLE_FS_VALUE;
+	g.throttle_failsafe_value		= THROTTLE_FS_VALUE;
 	save_EEPROM_throttle();
 }
 
@@ -704,10 +703,10 @@ default_gains()
 
 	// custom dampeners
 	// roll pitch
-	stabilize_dampener 	= STABILIZE_DAMPENER;
+	g.stabilize_dampener 	= STABILIZE_DAMPENER;
 	
 	//yaw
-	hold_yaw_dampener	= HOLD_YAW_DAMPENER;
+	g.hold_yaw_dampener	= HOLD_YAW_DAMPENER;
 
 
 	// navigation
@@ -746,7 +745,7 @@ void report_current()
 	read_EEPROM_current();
 	Serial.printf_P(PSTR("Current Sensor\n"));
 	print_divider();
-	print_enabled(current_enabled);
+	print_enabled(g.current_enabled.get());
 
 	Serial.printf_P(PSTR("mah: %d"),milliamp_hours);
 	print_blanks(1);
@@ -761,16 +760,16 @@ void report_frame()
 	print_divider();
 
 	
-	if(frame_type == X_FRAME)
+	if(g.frame_type == X_FRAME)
 		Serial.printf_P(PSTR("X "));
-	else if(frame_type == PLUS_FRAME)
+	else if(g.frame_type == PLUS_FRAME)
 		Serial.printf_P(PSTR("Plus "));
-	else if(frame_type == TRI_FRAME)
+	else if(g.frame_type == TRI_FRAME)
 		Serial.printf_P(PSTR("TRI "));
-	else if(frame_type == HEXA_FRAME)
+	else if(g.frame_type == HEXA_FRAME)
 		Serial.printf_P(PSTR("HEXA "));
 
-	Serial.printf_P(PSTR("frame (%d)"), (int)frame_type);
+	Serial.printf_P(PSTR("frame (%d)"), (int)g.frame_type);
 	print_blanks(1);
 }
 
@@ -808,8 +807,8 @@ void report_gains()
 	Serial.printf_P(PSTR("yaw:\n"));
 	print_PID(&g.pid_yaw);
 	
-	Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), stabilize_dampener);
+	Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), g.stabilize_dampener);	
-	Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), hold_yaw_dampener);
+	Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), g.hold_yaw_dampener);
 	
 	// Nav
 	Serial.printf_P(PSTR("Nav:\nlat:\n"));
@@ -833,9 +832,9 @@ void report_xtrack()
 	Serial.printf_P(PSTR("XTRACK: %4.2f\n"
 						 "XTRACK angle: %d\n"
 						 "PITCH_MAX: %ld"),
-						 (float)g.crosstrack_gain,
+						 g.crosstrack_gain,
-						 (int)g.crosstrack_entry_angle,
+						 g.crosstrack_entry_angle,
-						 (long)g.pitch_max);
+						 g.pitch_max);
 	print_blanks(1);
 }
 
@@ -851,11 +850,11 @@ void report_throttle()
 						 "cruise: %d\n"
 						 "failsafe_enabled: %d\n"
 						 "failsafe_value: %d"),
-						 (int)g.throttle_min,
+						 g.throttle_min,
-						 (int)g.throttle_max,
+						 g.throttle_max,
-						 (int)g.throttle_cruise,
+						 g.throttle_cruise,
-						 throttle_failsafe_enabled,
+						 g.throttle_failsafe_enabled,
-						 throttle_failsafe_value);
+						 g.throttle_failsafe_value);
 	print_blanks(1);
 }
 
@@ -865,8 +864,8 @@ void report_imu()
 	Serial.printf_P(PSTR("IMU\n"));
 	print_divider();
 
-	imu.print_gyro_offsets();
+	print_gyro_offsets();
-	imu.print_accel_offsets();
+	print_accel_offsets();
 	print_blanks(1);
 }
 
@@ -877,20 +876,22 @@ void report_compass()
 	print_divider();
 
 	read_EEPROM_compass();
-	read_EEPROM_compass_declination();
+	//read_EEPROM_compass_declination();
-	read_EEPROM_compass_offset();
+	//read_EEPROM_compass_offset();
 
 	print_enabled(g.compass_enabled);
 	
 	// mag declination
 	Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), 
-							mag_declination);
+							degrees(compass.get_declination()));
+	
+	Vector3f offsets = compass.get_offsets();
 
 	// mag offsets
 	Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"),
-							mag_offset_x,
+							offsets.x,
-							mag_offset_y,
+							offsets.y,
-							mag_offset_z);
+							offsets.z);
 	print_blanks(1);
 }
 
@@ -921,14 +922,14 @@ print_PID(PID * pid)
 void 
 print_radio_values()
 {
-	Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
+	Serial.printf_P(PSTR("CH1: %d | %d\n"), g.rc_1.radio_min, g.rc_1.radio_max);
-	Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
+	Serial.printf_P(PSTR("CH2: %d | %d\n"), g.rc_2.radio_min, g.rc_2.radio_max);
-	Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
+	Serial.printf_P(PSTR("CH3: %d | %d\n"), g.rc_3.radio_min, g.rc_3.radio_max);
-	Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
+	Serial.printf_P(PSTR("CH4: %d | %d\n"), g.rc_4.radio_min, g.rc_4.radio_max);
-	Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
+	Serial.printf_P(PSTR("CH5: %d | %d\n"), g.rc_5.radio_min, g.rc_5.radio_max);
-	Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
+	Serial.printf_P(PSTR("CH6: %d | %d\n"), g.rc_6.radio_min, g.rc_6.radio_max);
-	Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
+	Serial.printf_P(PSTR("CH7: %d | %d\n"), g.rc_7.radio_min, g.rc_7.radio_max);
-	Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
+	Serial.printf_P(PSTR("CH8: %d | %d\n"), g.rc_8.radio_min, g.rc_8.radio_max);
 }
 
 void
@@ -1005,3 +1006,26 @@ void print_enabled(boolean b)
 
 
 
+void
+print_accel_offsets(void)
+{
+	//Serial.print("Accel offsets: ");
+	//Serial.print(imu._adc_offset[3], 2);
+	//Serial.print(", ");
+	//Serial.print(_adc_offset[4], 2);
+	//Serial.print(", ");
+	//Serial.println(_adc_offset[5], 2);
+}
+
+void
+print_gyro_offsets(void)
+{
+	//Serial.print("Gyro offsets: ");
+	//Serial.print(_adc_offset[0], 2);
+	//Serial.print(", ");
+	//Serial.print(_adc_offset[1], 2);
+	//Serial.print(", ");
+	//Serial.println(_adc_offset[2], 2);
+}
+
+

ArduCopterMega/system.pde

@@ -85,7 +85,7 @@ void init_ardupilot()
 #endif
 
 	Serial.printf_P(PSTR("\n\n"
-						 "Init ArduCopterMega 1.0.2 Public Alpha\n\n"
+						 "Init ArduCopterMega 1.0.3 Public Alpha\n\n"
 #if TELEMETRY_PORT == 3
 						 "Telemetry is on the xbee port\n"
 #endif
@@ -126,6 +126,9 @@ void init_ardupilot()
 	g_gps = &GPS;
 	g_gps->init();
 	
+	imu.init(IMU::WARM_START);	// offsets are loaded from EEPROM
+
+
 	if(g.compass_enabled)
 		init_compass();
 
@@ -243,7 +246,7 @@ void startup_ground(void)
 
 	// Warm up and read Gyro offsets
 	// -----------------------------
-	imu.init_gyro();
+	imu.init(IMU::COLD_START);
 
 	// Save the settings for in-air restart
 	// ------------------------------------
@@ -403,10 +406,9 @@ void
 init_compass()
 {
 	dcm.set_compass(&compass);
-	compass.init();
+	bool junkbool = compass.init();
 	compass.set_orientation(MAGORIENTATION);						// set compass's orientation on aircraft
-	compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);	// set offsets to account for surrounding interference
+	Vector3f junkvector = compass.get_offsets();						// load offsets to account for airframe magnetic interference	
-	compass.set_declination(ToRad(mag_declination));					// set local difference between magnetic north and true north
 }
 
 

ArduCopterMega/test.pde

@@ -686,15 +686,15 @@ test_relay(uint8_t argc, const Menu::arg *argv)
 	delay(1000);
 
 	while(1){
-		Serial.println("Relay A");
+		Serial.println("Relay on");
-		relay_A();
+		relay_on();
 		delay(3000);
 		if(Serial.available() > 0){
 			return (0);
 		}
 
-		Serial.println("Relay B");
+		Serial.println("Relay off");
-		relay_B();
+		relay_off();
 		delay(3000);
 		if(Serial.available() > 0){
 			return (0);
@@ -764,7 +764,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 			sum += abs_pressure;
 		delay(10);
 	}
-	abs_pressure_ground = (float)sum / 100.0;
+	ground_pressure = (float)sum / 100.0;
 	*/
 
 	home.alt = 0;
@@ -795,7 +795,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 						current_loc.alt,
 						next_WP.alt,
 						altitude_error,
-						g.,
+						g.throttle_cruise,
 						g.rc_3.servo_out);
 
 			/*