Added back in Mag offsets in CLI. Don' t know if it works yet!

removed CH_3 refs in Tri, fixed Yaw repoting bug for unsigned int

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

ArduCopterMega/Log.pde

@@ -99,8 +99,8 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 
 	if (/*(argc != 2) || */(dump_log < 1) || (dump_log > last_log_num)) {
 		Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
-		//return(-1);
-		Log_Read(1, 4095);
+		return(-1);
+		//Log_Read(1, 4095);
 		return (0);
 	}
 
@@ -380,7 +380,7 @@ void Log_Read_GPS()
 {
 	Serial.printf_P(PSTR("GPS, %ld, %d, %d, "
 					  "%4.7f, %4.7f, %4.4f, %4.4f, "
-					  "%d, %d\n"),
+					  "%d, %u\n"),
 
 						DataFlash.ReadLong(),					// 1 time
 						(int)DataFlash.ReadByte(),				// 2 fix

ArduCopterMega/motors_tri.pde

@@ -31,14 +31,12 @@ void output_motors_armed()
 	// limit output so motors don't stop
 	motor_out[CH_1]		= max(motor_out[CH_1], 	out_min);
 	motor_out[CH_2]		= max(motor_out[CH_2], 	out_min);
-	motor_out[CH_3]		= max(motor_out[CH_3], 	out_min);
 	motor_out[CH_4] 	= max(motor_out[CH_4], 	out_min);
 
 		// Send commands to motors
 	if(g.rc_3.servo_out > 0){
 		APM_RC.OutputCh(CH_1, motor_out[CH_1]);
 		APM_RC.OutputCh(CH_2, motor_out[CH_2]);
-		APM_RC.OutputCh(CH_3, motor_out[CH_3]);
 		APM_RC.OutputCh(CH_4, motor_out[CH_4]);
 		// InstantPWM
 		APM_RC.Force_Out0_Out1();
@@ -46,7 +44,6 @@ void output_motors_armed()
 	}else{
 		APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
 		APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
-		APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
 		APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
 	}
 }
@@ -67,7 +64,6 @@ void output_motors_disarmed()
 	// Send commands to motors
 	APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
 	APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
-	APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
 	APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
 }
 

ArduCopterMega/setup.pde

@@ -11,6 +11,7 @@ static int8_t	setup_flightmodes		(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_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);
@@ -30,6 +31,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"battery",			setup_batt_monitor},
 	{"sonar",			setup_sonar},
 	{"compass",			setup_compass},
+	{"offsets",			setup_mag_offset},
 	{"declination",		setup_declination},
 	{"show",			setup_show}
 };
@@ -398,6 +400,64 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
 	return 0;
 }
 
+static int8_t
+setup_mag_offset(uint8_t argc, const Menu::arg *argv)
+{
+	print_hit_enter();
+	init_compass();
+
+	float _min[3], _max[3];
+	Vector3f _offsets;
+	Vector3f compass_mag;
+
+	while(1){
+		static float min[3], _max[3], offset[3];
+		if (millis() - fast_loopTimer > 100) {
+			delta_ms_fast_loop 	= millis() - fast_loopTimer;
+			fast_loopTimer		= millis();
+			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;
+
+
+			compass.read();
+			compass.calculate(0, 0);	// roll = 0, pitch = 0 for this example
+			compass_mag = compass.get_offsets();
+
+			// capture min
+			_min[0] = min(_min[0], compass_mag.x);
+			_min[1] = min(_min[1], compass_mag.y);
+			_min[2] = min(_min[2], compass_mag.z);
+
+			// capture max
+			_max[0] = max(_max[0], compass_mag.x);
+			_max[1] = max(_max[1], compass_mag.y);
+			_max[2] = max(_max[2], compass_mag.z);
+
+			// calculate offsets
+			_offsets.x = -(_max[0] + _min[0]) / 2;
+			_offsets.y = -(_max[1] + _min[1]) / 2;
+			_offsets.z = -(_max[2] + _min[2]) / 2;
+
+			// display all to user
+			Serial.printf_P(PSTR("Heading: %u, \t (%4.4f, %4.4f, %4.4f) (%4.4f, %4.4f, %4.4f)\n"),
+					(uint16_t)wrap_360(ToDeg(compass.heading)),
+					compass_mag.x,
+					compass_mag.y,
+					compass_mag.z,
+					_offsets.x,
+					_offsets.y,
+					_offsets.z);
+
+			if(Serial.available() > 0){
+				compass.set_offsets(_offsets);
+				//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
+				report_compass();
+				break;
+			}
+		}
+	}
+}
+
+
 /***************************************************************************/
 // CLI defaults
 /***************************************************************************/

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.14 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.15 Beta", main_menu_commands);
 
 void init_ardupilot()
 {