alt hold updates

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

ArduCopterMega/APM_Config.h

@@ -8,6 +8,9 @@
 //#define MAGORIENTATION	AP_COMPASS_COMPONENTS_UP_PINS_BACK
 #define MAGORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
+# define SERIAL0_BAUD			38400
+//# define SERIAL0_BAUD			115200
+
 //# define STABILIZE_ROLL_P 		0.4
 //# define STABILIZE_PITCH_P		0.4
 //# define STABILIZE_DAMPENER		0.1

ArduCopterMega/ArduCopterMega.pde

@@ -198,6 +198,12 @@ byte	command_may_ID;						// current command ID
 float	altitude_gain;						// in nav
 float	distance_gain;						// in nav
 
+float cos_roll_x;
+float sin_roll_y;
+
+float cos_pitch_x;
+float sin_pitch_y;
+
 float sin_yaw_y;
 float cos_yaw_x;
 
@@ -507,6 +513,10 @@ void medium_loop()
 	// ----------
 	read_radio();			// read the radio first
 
+	
+	// reads all of the necessary trig functions for cameras, throttle, etc.
+	update_trig();
+	
 	// This is the start of the medium (10 Hz) loop pieces
 	// -----------------------------------------
 	switch(medium_loopCounter) {
@@ -1013,3 +1023,21 @@ void read_AHRS(void)
 	//dcm.pitch_sensor = 0;
 	//dcm.roll_sensor = 0;
 }
+
+void update_trig(void){
+	Vector2f yawvector;
+	Matrix3f temp 	= dcm.get_dcm_matrix();
+	
+	yawvector.x 	= temp.a.x; // sin
+	yawvector.y 	= temp.b.x;	// cos
+	yawvector.normalize();
+
+	cos_yaw_x 		= yawvector.y;	// 0 x = north
+	sin_yaw_y 		= yawvector.x;	// 1 y 
+	
+	sin_pitch_y 	= -temp.c.x;
+	cos_pitch_x 	= sqrt(1 - (temp.c.x * temp.c.x));
+	
+	cos_roll_x 		= temp.c.z / cos_pitch_x;
+	sin_roll_y 		= temp.c.y / cos_pitch_x;
+}

ArduCopterMega/config.h

@@ -380,10 +380,10 @@
 // Throttle control gains
 //
 #ifndef THROTTLE_BARO_P
-# define THROTTLE_BARO_P		0.12
+# define THROTTLE_BARO_P		0.25
 #endif
 #ifndef THROTTLE_BARO_I
-# define THROTTLE_BARO_I		0.05
+# define THROTTLE_BARO_I		0.01
 #endif
 #ifndef THROTTLE_BARO_D
 # define THROTTLE_BARO_D		0.2

ArduCopterMega/flight_control.pde

