diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h
index 57399259ba..a202d8000c 100644
--- a/ArduCopterMega/APM_Config.h
+++ b/ArduCopterMega/APM_Config.h
@@ -5,8 +5,8 @@
 #define GPS_PROTOCOL  		GPS_PROTOCOL_MTK
 #define GCS_PROTOCOL        GCS_PROTOCOL_NONE
 
-//#define MAGORIENTATION	AP_COMPASS_COMPONENTS_UP_PINS_BACK
-#define MAGORIENTATION	AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
+#define MAGORIENTATION	AP_COMPASS_COMPONENTS_UP_PINS_BACK
+//#define MAGORIENTATION	AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
 #define ARM_AT_STARTUP	0
 
@@ -19,3 +19,6 @@
 //#define ENABLE_AM         ENABLED
 //#define ENABLE_xx         ENABLED
 
+
+// Logging
+#define LOG_CURRENT ENABLED
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 43b0bf15e3..231d3a769b 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -223,6 +223,11 @@ float 	battery_voltage2 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 +
 float 	battery_voltage3 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
 float 	battery_voltage4 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
 
+float 	current_voltage 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
+float	current_amps;
+
+boolean	current_sensor		= false;
+
 // Magnetometer variables
 // ----------------------
 int 	magnetom_x;
@@ -556,12 +561,18 @@ void medium_loop()
 			if (log_bitmask & MASK_LOG_GPS)
 				Log_Write_GPS(GPS.time, current_loc.lat, current_loc.lng, GPS.altitude, current_loc.alt, (long) GPS.ground_speed, GPS.ground_course, GPS.fix, GPS.num_sats);
 
+			if (log_bitmask & MASK_LOG_CUR)
+				Log_Write_Current();
+
 			send_message(MSG_ATTITUDE);		// Sends attitude data
 			break;
 			
 		// This case controls the slow loop
 		//---------------------------------
 		case 4:
+			if (current_sensor){
+				read_current();
+			}
 			
 			// shall we trim the copter?
 			// ------------------------
diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde
index e40d620d8c..27006f475e 100644
--- a/ArduCopterMega/EEPROM.pde
+++ b/ArduCopterMega/EEPROM.pde
@@ -203,6 +203,18 @@ void read_EEPROM_mag_declination(void)
 
 /********************************************************************************/
 
+void save_EEPROM_current_sensor(void)
+{
+	eeprom_write_byte((uint8_t *)	EE_CURRENT_SENSOR, 	current_sensor);
+}
+
+void read_EEPROM_current_sensor(void)
+{
+	current_sensor	= eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR);
+}
+
+/********************************************************************************/
+
 void save_EEPROM_mag_offset(void)
 {
 	write_EE_compressed_float(mag_offset_x, 	EE_MAG_X, 2);
diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index cd54352d68..7392701298 100644
--- a/ArduCopterMega/Log.pde
+++ b/ArduCopterMega/Log.pde
@@ -69,6 +69,7 @@ print_log_menu(void)
 		PLOG(MODE);
 		PLOG(RAW);
 		PLOG(CMD);
+		PLOG(CURRENT);
 #undef PLOG
 	}
 	Serial.println();
@@ -165,6 +166,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
 		TARG(MODE);
 		TARG(RAW);
 		TARG(CMD);
+		TARG(CURRENT);
 #undef TARG
 	}
 
@@ -338,6 +340,26 @@ void Log_Write_Raw()
 	DataFlash.WriteByte(END_BYTE);
 }
 
