diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h
index 13dac48ee1..400c72dc27 100644
--- a/ArduCopterMega/APM_Config.h
+++ b/ArduCopterMega/APM_Config.h
@@ -4,6 +4,7 @@
 
 // GPS is auto-selected
 
+//#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
 #define NAV_TEST 0	// 0 = traditional, 1 = rate controlled
 
@@ -37,7 +38,7 @@
 # define LOG_CTUN				ENABLED
 # define LOG_NTUN				ENABLED
 # define LOG_MODE				ENABLED
-# define LOG_RAW				DISABLED
+# define LOG_RAW				ENABLED
 # define LOG_CMD				ENABLED
 # define LOG_CURRENT			DISABLED
 
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index ec72214498..d46bf9d363 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -669,8 +669,8 @@ void medium_loop()
 		case 4:
 			medium_loopCounter = 0;
 
-			if (g.current_enabled){
-				read_current();
+			if (g.battery_monitoring != 0){
+				read_battery();
 			}
 
 			// Accel trims 		= hold > 2 seconds
diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index 0fe1b7131d..07bb0e3067 100644
--- a/ArduCopterMega/Log.pde
+++ b/ArduCopterMega/Log.pde
@@ -362,6 +362,25 @@ void Log_Write_Nav_Tuning()
 	DataFlash.WriteByte(END_BYTE);
 }
 
+/*
+Matrix3f tempmat = dcm.get_dcm_matrix();
+
+DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
+DataFlash.WriteInt((int)wp_distance);
+DataFlash.WriteInt((uint16_t)target_bearing);
+DataFlash.WriteInt((uint16_t)nav_bearing);
+DataFlash.WriteInt(altitude_error);
+DataFlash.WriteInt((int)airspeed);
+DataFlash.WriteInt((int)(nav_gain_scaler*1000));
+DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack
+DataFlash.WriteLong(compass.last_update); // Just a temp hack
+DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack
+DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack
+DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack
+DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack
+*/
+
+
     //	1	2	3	4  5  6	 7	 8	   9	  10   11
 //NTUN, 236, 0, 132, 10, 0, 0, 29, 2963, 16545, 16682, 108
 
@@ -387,6 +406,7 @@ void Log_Read_Nav_Tuning()
 }
 
 
