Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2031 → Rev 2032

/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
5,11 → 5,13
// Nothing to calibrate.
}
 
void gyro_init() {
void gyro_init(void) {
// No amplifiers, no DAC.
}
 
void gyro_setDefaultParameters(void) {
staticParams.accQuadrant = 4;
staticParams.imuReversedFlags = IMU_REVERSE_ACC_XY;
staticParams.gyroD = 5;
staticParams.driftCompDivider = 1;
staticParams.driftCompLimit = 3;
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
76,9 → 76,10
}
 
void gyro_setDefaultParameters(void) {
staticParams.accQuadrant = 4;
staticParams.imuReversedFlags = IMU_REVERSE_ACC_XY;
staticParams.gyroD = 3;
staticParams.driftCompDivider = 1;
staticParams.driftCompLimit = 200;
staticParams.zerothOrderCorrection = 25;
staticParams.imuReversedFlags = IMU_REVERSE_ACC_XY;
}
/branches/dongfang_FC_rewrite/analog.c
1,28 → 1,28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + Nur f�r den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.
// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt und genannt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// + mit unserer Zustimmung zul�ssig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
426,7 → 426,6
}
 
rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY);
 
for(uint8_t axis=0; axis<3; axis++) {
filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
measureNoise(acc[axis], &accNoisePeak[axis], 1);
/branches/dongfang_FC_rewrite/attitude.c
1,28 → 1,28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + Nur f�r den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zulässig ist.
// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt und genannt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// + mit unserer Zustimmung zul�ssig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
156,7 → 156,8
 
int32_t getAngleEstimateFromAcc(uint8_t axis) {
//int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;
return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];// + correctionTerm;
return GYRO_ACC_FACTOR * filteredAcc[axis];// + correctionTerm;
// return 342L * filteredAcc[axis];
}
 
void setStaticAttitudeAngles(void) {
173,7 → 174,7
************************************************************************/
void attitude_setNeutral(void) {
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;
// dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;
 
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
correctionSum[PITCH] = correctionSum[ROLL] = 0;
331,8 → 332,7
*/
for (axis = PITCH; axis <= ROLL; axis++) {
accDerived = getAngleEstimateFromAcc(axis);
debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
 
//debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
// 1000 * the correction amount that will be added to the gyro angle in next line.
temp = angle[axis];
angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
340,12 → 340,18
correctionSum[axis] += angle[axis] - temp;
}
} else {
debugOut.analog[9] = 0;
debugOut.analog[10] = 0;
//debugOut.analog[9] = 0;
//debugOut.analog[10] = 0;
// experiment: Kill drift compensation updates when not flying smooth.
// correctionSum[PITCH] = correctionSum[ROLL] = 0;
debugOut.digital[0] |= DEBUG_ACC0THORDER;
}
 
int32_t accDerived = getAngleEstimateFromAcc(0);
debugOut.analog[9 + 0] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
 
accDerived = getAngleEstimateFromAcc(1);
debugOut.analog[9 + 1] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
}
 
/************************************************************************
/branches/dongfang_FC_rewrite/configuration.c
88,26 → 88,19
SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200);
SET_POT(dynamicParams.gyroI, staticParams.gyroI);
SET_POT(dynamicParams.gyroD, staticParams.gyroD);
SET_POT(dynamicParams.IFactor, staticParams.IFactor);
SET_POT(dynamicParams.yawIFactor, staticParams.yawIFactor);
SET_POT(dynamicParams.compassYawEffect,staticParams.compassYawEffect);
 
SET_POT(dynamicParams.externalControl, staticParams.externalControl);
SET_POT(dynamicParams.axisCoupling1, staticParams.axisCoupling1);
SET_POT(dynamicParams.axisCoupling2, staticParams.axisCoupling2);
SET_POT(dynamicParams.axisCouplingYawCorrection, staticParams.axisCouplingYawCorrection);
SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability);
SET_POT(dynamicParams.maxAccVector,staticParams.maxAccVector);
 
SET_POT_MM(dynamicParams.heightP, staticParams.heightP,0,100);
SET_POT_MM(dynamicParams.heightD, staticParams.heightD,0,100);
SET_POT(dynamicParams.heightP, staticParams.heightP);
SET_POT(dynamicParams.heightI, staticParams.heightI);
SET_POT(dynamicParams.heightD, staticParams.heightD);
SET_POT(dynamicParams.heightSetting,staticParams.heightSetting);
 
SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl);
 
for (i=0; i<sizeof(staticParams.userParams); i++) {
SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]);
}
 
SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl);
SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl);
 
116,6 → 109,10
 
SET_POT(dynamicParams.levelCorrection[0], staticParams.levelCorrection[0]);
SET_POT(dynamicParams.levelCorrection[1], staticParams.levelCorrection[1]);
 
for (i=0; i<sizeof(staticParams.userParams); i++) {
SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]);
}
}
 
const XLATION XLATIONS[] = {
/branches/dongfang_FC_rewrite/configuration.h
40,38 → 40,31
typedef struct {
// IMU
/*PMM*/uint8_t gyroP;
/* P */uint8_t gyroI;
/* P */uint8_t gyroD;
/* P */uint8_t gyroI;
/* P */uint8_t IFactor;
uint8_t yawIFactor;
/* P */uint8_t compassYawEffect;
 