+void Log_Write_Current()
+{
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_CURRENT_MSG);
+	DataFlash.WriteLong((long)(current_voltage * 1000.0));
+	DataFlash.WriteLong((long)(current_amps * 1000.0));
+	DataFlash.WriteByte(END_BYTE);
+}
+// Read a Current packet
+void Log_Read_Current()
+{
+	float logvoltage, logcurrent;
+	Serial.print("CURR:");
+	Serial.print((float)DataFlash.ReadLong() / 1000.f);
+	Serial.print(comma);
+	Serial.print((float)DataFlash.ReadLong() / 1000.f);
+	Serial.println();
+}
+
 // Read an control tuning packet
 void Log_Read_Control_Tuning()
 {	
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 4ea2eb05d6..c759110a4b 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -111,11 +111,17 @@
 # define BATTERY_TYPE			0
 #endif
 #ifndef LOW_VOLTAGE
-# define LOW_VOLTAGE			11.4
+# define LOW_VOLTAGE			9.6
 #endif
 #ifndef VOLT_DIV_RATIO
 # define VOLT_DIV_RATIO			3.0
 #endif
+#ifndef CURR_VOLT_DIV_RATIO
+# define CURR_VOLT_DIV_RATIO	15.7
+#endif
+#ifndef CURR_AMP_DIV_RATIO
+# define CURR_AMP_DIV_RATIO		30.35
+#endif
 
 
 //////////////////////////////////////////////////////////////////////////////
@@ -458,6 +464,9 @@
 #ifndef LOG_CMD
 # define LOG_CMD				ENABLED
 #endif
+#ifndef LOG_CURRENT
+# define LOG_CURRENT			DISABLED
+#endif
 
 
 
diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index 049d634053..da9f0f01e5 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -171,19 +171,21 @@
 #define LOG_PERFORMANCE_MSG		0X06
 #define LOG_RAW_MSG				0x07
 #define LOG_CMD_MSG				0x08
-#define LOG_STARTUP_MSG			0x09
+#define LOG_CURRENT_MSG 		0x09
+#define LOG_STARTUP_MSG 		0x0A
 #define TYPE_AIRSTART_MSG		0x00
 #define TYPE_GROUNDSTART_MSG	0x01
 
-#define MASK_LOG_ATTITUDE_FAST 0
-#define MASK_LOG_ATTITUDE_MED 2
-#define MASK_LOG_GPS 4
-#define MASK_LOG_PM 8
-#define MASK_LOG_CTUN 16
-#define MASK_LOG_NTUN 32
-#define MASK_LOG_MODE 64
-#define MASK_LOG_RAW 128
-#define MASK_LOG_CMD 256
+#define MASK_LOG_ATTITUDE_FAST 	(1<<0)
+#define MASK_LOG_ATTITUDE_MED 	(1<<1)
+#define MASK_LOG_GPS 			(1<<2)
+#define MASK_LOG_PM 			(1<<3)
+#define MASK_LOG_CTUN 			(1<<4)
+#define MASK_LOG_NTUN			(1<<5)
+#define MASK_LOG_MODE			(1<<6)
+#define MASK_LOG_RAW			(1<<7)
+#define MASK_LOG_CMD			(1<<8)
+#define MASK_LOG_CUR			(1<<9)
 
 // Waypoint Modes
 // ----------------
@@ -205,13 +207,20 @@
 
 #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	AIRSPEED_CH 7			// The external ADC channel for the airspeed sensor
 #define BATTERY_PIN1 0		        // These are the pins for the voltage dividers
 #define BATTERY_PIN2 1
 #define BATTERY_PIN3 2
 #define BATTERY_PIN4 3
+#define CURRENT_PIN1 4 // These are the pins for current sensor: voltage
+#define CURRENT_PIN2 5 // and current
+
 #define RELAY_PIN 47
 
+
 // sonar
 #define SonarToCm(x) (x*1.26)   // Sonar raw value to centimeters
 
@@ -301,6 +310,7 @@
 #define EE_GND_TEMP 0x11A
 #define EE_GND_ALT 0x11C
 #define EE_AP_OFFSET 0x11E
+#define EE_CURRENT_SENSOR 0x127
 
 // log
 #define EE_LAST_LOG_PAGE 0xE00
@@ -317,4 +327,5 @@
 #define LOGBIT_MODE				(1<<6)
 #define LOGBIT_RAW				(1<<7)
 #define LOGBIT_CMD				(1<<8)
+#define LOGBIT_CURRENT			(1<<9)
 
diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde
index f2afa4469d..b0c4506d48 100644
--- a/ArduCopterMega/sensors.pde
+++ b/ArduCopterMega/sensors.pde
@@ -38,18 +38,25 @@ void read_battery(void)
 	battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
 	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)