+
 // Write a mode packet. Total length : 5 bytes
 void Log_Write_Mode(byte mode)
 {
diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h
index bebd8e4de8..3d501302d8 100644
--- a/ArduCopterMega/Parameters.h
+++ b/ArduCopterMega/Parameters.h
@@ -61,8 +61,8 @@ public:
         k_param_IMU_calibration = 140,
         k_param_ground_temperature,
         k_param_ground_pressure,
-		k_param_current,
-		k_param_milliamp_hours,
+		k_param_battery_monitoring,
+		k_param_pack_capacity,
 		k_param_compass_enabled,
 		k_param_compass,
 		k_param_sonar,
@@ -183,8 +183,8 @@ public:
     AP_Int8		frame_type;
 
     AP_Int8		sonar_enabled;
-    AP_Int8		current_enabled;
-    AP_Int16	milliamp_hours;
+    AP_Int8		battery_monitoring;	// 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
+	AP_Int16	pack_capacity;		// Battery pack capacity less reserve
     AP_Int8		compass_enabled;
 	AP_Int8		esc_calibrate;
 
@@ -229,8 +229,8 @@ public:
         frame_type  			(FRAME_CONFIG,				k_param_frame_type,						PSTR("FRAME_CONFIG")),
 
         sonar_enabled  			(DISABLED,					k_param_sonar,							PSTR("SONAR_ENABLE")),
-        current_enabled  		(DISABLED,					k_param_current,						PSTR("CURRENT_ENABLE")),
-        milliamp_hours  		(CURR_AMP_HOURS,			k_param_milliamp_hours,					PSTR("MAH")),
+        battery_monitoring 		(DISABLED,					k_param_battery_monitoring,				PSTR("BATT_MONITOR")),
+        pack_capacity	 		(HIGH_DISCHARGE,			k_param_pack_capacity,					PSTR("BATT_CAPACITY")),
         compass_enabled			(MAGNETOMETER,				k_param_compass_enabled,				PSTR("MAG_ENABLE")),
 
         waypoint_mode           (0,                         k_param_waypoint_mode,          		PSTR("WP_MODE")),
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 9f7ce82be3..543c115267 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -77,14 +77,16 @@
 #define HIL_MODE	HIL_MODE_DISABLED
 #endif
 
- #ifndef HIL_PROTOCOL
+#ifndef HIL_PROTOCOL
 #define HIL_PROTOCOL	HIL_PROTOCOL_MAVLINK
- #endif
+#endif
 
- #ifndef HIL_PORT
+#ifndef HIL_PORT
 #define HIL_PORT 0
- #endif
+#endif
+
 #if HIL_MODE != HIL_MODE_DISABLED	// we are in HIL mode
+
  # undef GPS_PROTOCOL
  # define GPS_PROTOCOL GPS_PROTOCOL_NONE
 
@@ -131,6 +133,10 @@
 # define GCS_PORT			3
 #endif
 
+#ifndef MAV_SYSTEM_ID
+# define MAV_SYSTEM_ID		1
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Serial port speeds.
 //
@@ -148,27 +154,23 @@
 #ifndef BATTERY_EVENT
 # define BATTERY_EVENT			DISABLED
 #endif
-#ifndef BATTERY_TYPE
-# define BATTERY_TYPE			0
-#endif
 #ifndef LOW_VOLTAGE
 # define LOW_VOLTAGE			9.6
 #endif
 #ifndef VOLT_DIV_RATIO
-# define VOLT_DIV_RATIO			3.0
+# define VOLT_DIV_RATIO			3.56
 #endif
 
-#ifndef CURR_VOLT_DIV_RATIO
-# define CURR_VOLT_DIV_RATIO	15.7
+#ifndef CURR_AMP_PER_VOLT
+# define CURR_AMP_PER_VOLT		27.32
 #endif
-#ifndef CURR_AMP_DIV_RATIO
-# define CURR_AMP_DIV_RATIO		30.35
+#ifndef CURR_AMPS_OFFSET
+# define CURR_AMPS_OFFSET		0.0
 #endif
-#ifndef CURR_AMP_HOURS
-# define CURR_AMP_HOURS		2000
+#ifndef HIGH_DISCHARGE
+# define HIGH_DISCHARGE		1760
 #endif
 
-
 //////////////////////////////////////////////////////////////////////////////
 // INPUT_VOLTAGE
 //
diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index 1f70708442..17cc56a771 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -266,9 +266,7 @@
 
 
 #define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
-
-#define CURRENT_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_VOLT_DIV_RATIO
-#define CURRENT_AMPS(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_AMP_DIV_RATIO
+#define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT
 
 #define	AIRSPEED_CH 7			// The external ADC channel for the airspeed sensor
 #define BATTERY_PIN1 0		        // These are the pins for the voltage dividers
diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde
index 7234fad6c1..7fa9da899b 100644
--- a/ArduCopterMega/sensors.pde
+++ b/ArduCopterMega/sensors.pde
@@ -86,7 +86,6 @@ void zero_airspeed(void)
 
 #endif // HIL_MODE != HIL_MODE_ATTITUDE
 
-#if BATTERY_EVENT == 1
 void read_battery(void)
 {
 	battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
@@ -94,27 +93,21 @@ void read_battery(void)
 	battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
 	battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
 
-	#if BATTERY_TYPE == 0
-		if(battery_voltage3 < LOW_VOLTAGE)
-			low_battery_event();
+	if(g.battery_monitoring == 1)
 		battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
-	#endif
+	if(g.battery_monitoring == 2)
+		battery_voltage = battery_voltage4;
+	if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
+		battery_voltage = battery_voltage1;
+	if(g.battery_monitoring == 4) {
+		current_amps	 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
+		current_total	 += current_amps * (float)delta_ms_medium_loop * 0.000278;
+	}
 
-	#if BATTERY_TYPE == 1
-		if(battery_voltage4 < LOW_VOLTAGE)
-			low_battery_event();
-		battery_voltage = battery_voltage4; // set total battery voltage, for telemetry stream
+	#if BATTERY_EVENT == 1
+		if(battery_voltage < LOW_VOLTAGE)	low_battery_event();
+		if(g.battery_monitoring == 4 && current_total > g.pack_capacity)	low_battery_event();
 	#endif
 }
-#endif
-
-
-void read_current(void)
-{
-	current_voltage 	= CURRENT_VOLTAGE(analogRead(VOLTAGE_PIN_0)) * .1 	+ current_voltage * .9; //reads power sensor voltage pin
-	current_amps 		= CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 		+ current_amps * .9; //reads power sensor current pin
-	current_total		+= (current_amps * 0.27777) / delta_ms_medium_loop;
-}
-
 
 //v: 10.9453, a: 17.4023, mah: 8.2
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index bea8450398..92c7ee9dae 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -9,7 +9,7 @@ static int8_t	setup_erase				(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_flightmodes		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_pid				(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_frame				(uint8_t argc, const Menu::arg *argv);
-static int8_t	setup_current			(uint8_t argc, const Menu::arg *argv);
+static int8_t	setup_batt_monitor		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_sonar				(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_compass			(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_declination		(uint8_t argc, const Menu::arg *argv);
@@ -29,7 +29,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"level",			setup_accel},
 	{"modes",			setup_flightmodes},
 	{"frame",			setup_frame},
-	{"current",			setup_current},
+	{"battery",			setup_batt_monitor},
 	{"sonar",			setup_sonar},
 	{"compass",			setup_compass},
 	{"declination",		setup_declination},
@@ -73,7 +73,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
 	report_version();
 	report_radio();
 	report_frame();
-	report_current();
+	report_batt_monitor();
 	report_sonar();
 	report_gains();
 	report_xtrack();
@@ -521,24 +521,16 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
 }
 
 static int8_t
-setup_current(uint8_t argc, const Menu::arg *argv)
+setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
 {
-	if (!strcmp_P(argv[1].str, PSTR("on"))) {
-		g.current_enabled.set_and_save(true);
+	if(argv[1].i >= 0 && argv[1].i <= 4){
+		g.battery_monitoring.set_and_save(argv[1].i);
 
-	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
-		g.current_enabled.set_and_save(false);
-
-	} else if(argv[1].i > 10){
-		g.milliamp_hours.set_and_save(argv[1].i);
-
-	}else{
-		Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n"));
-		report_current();
-		return 0;
+	} else {
+		Serial.printf_P(PSTR("\nOptions: 0-4"));
 	}
 
-	report_current();
+	report_batt_monitor();
 	return 0;
 }
 
@@ -674,13 +666,14 @@ default_frame()
 	g.frame_type.set_and_save(PLUS_FRAME);
 }
 
-void
+/*void
 default_current()
 {
 	g.milliamp_hours 		= 2000;
 	g.current_enabled.set(false);
 	save_EEPROM_current();
 }
+*/
 
 void
 default_flight_modes()
@@ -805,6 +798,19 @@ default_gains()
 /***************************************************************************/
 // CLI reports
 /***************************************************************************/
+
+void report_batt_monitor()
+{
+	//print_blanks(2);
+	Serial.printf_P(PSTR("Batt Mointor\n"));
+	print_divider();
+	if(g.battery_monitoring == 0)	Serial.printf_P(PSTR("Batt mon. disabled"));
+	if(g.battery_monitoring == 1)	Serial.printf_P(PSTR("3 cells"));
+	if(g.battery_monitoring == 2)	Serial.printf_P(PSTR("4 cells"));
+	if(g.battery_monitoring == 3)	Serial.printf_P(PSTR("batt volts"));
+	if(g.battery_monitoring == 4)	Serial.printf_P(PSTR("volts and cur"));
+	print_blanks(2);
+}
 void report_wp(byte index = 255)
 {
 	if(index == 255){
@@ -830,6 +836,7 @@ void print_wp(struct Location *cmd, byte index)
 		cmd->lng);
 }
 
+/*
 void report_current()
 {
 	//read_EEPROM_current();
@@ -840,7 +847,7 @@ void report_current()
 	Serial.printf_P(PSTR("mah: %d"),(int)g.milliamp_hours.get());
 	print_blanks(2);
 }
-
+*/
 void report_gps()
 {
 	Serial.printf_P(PSTR("\nGPS\n"));
@@ -1242,10 +1249,10 @@ void save_EEPROM_PID(void)
 
 /********************************************************************************/
 
-void save_EEPROM_current(void)
+/*void save_EEPROM_current(void)
 {
 	g.current_enabled.save();
-	g.milliamp_hours.save();
+	//g.milliamp_hours.save();
 }
 
 void read_EEPROM_current(void)
@@ -1253,7 +1260,7 @@ void read_EEPROM_current(void)
 	g.current_enabled.load();
 	g.milliamp_hours.load();
 }
-
+*/
 /********************************************************************************/
 
 void read_EEPROM_radio(void)
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 66b7e2748f..24fbe2c811 100644
--- a/ArduCopterMega/system.pde
+++ b/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 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.1 Beta", main_menu_commands);
 
 void init_ardupilot()
 {
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 8862bea980..00eb025548 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -652,18 +652,16 @@ test_current(uint8_t argc, const Menu::arg *argv)
 	while(1){
 		delay(100);
 		read_radio();
-		read_current();
+		read_battery();
 		Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
 						current_voltage,
 						current_amps,
 						current_total);
 
-		//if(g.rc_3.control_in > 0){
-			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);
-			APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
-		//}
+		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);
+		APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
 
 		if(Serial.available() > 0){
 			return (0);