// Control
/* P */uint8_t externalControl;
 
/* P */uint8_t axisCoupling1;
/* P */uint8_t axisCoupling2;
/* P */uint8_t axisCouplingYawCorrection;
/* P */uint8_t dynamicStability;
uint8_t maxAccVector;
 
// Height control
/*PMM*/uint8_t heightP;
/* P */uint8_t heightI;
/*PMM*/uint8_t heightD;
/* P */uint8_t heightSetting;
/* P */uint8_t heightACCEffect;
 
uint8_t attitudeControl;
 
// The rest...
/* P */uint8_t userParams[8];
/*PMM*/uint8_t output0Timing;
/*PMM*/uint8_t output1Timing;
 
uint8_t servoManualControl[2];
uint8_t levelCorrection[2];
 
uint8_t motorSmoothing;
uint8_t levelCorrection[2];
/* P */uint8_t userParams[8];
} dynamicParam_t;
 
extern volatile dynamicParam_t dynamicParams;
120,30 → 113,6
// Global bitflags
uint8_t bitConfig; // see upper defines for bitcoding
// Height Control
uint8_t airpressureFilter;
uint8_t airpressureAccZCorrection;
uint8_t heightP;
uint8_t heightD;
uint8_t heightSetting;
uint8_t heightControlMaxThrottleChange;
uint8_t heightSlewRate;
 
// Attitude Control
uint8_t attitudeControl;
 
// Control
uint8_t stickP;
uint8_t stickD;
uint8_t stickYawP;
uint8_t stickThrottleD;
uint8_t minThrottle;
uint8_t maxThrottle;
uint8_t externalControl; // for serial Control
uint8_t maxAccVector;
uint8_t maxControlActivity;
uint8_t motorSmoothing;
// IMU
uint8_t gyroQuadrant;
uint8_t accQuadrant;
154,10 → 123,8
uint8_t gyroDFilterConstant;
uint8_t accFilterConstant;
 
uint8_t gyroP;
uint8_t gyroI;
uint8_t gyroD;
 
uint8_t maxAccVector;
uint8_t maxControlActivity;
uint8_t zerothOrderCorrection;
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung)
uint8_t driftCompLimit; // limit for gyrodrift compensation
166,22 → 133,46
uint8_t axisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t axisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
 
uint8_t levelCorrection[2];
 
// Control
uint8_t gyroP;
uint8_t gyroI;
uint8_t gyroD;
 
uint8_t attitudeControl;
uint8_t stickP;
uint8_t stickD;
uint8_t stickYawP;
uint8_t stickThrottleD;
uint8_t minThrottle;
uint8_t maxThrottle;
uint8_t externalControl; // for serial Control
uint8_t motorSmoothing;
uint8_t dynamicStability; // PID limit for Attitude controller
uint8_t IFactor;
uint8_t yawIFactor;
uint8_t compassYawEffect;
uint8_t levelCorrection[2];
uint8_t batteryVoltageWarning;
uint8_t emergencyThrottle;
uint8_t emergencyFlightDuration;
 