+		if(battery_voltage3 < LOW_VOLTAGE)
 			low_battery_event();
 		battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
 	#endif
-
+	
 	#if BATTERY_TYPE == 1
 		if(battery_voltage4 < LOW_VOLTAGE)
 			low_battery_event();
 		battery_voltage = battery_voltage4; // set total battery voltage, for telemetry stream
-	#endif	
+	#endif
 }
 #endif
 
+
+
+void read_current(void)
+{
+	current_voltage 	= CURRENT_VOLTAGE(analogRead(CURRENT_PIN1)) * .1 + battery_voltage1 * .9; //reads power sensor voltage pin
+	current_amps 		= CURRENT_AMPS(analogRead(CURRENT_PIN2)) * .1 + battery_voltage2 * .9; //reads power sensor current pin
+}
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index 07a82bb793..faee998ea2 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -79,6 +79,16 @@ setup_show(uint8_t argc, const Menu::arg *argv)
 	Serial.printf_P(PSTR("(%d)\n"), (int)frame_type);
 		
 	
+	print_blanks(2);
+	read_EEPROM_current_sensor();
+	Serial.printf_P(PSTR("Current Sensor "));
+	if(current_sensor){
+		Serial.printf_P(PSTR("enabled\n"));
+	} else {
+		Serial.printf_P(PSTR("disabled\n"));
+	}
+	
+	
 	print_blanks(2);
 	Serial.printf_P(PSTR("Gains\n"));
 	print_divider();
@@ -240,7 +250,8 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 		LOGBIT(NTUN)			|
 		LOGBIT(MODE)			|
 		LOGBIT(RAW)				|
-		LOGBIT(CMD);
+		LOGBIT(CMD)				|
+		LOGBIT(CURRENT);
 	#undef LOGBIT
 	save_EEPROM_configs();
 	print_done();
@@ -297,7 +308,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 	rc_6.radio_trim = 1500;
 	rc_7.radio_trim = 1500;
 	rc_8.radio_trim = 1500;
-			
+
+
 	Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: "));
 	while(1){
 		
@@ -386,7 +398,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 				Serial.println("2");
 			}
 
-		}else if(frame_type == PLUS_FRAME){
+		}else if(frame_type == X_FRAME){
 
 			// lower right
 			if((rc_1.control_in > 0) 		&& (rc_2.control_in > 0)){
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 5546f8adab..1fb8be8ada 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -234,7 +234,7 @@ void startup_ground(void)
 
 	// read the radio to set trims
 	// ---------------------------
-	trim_yaw();
+	trim_radio();
 
 	// Warm up and read Gyro offsets
 	// -----------------------------
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 0b1ad4f5fa..5c25fd77f9 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -11,6 +11,7 @@ static int8_t	test_gyro(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_dcm(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_omega(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_battery(uint8_t argc, 		const Menu::arg *argv);
+static int8_t	test_current(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_relay(uint8_t argc,	 	const Menu::arg *argv);
 static int8_t	test_wp(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_pressure(uint8_t argc, 	const Menu::arg *argv);
@@ -50,6 +51,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"dcm",			test_dcm},
 	{"omega",		test_omega},
 	{"battery",		test_battery},
+	{"current",		test_current},
 	{"relay",		test_relay},
 	{"waypoints",	test_wp},
 	{"airpressure",	test_pressure},
@@ -609,6 +611,29 @@ test_battery(uint8_t argc, const Menu::arg *argv)
 	return (0);
 }
 
+static int8_t
+test_current(uint8_t argc, const Menu::arg *argv)
+{
+	print_hit_enter();
+	while(1){
+		for (int i = 0; i < 20; i++){
+			delay(20);
+			read_current();
+		}
+		Serial.printf_P(PSTR("Volts:"));
+		Serial.print(battery_voltage1, 2); //power sensor voltage pin
+		Serial.print(" Amps:");
+		Serial.println(battery_voltage2, 2); //power sensor current pin
+		
+		if(Serial.available() > 0){
+			return (0);
+		}
+	}
+}
+
+
+
+
 
 static int8_t
 test_relay(uint8_t argc, const Menu::arg *argv)