Added Tri-copter support for Max

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

ArduCopterMega/Attitude.pde

@@ -5,7 +5,7 @@ void init_pids()
 	// this creates symmetry with the P gain value preventing oscillations
 	
 	max_stabilize_dampener 	= pid_stabilize_roll.kP() * 2500;		// = 0.6 * 2500 = 1500 or 15°
-	max_yaw_dampener		= pid_yaw.kP() * 6000;					// .5 * 6000 = 3000
+	max_yaw_dampener		= pid_yaw.kP() * 6000;			// .5 * 6000 = 3000
 }
 
 
@@ -16,8 +16,6 @@ void output_stabilize()
 	long rate;
 	int dampener;
 	
-	//pitch_sensor = roll_sensor = 0; // testing only
-	
 	// control +- 45° is mixed with the navigation request by the Autopilot
 	// output is in degrees = target pitch and roll of copter
 	rc_1.servo_out = rc_1.control_mix(nav_roll);
@@ -30,14 +28,10 @@ void output_stabilize()
 	roll_error 		= constrain(roll_error,  -2500, 2500);
 	pitch_error 	= constrain(pitch_error, -2500, 2500);
 
-	//Serial.printf("s: %d \t mix  %d, err  %d", (int)roll_sensor, (int)rc_1.servo_out, (int)roll_error);
-
 	// write out angles back to servo out - this will be converted to PWM by RC_Channel
 	rc_1.servo_out 	= pid_stabilize_roll.get_pid(roll_error,  	deltaMiliSeconds, 1.0);
 	rc_2.servo_out 	= pid_stabilize_pitch.get_pid(pitch_error, 	deltaMiliSeconds, 1.0);
 
-	//Serial.printf("\tpid: %d", (int)rc_1.servo_out);
-
 	// We adjust the output by the rate of rotation:
 	// Rate control through bias corrected gyro rates
 	// omega is the raw gyro reading
@@ -45,16 +39,11 @@ void output_stabilize()
 	// Limit dampening to be equal to propotional term for symmetry
 	rate			= degrees(omega.x) * 100; 													// 6rad = 34377
 	dampener 		= ((float)rate * stabilize_dampener);										// 34377 * .175 = 6000
-	rc_1.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500
+	rc_1.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
 
 	rate			= degrees(omega.y) * 100; 													// 6rad = 34377
 	dampener 		= ((float)rate * stabilize_dampener);										// 34377 * .175 = 6000
-	rc_2.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500
+	rc_2.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
-
-	//Serial.printf(" yaw out: %d, d: %d", (int)rc_4.angle_to_pwm(), yaw_dampener);
-
-	//Serial.printf("\trd: %d", roll_dampener);
-	//Serial.printf("\tlimit: %d, PWM: %d", rc_1.servo_out, rc_1.angle_to_pwm());	
 }
 
 void
@@ -66,30 +55,32 @@ clear_yaw_control()
 	yaw_error 		= 0;
 }
 
+
 void output_yaw_with_hold(boolean hold)
 {
 	Vector3f omega 	= dcm.get_gyro();
 
 	if(hold){
 		// yaw hold
+		
 		if(rate_yaw_flag){
 			// we are still in motion from rate control
 			if(fabs(omega.y) < .15){
-				//Serial.print("trans ");
 				clear_yaw_control();
 				hold 			= true;			// just to be explicit
 			}else{
-				//Serial.print("coast ");
 				// return to rate control until we slow down.
 				hold = false;
 			}
 		}else{
-			//Serial.print("hold ");		
+
 		}
 		
-	} else {
+	}else{
-		//Serial.print("rate ");
 		// rate control
+		
+		// this indicates we are under rate control, when we enter Yaw Hold and 
+		// return to 0° per second, we exit rate control and hold the current Yaw		
 		rate_yaw_flag 	= true;
 		yaw_error 		= 0;
 	}
@@ -100,6 +91,8 @@ void output_yaw_with_hold(boolean hold)
 
 		// limit the error we're feeding to the PID
 		yaw_error		= constrain(yaw_error,   -6000, 6000);						// limit error to 60 degees
+
+		// Apply PID and save the new angle back to RC_Channel
 		rc_4.servo_out 	= pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); 		// .5 * 6000 = 3000
 	
 		// We adjust the output by the rate of rotation
@@ -109,20 +102,16 @@ void output_yaw_with_hold(boolean hold)
 		// Limit dampening to be equal to propotional term for symmetry
 		rc_4.servo_out	-= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); 	// -3000
 	
