WebSVN - FlightCtrl - Blame - Rev 2103 - /branches/dongfang_FC

Rev	Author	Line No.	Line
1910	-	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
		10	#include "timer2.h"
2103	-	11	#include "analog.h"
1910	-	12	#include "attitude.h"
		13	#include "controlMixer.h"
		14
		15	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
		16
		17	/*
2099	-	18	* target-directions integrals.
1910	-	19	*/
2099	-	20	int32_t target[3];
1910	-	21
2099	-	22	/*
		23	* Error integrals.
		24	*/
		25	int32_t error[3];
1910	-	26
2099	-	27	uint8_t pFactor[3];
		28	uint8_t dFactor[3];
		29	uint8_t iFactor[3];
		30	uint8_t reverse[3];
		31	int32_t IPart[3] = { 0, 0, 0 };
1910	-	32
2102	-	33	int16_t controlServos[MAX_CONTROL_SERVOS];
1910	-	34
		35	/************************************************************************/
		36	/* Neutral Readings */
		37	/************************************************************************/
		38	#define CONTROL_CONFIG_SCALE 10
		39
2099	-	40	void flight_setGround(void) {
2102	-	41	IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
		42	target[PITCH] = attitude[PITCH];
		43	target[ROLL] = attitude[ROLL];
		44	target[YAW] = attitude[YAW];
1910	-	45	}
		46
2102	-	47	// this should be followed by a call to switchToFlightMode!!
2103	-	48	void flight_updateFlightParametersToFlightMode(uint8_t flightMode) {
		49	debugOut.analog[16] = flightMode;
1910	-	50
2103	-	51	reverse[PITCH] = staticParams.controlServosReverse
2102	-	52	& CONTROL_SERVO_REVERSE_ELEVATOR;
2103	-	53	reverse[ROLL] = staticParams.controlServosReverse
2102	-	54	& CONTROL_SERVO_REVERSE_AILERONS;
2103	-	55	reverse[YAW] = staticParams.controlServosReverse
2102	-	56	& CONTROL_SERVO_REVERSE_RUDDER;
1910	-	57
2102	-	58	for (uint8_t i = 0; i < 3; i++) {
		59	if (flightMode == FLIGHT_MODE_MANUAL) {
		60	pFactor[i] = 0;
		61	dFactor[i] = 0;
		62	} else if(flightMode == FLIGHT_MODE_RATE \|\| flightMode == FLIGHT_MODE_ANGLES) {
		63	pFactor[i] = staticParams.gyroPID[i].P;
		64	dFactor[i] = staticParams.gyroPID[i].D;
		65	}
		66
		67	if (flightMode == FLIGHT_MODE_ANGLES) {
		68	iFactor[i] = staticParams.gyroPID[i].I;
		69	} else if(flightMode == FLIGHT_MODE_RATE \|\| flightMode == FLIGHT_MODE_MANUAL) {
		70	iFactor[i] = 0;
		71	}
		72	}
1910	-	73	}
		74
		75	/************************************************************************/
		76	/* Main Flight Control */
		77	/************************************************************************/
		78	void flight_control(void) {
2102	-	79	// Mixer Fractions that are combined for Motor Control
2103	-	80	int16_t term[4];
2099	-	81
2102	-	82	// PID controller variables
		83	int16_t PDPart[3];
1910	-	84
2102	-	85	static int8_t debugDataTimer = 1;
1910	-	86
2102	-	87	// High resolution motor values for smoothing of PID motor outputs
		88	// static int16_t outputFilters[MAX_OUTPUTS];
1910	-	89
2102	-	90	uint8_t axis;
1910	-	91
2102	-	92	// TODO: Check modern version.
		93	// calculateFlightAttitude();
		94	// TODO: Check modern version.
		95	// controlMixer_update();
2103	-	96	term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
1910	-	97
2102	-	98	// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
2103	-	99	target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 6;
		100	target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 6;
		101	target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 6;
1910	-	102
2102	-	103	for (axis = PITCH; axis <= YAW; axis++) {
		104	if (target[axis] > OVER180) {
		105	target[axis] -= OVER360;
2103	-	106	} else if (target[axis] <= -OVER180) {
		107	target[axis] += OVER360;
2102	-	108	}
1910	-	109
2102	-	110	if (reverse[axis])
		111	error[axis] = attitude[axis] + target[axis];
		112	else
		113	error[axis] = attitude[axis] - target[axis];
		114	if (error[axis] > OVER180) {
		115	error[axis] -= OVER360;
		116	} else if (error[axis] <= -OVER180) {
		117	error[axis] += OVER360;
		118	}
1910	-	119
2102	-	120	/************************************************************************/
		121	/* Calculate control feedback from angle (gyro integral) */
		122	/* and angular velocity (gyro signal) */
		123	/************************************************************************/
2103	-	124	PDPart[axis] = (((int32_t) gyro_PID[axis] * pFactor[axis]) >> 6)
		125	+ ((gyroD[axis] * (int16_t) dFactor[axis]) >> 4);
2102	-	126	if (reverse[axis])
		127	PDPart[axis] = -PDPart[axis];
1910	-	128
2102	-	129	int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
		130	if (reverse[axis])
		131	PDPart[axis] += anglePart;
		132	else
		133	PDPart[axis] -= anglePart;
1910	-	134
2102	-	135	// Add I parts here... these are integrated errors.
		136	// When an error wraps, actually its I part should be negated or something...
1910	-	137
2102	-	138	term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
		139	}
1910	-	140
2102	-	141	debugOut.analog[12] = term[PITCH];
		142	debugOut.analog[13] = term[ROLL];
2103	-	143	debugOut.analog[14] = term[YAW];
		144	debugOut.analog[15] = term[THROTTLE];
2099	-	145
2102	-	146	for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {
		147	int16_t tmp;
		148	if (servoTestActive) {
		149	controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
		150	} else {
		151	// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
		152	switch (i) {
		153	case 0:
		154	tmp = term[ROLL];
		155	break;
		156	case 1:
		157	tmp = term[PITCH];
		158	break;
		159	case 2:
2103	-	160	tmp = term[THROTTLE];
2102	-	161	break;
		162	case 3:
		163	tmp = term[YAW];
		164	break;
		165	default:
		166	tmp = 0;
		167	}
		168	// These are all signed and in the same units as the RC stuff in rc.c.
		169	controlServos[i] = tmp;
		170	}
		171	}
1910	-	172
2103	-	173	calculateControlServoValues();
1910	-	174
2102	-	175	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		176	// Debugging
		177	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		178	if (!(--debugDataTimer)) {
		179	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
2103	-	180	debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg
		181	debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg
		182	debugOut.analog[2] = gyro_PID[YAW];
1910	-	183
2102	-	184	debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		185	debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		186	debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);
2099	-	187
2102	-	188	debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		189	debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		190	debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
		191
		192	debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		193	debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
		194	debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10);
		195
		196	debugOut.analog[12] = term[PITCH];
		197	debugOut.analog[13] = term[ROLL];
		198	debugOut.analog[14] = term[YAW];
		199
		200	//DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
		201	//DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
		202	//debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg
		203	//debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg
		204	}
1910	-	205	}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/flight.c - Rev 2103