@@ -22,14 +22,14 @@ void output_auto_throttle()
 void calc_nav_throttle()
 {
 	// limit error
-	long error = constrain(altitude_error, -300, 300);
+	long error = constrain(altitude_error, -400, 400);
 	
 	if(altitude_sensor == BARO) {
 		float t = pid_baro_throttle.kP();
 		
-		if(error > 0){
+		if(error > 0){ 					// go up
 			pid_baro_throttle.kP(t);
-		}else{
+		}else{							// go down
 			pid_baro_throttle.kP(t/4.0);
 		}
 		
@@ -53,15 +53,8 @@ void calc_nav_throttle()
 
 float angle_boost()
 {
-	// This is the furture replacement for the heavyweight trig functions in use now.
+	float temp = cos_pitch_x * cos_roll_x;
-	//Matrix3f temp = dcm.get_dcm_matrix();
+	temp = 2.0 - constrain(temp, .7, 1.0);
-	//cos_pitch 	= sqrt(1 - (temp.c.x * temp.c.x));
-	//cos_roll 		= dcm.c.z / cos_pitch;
-
-	//static byte flipper;
-	//float temp = 1 / (cos(dcm.roll) * cos(dcm.pitch));
-	float temp = cos(dcm.roll) * cos(dcm.pitch);
-	temp = 2.0 - constrain(temp, .7071, 1.0);
 	return temp;
 }
 

ArduCopterMega/motors.pde

@@ -205,6 +205,14 @@ set_servos_4()
 			
 			flush(10);
 			//*/
+			
+			/*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n",
+					current_loc.alt,
+					altitude_error,
+					(int)pid_baro_throttle.get_integrator(),
+					nav_throttle,
+					angle_boost());
+			*/
 		}
 		
 		// Send commands to motors

ArduCopterMega/navigation.pde

@@ -56,13 +56,13 @@ void calc_nav()
 	pitch_max = 22° (2200)
 	*/	
 
-	temp 			= dcm.get_dcm_matrix();
+	//temp 			= dcm.get_dcm_matrix();
-	yawvector.y 	= temp.b.x;	// cos
+	//yawvector.y 	= temp.b.x;	// cos
-	yawvector.x 	= temp.a.x; // sin
+	//yawvector.x 	= temp.a.x; // sin
-	yawvector.normalize();
+	//yawvector.normalize();
 	
-	cos_yaw_x = yawvector.y;	// 0
+	//cos_yaw_x = yawvector.y;	// 0
-	sin_yaw_y = yawvector.x;	// 1
+	//sin_yaw_y = yawvector.x;	// 1
 
 	long_error	= (float)(next_WP.lng - current_loc.lng) * scaleLongDown;   // 50 - 30 = 20 pitch right
 	lat_error	= next_WP.lat - current_loc.lat;							// 50 - 30 = 20 pitch up

ArduCopterMega/test.pde

@@ -404,8 +404,8 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 	print_hit_enter();
 	delay(1000);
 	
-	float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
+	//float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
-	Vector2f yawvector;
+	
 	
 	while(1){
 		delay(20);
@@ -413,8 +413,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 			delta_ms_fast_loop 	= millis() - fast_loopTimer;
 			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
 			fast_loopTimer		= millis();
-
+			/*
-			
 			Matrix3f temp 	= dcm.get_dcm_matrix();
 			
 			sin_pitch 		= -temp.c.x;
@@ -428,7 +427,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 			yawvector.normalize();
 			cos_yaw 		= yawvector.y;	// 0 x = north
 			sin_yaw 		= yawvector.x;	// 1 y 
-
+			*/
 
 			// IMU
 			// ---
@@ -437,6 +436,8 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 			Vector3f accels 	= imu.get_accel();
 			Vector3f gyros 		= imu.get_gyro();
 			
+			update_trig();
+
 			if(compass_enabled){
 				medium_loopCounter++;
 				if(medium_loopCounter == 5){
@@ -459,12 +460,12 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 								dcm.yaw_sensor);
 
 			Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
-								cos_pitch,
+								cos_pitch_x,
-								sin_pitch,
+								sin_pitch_y,
-								cos_roll,
+								cos_roll_x,
-								sin_roll,
+								sin_roll_y,
-								cos_yaw,	// x
+								cos_yaw_x,	// x
-								sin_yaw);	// y
+								sin_yaw_y);	// y
 		}
 		
 		if(Serial.available() > 0){
@@ -479,8 +480,9 @@ test_gps(uint8_t argc, const Menu::arg *argv)
 	print_hit_enter();
 	delay(1000);
 	
-	while(1){
+	/*while(1){
 		delay(100);
+		
 		update_GPS();
 		
 		if(Serial.available() > 0){
@@ -490,26 +492,23 @@ test_gps(uint8_t argc, const Menu::arg *argv)
 		if(home.lng != 0){
 			break;
 		}
-	}
+	}*/
 	
 	while(1){
 		delay(100);
+		update_GPS();
+		
 		calc_distance_error();
-		// Blink GPS LED if we don't have a fix
-		// ------------------------------------
-		//update_GPS_light();
 		
-		GPS.update();
+		//if (GPS.new_data){
-		
-		if (GPS.new_data){
 			Serial.print("Lat:");
 			Serial.print((float)GPS.latitude/10000000, 10);
 			Serial.print(" Lon:");
 			Serial.print((float)GPS.longitude/10000000, 10);
 			Serial.printf_P(PSTR(" alt %dm, spd: %d dist:%d, #sats: %d\n"), (int)GPS.altitude/100, (int)GPS.ground_speed, (int)wp_distance, (int)GPS.num_sats);
-		}else{
+		//}else{
 			//Serial.print(".");
-		}
+		//}
 		if(Serial.available() > 0){
 			return (0);
 		}