-	}else{
+	}else{		
-		//yaw_error		= 0;
-		
 		// rate control
+		
 		long rate		= degrees(omega.z) * 100; 									// 3rad = 17188 , 6rad = 34377
 		rate			= constrain(rate, -36000, 36000);							// limit to something fun!
-		long error		= ((long)rc_4.control_in * 6) - rate;						// control is += 6000
+		long error		= ((long)rc_4.control_in * 6) - rate;						// control is += 6000 * 6 = 36000
 																					// -error = CCW, 	+error = CW
 		rc_4.servo_out 	= pid_acro_rate_yaw.get_pid(error, deltaMiliSeconds, 1.0); 	// .075 * 36000 = 2700
 		rc_4.servo_out 	= constrain(rc_4.servo_out, -2400, 2400);					// limit to 2400
 
-		// this indicates we are under rate control, when we enter Yaw Hold and 
-		// return to 0° per second, we exit rate control and hold the current Yaw
-		//rate_yaw_flag 	= true;
 	}
 }
 

ArduCopterMega/EEPROM.pde

@@ -138,6 +138,7 @@ void read_EEPROM_PID(void)
 
 	// yaw
 	hold_yaw_dampener			= read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4);
+ 	init_pids();
 }
 
 void save_EEPROM_PID(void)

ArduCopterMega/defines.h

@@ -46,10 +46,10 @@
 #define CH_AUX2 CH_6
 
 // motors
-#define FRONT 0
+#define RIGHT 0
-#define RIGHT 1
+#define LEFT 1
-#define BACK 2
+#define FRONT 2
-#define LEFT 3
+#define BACK 3
 
 #define MAX_SERVO_OUTPUT 2700
 

ArduCopterMega/flight_control.pde

@@ -37,9 +37,6 @@ float angle_boost()
 }
 
 
-
-
-
 /*************************************************************
 yaw control
 ****************************************************************/

ArduCopterMega/navigation.pde

@@ -50,6 +50,7 @@ void navigate()
 	}
 }
 
+/*
 void verify_missed_wp()
 {
 	// check if we have missed the WP
@@ -63,6 +64,7 @@ void verify_missed_wp()
 	if (loiter_delta < -170) loiter_delta += 360;
 	loiter_sum 		+= abs(loiter_delta);
 }
+*/
 
 void calc_bearing_error()
 {

ArduCopterMega/radio.pde

@@ -134,13 +134,14 @@ void set_servos_4(void)
 		//Serial.printf("yaw: %d ", rc_4.radio_out);
 		
 		if(frame_type == PLUS_FRAME){
-			//Serial.println("+");
+
 			motor_out[RIGHT]	= rc_3.radio_out - rc_1.pwm_out;
 			motor_out[LEFT]		= rc_3.radio_out + rc_1.pwm_out;
 			motor_out[FRONT]	= rc_3.radio_out + rc_2.pwm_out;
 			motor_out[BACK] 	= rc_3.radio_out - rc_2.pwm_out;
 			
 		}else if(frame_type == X_FRAME){
+		
 			int roll_out 	= rc_1.pwm_out / 2;
 			int pitch_out 	= rc_2.pwm_out / 2;
 
@@ -149,28 +150,39 @@ void set_servos_4(void)
 
 			motor_out[RIGHT]	= rc_3.radio_out - roll_out + pitch_out;
 			motor_out[BACK] 	= rc_3.radio_out - roll_out - pitch_out;
-		}else{
-			/*
-			replace this with Tri-frame control law
 			
-			int roll_out 	= rc_1.pwm_out / 2;
+		}else if(frame_type == TRI_FRAME){
-			int pitch_out 	= rc_2.pwm_out / 2;
-
-			motor_out[FRONT]	= rc_3.radio_out + roll_out + pitch_out;
-			motor_out[LEFT]		= rc_3.radio_out + roll_out - pitch_out;
 
+			// Tri-copter power distribution
+			
+			int roll_out 	= (float)rc_1.pwm_out * .866;
+			int pitch_out 	= rc_2.pwm_out / 2;
+			
+			// front two motors
+			motor_out[LEFT]		= rc_3.radio_out + roll_out + pitch_out;
 			motor_out[RIGHT]	= rc_3.radio_out - roll_out + pitch_out;
-			motor_out[BACK] 	= rc_3.radio_out - roll_out - pitch_out;
+			
-			*/
+			// rear motors
+			motor_out[BACK] 	= rc_3.radio_out - rc_2.pwm_out;
+			
+			// servo Yaw
+			motor_out[FRONT] 	= rc_4.radio_out;
+			
+		}else{
+		
+			Serial.print("frame error");
+			
 		}
 		
 		//Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
 
 		if((frame_type == PLUS_FRAME) || (frame_type == X_FRAME)){
+		
 			motor_out[RIGHT]	+=  rc_4.pwm_out;
 			motor_out[LEFT]		+=  rc_4.pwm_out;
 			motor_out[FRONT]	-=  rc_4.pwm_out;
 			motor_out[BACK] 	-=  rc_4.pwm_out;
+			
 		}
 		
 		//Serial.printf("\tl8r: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
@@ -229,15 +241,19 @@ void set_servos_4(void)
 		
 		// Send commands to motors
 		if(rc_3.servo_out > 0){
+		
 			APM_RC.OutputCh(CH_1, motor_out[RIGHT]);
 			APM_RC.OutputCh(CH_2, motor_out[LEFT]);
 			APM_RC.OutputCh(CH_3, motor_out[FRONT]);
 			APM_RC.OutputCh(CH_4, motor_out[BACK]);
+			
 		}else{
+		
 			APM_RC.OutputCh(CH_1, rc_3.radio_min);
 			APM_RC.OutputCh(CH_2, rc_3.radio_min);
 			APM_RC.OutputCh(CH_3, rc_3.radio_min);
 			APM_RC.OutputCh(CH_4, rc_3.radio_min);
+			
 		}
 	
 		// InstantPWM

ArduCopterMega/setup.pde

@@ -270,6 +270,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
 			// stop here
 		}
 	}
