WebSVN - FlightCtrl - Blame - Rev 2132 - /branches/dongfang_FC_fixedwing/arduino

Rev	Author	Line No.	Line
2108	-	1	#include <stdlib.h>
		2	#include <avr/io.h>
		3	#include "eeprom.h"
		4	#include "flight.h"
		5	#include "output.h"
		6
		7	// Necessary for external control and motor test
		8	#include "uart0.h"
		9	#include "timer2.h"
		10	#include "analog.h"
		11	#include "attitude.h"
		12	#include "controlMixer.h"
		13	#include "configuration.h"
		14
		15	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
		16
		17	/*
		18	* target-directions integrals.
		19	*/
		20	int32_t target[3];
		21
		22	/*
		23	* Error integrals.
		24	*/
		25	int32_t error[3];
		26
		27	uint8_t reverse[3];
		28	int32_t maxError[3];
		29	int32_t IPart[3] = { 0, 0, 0 };
		30	PID_t airspeedPID[3];
		31
		32	int16_t controlServos[NUM_CONTROL_SERVOS];
		33
		34	/************************************************************************/
		35	/* Neutral Readings */
		36	/************************************************************************/
		37	#define CONTROL_CONFIG_SCALE 10
		38
		39	void flight_setGround(void) {
		40	IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
		41	target[PITCH] = attitude[PITCH];
		42	target[ROLL] = attitude[ROLL];
		43	target[YAW] = attitude[YAW];
		44	}
		45
		46	void flight_updateFlightParametersToFlightMode(void) {
		47	debugOut.analog[16] = currentFlightMode;
2132	-	48	reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
		49	reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;
		50	reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;
2108	-	51
		52	// At a switch to angles, we want to kill errors first.
		53	// This should be triggered only once per mode change!
		54	if (currentFlightMode == FLIGHT_MODE_ANGLES) {
		55	target[PITCH] = attitude[PITCH];
		56	target[ROLL] = attitude[ROLL];
		57	target[YAW] = attitude[YAW];
		58	}
		59
		60	for (uint8_t axis=0; axis<3; axis++) {
		61	maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;
		62	}
		63	}
		64
		65	// Normal at airspeed = 10.
		66	uint8_t calcAirspeedPID(uint8_t pid) {
2132	-	67	//if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) {
2108	-	68	return pid;
2132	-	69	//}
		70	}
2108	-	71
		72	void setAirspeedPIDs(void) {
		73	for (uint8_t axis = 0; axis<3; axis++) {
		74	airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P);
		75	airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???
		76	airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;
		77	}
		78	}
		79
2132	-	80	#define LOG_STICK_SCALE 8
		81	#define LOG_P_SCALE 6
		82	#define LOG_I_SCALE 10
		83	#define LOG_D_SCALE 6
		84