// Height Control
uint8_t airpressureFilter;
uint8_t airpressureAccZCorrection;
uint8_t heightP;
uint8_t heightI;
uint8_t heightD;
uint8_t heightSetting;
uint8_t heightControlMaxIntegralThrottleChange;
uint8_t heightControlMaxThrottleChange;
uint8_t heightSlewRate;
 
// Servos
uint8_t servoCount;
uint8_t servoManualMaxSpeed;
servo_t servoConfigurations[2]; // [PITCH, ROLL]
 
// Battery warning and emergency flight
uint8_t batteryVoltageWarning;
uint8_t emergencyThrottle;
uint8_t emergencyFlightDuration;
 
// Outputs
output_flash_t outputFlash[2];
uint8_t outputDebugMask;
/branches/dongfang_FC_rewrite/flight.c
128,11 → 128,11
 
void setNormalFlightParameters(void) {
setFlightParameters(
dynamicParams.IFactor,
staticParams.IFactor,
dynamicParams.gyroP,
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
dynamicParams.gyroP,
dynamicParams.yawIFactor
staticParams.yawIFactor
);
}
 
/branches/dongfang_FC_rewrite/heightControl.c
58,7 → 58,7
}
} else {
// Switch is not activated; take the "max-height" as the target height.
targetHeight = (uint16_t) dynamicParams.heightSetting * 5L - 500L; // should be: 100 (or make a param out of it)
targetHeight = (uint16_t) dynamicParams.heightSetting * 50L - 3000L; // should be: 100 (or make a param out of it)
}
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
97,12 → 97,9
int16_t dHeight = height - lastHeight;
lastHeight = height;
// DebugOut.Analog[20] = dHeight;
// DebugOut.Analog[21] = dynamicParams.MaxHeight;
 
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
// iHeight += heightError;
iHeight += heightError;
if (heightError > 0) {
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
112,16 → 109,15
debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
}
/*
if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
*/
int16_t dThrottle = ((heightError * staticParams.heightP) >> 10)
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */-((dHeight * staticParams.heightD) >> 7);
int16_t dThrottle = (iHeight * staticParams.heightI) / 10000L;
 
if (dThrottle > staticParams.heightControlMaxIntegralThrottleChange)
dThrottle = staticParams.heightControlMaxIntegralThrottleChange;
else if (dThrottle < -staticParams.heightControlMaxIntegralThrottleChange)
dThrottle = -staticParams.heightControlMaxIntegralThrottleChange;
 
dThrottle += ((heightError * staticParams.heightP) >> 10) + ((dHeight * staticParams.heightD) >> 7);
 
if (dThrottle > staticParams.heightControlMaxThrottleChange)
dThrottle = staticParams.heightControlMaxThrottleChange;
else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
130,6 → 126,7
debugOut.analog[19] = rampedTargetHeight;
debugOut.analog[21] = dThrottle;
debugOut.analog[26] = height;
debugOut.analog[27] = (iHeight * staticParams.heightI) / 10000L;
 
if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
/branches/dongfang_FC_rewrite/makefile
8,7 → 8,7
VERSION_PATCH = 0
 
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version
VERSION_SERIAL_MINOR = 2 # Serial Protocol Minor Version
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version
 
#-------------------------------------------------------------------
264,8 → 264,8
#AVRDUDE_PROGRAMMER = dt006
#AVRDUDE_PROGRAMMER = stk200
#AVRDUDE_PROGRAMMER = ponyser
AVRDUDE_PROGRAMMER = avrispv2
#AVRDUDE_PROGRAMMER = usbtiny
#AVRDUDE_PROGRAMMER = avrispv2
AVRDUDE_PROGRAMMER = usbtiny
#falls Ponyser ausgewaehlt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden
 
#AVRDUDE_PORT = com1 # programmer connected to serial device
517,4 → 517,3
# Listing of phony targets.
.PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \
clean clean_list program
/branches/dongfang_FC_rewrite/uart0.c
148,7 → 148,7
"M3 ",
"M4 ", //25
"Height ",
"Airpress. Range ",
"HeightI",
"DriftCompPitch ",
"DriftCompRoll ",
"Altitude ", //30