+	
 	rc_1.radio_min = rc_1.radio_in;
 	rc_2.radio_min = rc_2.radio_in;
 	rc_3.radio_min = rc_3.radio_in;
@@ -346,9 +347,14 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 	
 	if(frame_type == PLUS_FRAME){
 		Serial.printf_P(PSTR("PLUS"));
-	}else{
+
+	}else if(frame_type == X_FRAME){
 		Serial.printf_P(PSTR("X"));
+	
+	}else if(frame_type == TRI_FRAME){
+		Serial.printf_P(PSTR("TRI"));
 	}
+	
 	Serial.printf_P(PSTR(" Frame\n"));
 	
 	while(1){
@@ -380,7 +386,8 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 				Serial.println("2");
 			}
 
-		}else{
+		}else if(frame_type == PLUS_FRAME){
+
 			// lower right
 			if((rc_1.control_in > 0) 		&& (rc_2.control_in > 0)){
 				motor_out[BACK] 	= out_min;
@@ -400,6 +407,29 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 				motor_out[RIGHT]	= out_min;
 				Serial.println("0");
 			}
+			
+		}else if(frame_type == TRI_FRAME){
+
+			if(rc_1.control_in > 0){
+				motor_out[RIGHT] 	= out_min;
+				
+			}else if(rc_1.control_in < 0){
+				motor_out[LEFT]		= out_min;
+			}
+			
+			if(rc_2.control_in > 0){
+				motor_out[BACK] 	= out_min;	
+			}
+
+			if(rc_4.control_in > 0){
+				rc_4.servo_out	= 2000;
+				
+			}else if(rc_4.control_in < 0){
+				rc_4.servo_out	= -2000;
+			}
+			
+			rc_4.calc_pwm();
+			motor_out[FRONT] 	= rc_4.radio_out;
 		}
 		
 		if(rc_3.control_in > 0){
@@ -419,6 +449,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 		}
 	}
 }
+
 static int8_t
 setup_accel(uint8_t argc, const Menu::arg *argv)
 {
@@ -620,7 +651,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
 	}else if(argv[1].i == 3){
 
 		Serial.printf_P(PSTR("Tri "));
-		frame_type = X_FRAME;
+		frame_type = TRI_FRAME;
 	}
 	Serial.printf_P(PSTR("frame\n\n"));
 	save_EEPROM_frame();	

ArduCopterMega/system.pde

@@ -94,7 +94,6 @@ void init_ardupilot()
 
 
 	read_EEPROM_startup(); // Read critical config information to start
- 	init_pids();
 
 	init_rc_in();		// sets up rc channels from radio
 	init_rc_out();		// sets up the timer libs