2108	-	85	/************************************************************************/
		86	/* Main Flight Control */
		87	/************************************************************************/
		88	void flight_control(void) {
		89	// Mixer Fractions that are combined for Motor Control
		90	int16_t term[4];
		91
		92	// PID controller variables
		93	int16_t PDPart[3];
		94
		95	static int8_t debugDataTimer = 1;
		96
		97	// High resolution motor values for smoothing of PID motor outputs
		98	// static int16_t outputFilters[MAX_OUTPUTS];
		99
		100	uint8_t axis;
		101
		102	setAirspeedPIDs();
		103
		104	term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
		105
		106	// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
2132	-	107	int32_t tmp;
2108	-	108
2132	-	109	tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
		110	if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp;
		111
		112	tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
		113	if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp;
		114
		115	tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
		116	if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp;
		117
2108	-	118	for (axis = PITCH; axis <= YAW; axis++) {
		119	if (target[axis] > OVER180) {
		120	target[axis] -= OVER360;
		121	} else if (target[axis] <= -OVER180) {
		122	target[axis] += OVER360;
		123	}
		124
2132	-	125	error[axis] = attitude[axis] - target[axis];
2108	-	126
2132	-	127	#define ROTATETARGET 1
		128	// #define TRUNCATEERROR 1
		129
		130	#ifdef ROTATETARGET
		131	if(abs(error[axis]) > OVER180) {
		132	// The shortest way from attitude to target crosses -180.
		133	// Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
		134	// Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
		135	if (error[axis] > 0) {
		136	if (error[axis] < OVER360 - maxError[axis]) {
		137	// too much err.
		138	error[axis] = -maxError[axis];
		139	target[axis] = attitude[axis] + maxError[axis];
		140	if (target[axis] > OVER180) target[axis] -= OVER360;
		141	} else {
		142	// Normal case, we just need to correct for the wrap. Error will be negative.
		143	error[axis] -= OVER360;
		144	}
		145	} else {
		146	if (error[axis] > maxError[axis] - OVER360) {
		147	// too much err.
		148	error[axis] = maxError[axis];
		149	target[axis] = attitude[axis] - maxError[axis];
		150	if (target[axis] < -OVER180) target[axis] += OVER360;
		151	} else {
		152	// Normal case, we just need to correct for the wrap. Error will be negative.
		153	error[axis] += OVER360;
		154	}
		155	}
		156	} else {
		157	// Simple case, linear range.
		158	if (error[axis] > maxError[axis]) {
		159	error[axis] = maxError[axis];
		160	target[axis] = attitude[axis] - maxError[axis];
		161	} else if (error[axis] < -maxError[axis]) {
		162	error[axis] = -maxError[axis];
		163	target[axis] = attitude[axis] + maxError[axis];
		164	}
		165	}
		166	#endif
		167	#ifdef TUNCATEERROR
2108	-	168	if (error[axis] > maxError[axis]) {
2132	-	169	error[axis] = maxError[axis];
2108	-	170	} else if (error[axis] < -maxError[axis]) {
2132	-	171	error[axis] = -maxError[axis];
		172	} else {
		173	// update I parts here for angles mode. I parts in rate mode is something different.
2108	-	174	}
2132	-	175	#endif
2108	-	176
		177	/************************************************************************/
		178	/* Calculate control feedback from angle (gyro integral) */
		179	/* and angular velocity (gyro signal) */
		180	/************************************************************************/
		181	if (currentFlightMode == FLIGHT_MODE_ANGLES \|\| currentFlightMode
		182	== FLIGHT_MODE_RATE) {
		183	PDPart[axis] = (((int32_t) gyro_PID[axis]
		184	* (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE)
		185	+ ((gyroD[axis] * (int16_t) airspeedPID[axis].D)
		186	>> LOG_D_SCALE);
		187	} else {
		188	PDPart[axis] = 0;
		189	}
		190
		191	if (currentFlightMode == FLIGHT_MODE_ANGLES) {
2132	-	192	int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
		193	PDPart[axis] += anglePart;
2108	-	194	}
2132	-	195
2108	-	196	// Add I parts here... these are integrated errors.
2132	-	197	if (reverse[axis])
		198	term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
		199	else
		200	term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
2108	-	201	}
		202
		203	debugOut.analog[12] = term[PITCH];
		204	debugOut.analog[13] = term[ROLL];
		205	debugOut.analog[14] = term[YAW];
		206	debugOut.analog[15] = term[THROTTLE];
		207
		208	for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
		209	int16_t tmp;
		210	if (servoTestActive) {
2132	-	211	controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;
2108	-	212	} else {
		213	// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
		214	switch (i) {
		215	case 0:
		216	tmp = term[ROLL];
		217	break;
		218	case 1:
		219	tmp = term[PITCH];
		220	break;
		221	case 2:
		222	tmp = term[THROTTLE];
		223	break;
		224	case 3:
		225	tmp = term[YAW];
		226	break;
		227	default:
		228	tmp = 0;
		229	}
		230	// These are all signed and in the same units as the RC stuff in rc.c.
		231	controlServos[i] = tmp;
		232	}
		233	}
		234
		235	calculateControlServoValues();
		236
		237	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		238	// Debugging
		239	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		240	if (!(--debugDataTimer)) {
		241	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
		242	debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg
		243	debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg
		244	debugOut.analog[2] = gyro_PID[YAW];
		245
		246	debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		247	debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		248	debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);
		249
		250	debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		251	debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		252	debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
		253
		254	debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		255	debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		256	debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10);
		257
		258	debugOut.analog[12] = term[PITCH];
		259	debugOut.analog[13] = term[ROLL];
		260	debugOut.analog[14] = term[YAW];
		261
		262	//DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
		263	//DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
		264	//debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg
		265	//debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg
		266	}
		267	}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/flight.c - Rev 2132