bug fixes and updates

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

ArduCopterMega/ArduCopterMega.pde

@@ -206,6 +206,8 @@ const char* flight_mode_strings[] = {
 			8	TBD
 */
 
+long error_a;
+
 // Radio
 // -----
 int motor_out[4];
@@ -678,7 +680,8 @@ void slow_loop()
 				// between 1 and 5 Hz
 			#else
 				gcs.send_message(MSG_LOCATION);
-// XXX				gcs.send_message(MSG_CPU_LOAD, load*100);
+				// XXX
+				// gcs.send_message(MSG_CPU_LOAD, load*100);
 			#endif
 
             gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz

ArduCopterMega/Attitude.pde

@@ -88,16 +88,23 @@ clear_yaw_control()
 void
 output_yaw_with_hold(boolean hold)
 {
+	//digitalWrite(B_LED_PIN, LOW);
 	if(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.y) < .08){
+			if(fabs(omega.z) < .5){
 				clear_yaw_control();
-				hold 			= true;			// just to be explicit
+				hold = true;			// just to be explicit
+				//Serial.print("C");
+
 			}else{
+
+				//digitalWrite(B_LED_PIN, HIGH);
+
 				// return to rate control until we slow down.
 				hold = false;
+				//Serial.print("D");
 			}
 		}
 
@@ -111,6 +118,7 @@ output_yaw_with_hold(boolean hold)
 	}
 
 	if(hold){
+		//Serial.println("H");
 		// try and hold the current nav_yaw setting
 		yaw_error		= nav_yaw - dcm.yaw_sensor; 									// +- 60°
 		yaw_error 		= wrap_180(yaw_error);
@@ -130,19 +138,26 @@ output_yaw_with_hold(boolean hold)
 
 	}else{
 
+		//Serial.println("R");
 		// rate control
 		long rate		= degrees(omega.z) * 100; 											// 3rad = 17188 , 6rad = 34377
 		rate			= constrain(rate, -36000, 36000);									// limit to something fun!
 
-		if(abs(rate) < 1000 )	//experiment to limit yaw reversing
-			rate = 0;
+		//if(abs(rate) < 1000 )	//experiment to limit yaw reversing
+		//	rate = 0;
 
-		long error		= ((long)g.rc_4.control_in * 6) - rate;								// control is += 6000 * 6 = 36000
+		long error		= ((long)g.rc_4.control_in * 6) - rate;									// control is += 6000 * 6 = 36000
 																							// -error = CCW, 	+error = CW
-		g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); 	// kP .07 * 36000 = 2520
+
+		if(g.rc_4.control_in == 0)
+			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 3.0);// kP .07 * 36000 = 2520
+		else
+			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520
+
 		g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -2400, 2400);						// limit to 24°
 	}
 }
+
 // slight left rudder give right roll.
 
 void

ArduCopterMega/commands_logic.pde

@@ -41,7 +41,7 @@ handle_process_may()
 			do_change_alt();
 			break;
 
-		case MAV_CMD_CONDITION_YAW:
+		case MAV_CMD_CONDITION_ANGLE:
 			do_yaw();
 			break;
 

ArduCopterMega/defines.h

@@ -117,7 +117,7 @@
 #define RELAY_TOGGLE 5
 #define STOP_REPEAT 10
 
-#define MAV_CMD_CONDITION_YAW 23
+//#define MAV_CMD_CONDITION_YAW 23
 
 //  GCS Message ID's
 #define MSG_ACKNOWLEDGE 0x00

ArduCopterMega/motors.pde

@@ -159,6 +159,7 @@ set_servos_4()
 		num++;
 		if (num > 50){
 			num = 0;
+			/*
 			Serial.printf("t_alt:%ld, alt:%ld, thr: %d sen: ",
 						target_altitude,
 						current_loc.alt,
@@ -169,6 +170,10 @@ set_servos_4()
 			}else{
 				Serial.println("Sonar");
 			}
+			*/
+
+
+
 			//Serial.print("!");
 			//debugging with Channel 6
 

ArduCopterMega/radio.pde

@@ -28,6 +28,8 @@ void init_rc_in()
 	g.rc_6.set_range(0,1000);
 	g.rc_7.set_range(0,1000);
 	g.rc_8.set_range(0,1000);
+
+	//catch bad RC_3 min values
 }
 
 void init_rc_out()
@@ -46,15 +48,23 @@ void init_rc_out()
 
 	APM_RC.Init();		// APM Radio initialization
 
+	for(byte i = 0; i < 10; i++){
+		delay(20);
+		read_radio();
+	}
+
+	// sanity check on the EEPROM values for radio_min
+	if(abs(g.rc_3.radio_min - g.rc_3.radio_in) > 40){
+		g.rc_3.radio_min = g.rc_3.radio_in;
+	}
+
 	APM_RC.OutputCh(CH_1, 	g.rc_3.radio_min);					// Initialization of servo outputs
 	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);
 
-
 	APM_RC.OutputCh(CH_7,     g.rc_3.radio_min);
     APM_RC.OutputCh(CH_8,     g.rc_3.radio_min);
-
 }
 
 void read_radio()

ArduCopterMega/setup.pde

@@ -558,11 +558,12 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
 
 			if(Serial.available() > 0){
 
+
 				//mag_offset_x = offset[0];
 				//mag_offset_y = offset[1];
 				//mag_offset_z = offset[2];
 
-				//save_EEPROM_mag_offset();
+				//setup_mag_offset();
 
 				// set offsets to account for surrounding interference
 				//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);

ArduCopterMega/test.pde

@@ -11,7 +11,7 @@ static int8_t	test_gps(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_adc(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_imu(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_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);
@@ -52,7 +52,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
 #endif
 	{"imu",			test_imu},
 	//{"dcm",			test_dcm},
-	//{"omega",		test_omega},
+	{"omega",		test_omega},
 	{"battery",		test_battery},
 	{"current",		test_current},
 	{"relay",		test_relay},
@@ -238,6 +238,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
 	control_mode = STABILIZE;
 	Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP());
 	Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
+	Serial.printf_P(PSTR("max_yaw_dampener:%d\n\n "), max_yaw_dampener);
 
 	trim_radio();
 
@@ -285,14 +286,21 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
 			set_servos_4();
 
 			ts_num++;
-			if (ts_num > 10){
+			if (ts_num > 0){
 				ts_num = 0;
+				/*
 				Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
 					(int)(dcm.roll_sensor/100),
 					(int)(dcm.pitch_sensor/100),
 					g.rc_1.pwm_out);
+				*/
 
-				print_motor_out();
+				Serial.printf_P(PSTR("e:%ld, 4out:%d,  O%4.4f\n"),
+					error_a,
+					g.rc_4.servo_out,
+					omega.z);
+
+				//print_motor_out();
 			}
 			// R: 1417,  L: 1453  F: 1453  B: 1417
 
@@ -611,7 +619,8 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
 }
 */
 
-/*static int8_t
+///*
+static int8_t
 test_omega(uint8_t argc, const Menu::arg *argv)
 {
 	static byte ts_num;
@@ -629,6 +638,7 @@ test_omega(uint8_t argc, const Menu::arg *argv)
 		// IMU
 		// ---
 		read_AHRS();
+
 		float my_oz = (dcm.yaw - old_yaw) * 50;
 
 		old_yaw = dcm.yaw;
@@ -646,7 +656,7 @@ test_omega(uint8_t argc, const Menu::arg *argv)
 	}
 	return (0);
 }
-*/
+//*/
 
 static int8_t
 test_battery(uint8_t argc, const Menu::arg *argv)