Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2025 → Rev 2026

/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
80,4 → 80,5
staticParams.driftCompDivider = 1;
staticParams.driftCompLimit = 200;
staticParams.zerothOrderCorrection = 25;
staticParams.imuReversedFlags = IMU_REVERSE_ACC_XY;
}
/branches/dongfang_FC_rewrite/analog.c
173,6 → 173,11
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
int32_t filteredAirPressure;
 
#define MAX_AIRPRESSURE_WINDOW_LENGTH 64
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
int32_t windowedAirPressure;
uint8_t windowPtr;
 
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
int32_t airPressureSum;
 
264,6 → 269,12
//Trigger Source to Free Running Mode
ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
 
for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
airPressureWindow[i] = 0;
}
 
windowedAirPressure = 0;
 
startAnalogConversionCycle();
 
// restore global interrupt flags
296,7 → 307,9
* Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
*/
uint16_t getSimplePressure(int advalue) {
return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10;
return result;
}
 
void startAnalogConversionCycle(void) {
434,8 → 447,6
if (pressureAutorangingWait) {
//A range switch was done recently. Wait for steadying.
pressureAutorangingWait--;
debugOut.analog[27] = (uint16_t) OCR0A;
debugOut.analog[31] = simpleAirPressure;
} else {
rawAirPressure = sensorInputs[AD_AIRPRESSURE];
if (rawAirPressure < MIN_RAWPRESSURE) {
467,8 → 478,6
// Even if the sample is off-range, use it.
simpleAirPressure = getSimplePressure(rawAirPressure);
debugOut.analog[27] = (uint16_t) OCR0A;
debugOut.analog[31] = simpleAirPressure;
if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
// Danger: pressure near lower end of range. If the measurement saturates, the
486,7 → 495,7
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
} else {
// normal case.
// If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample.
// If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample.
// The 2 cases above (end of range) are ignored for this.
debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING - 1)
502,6 → 511,13
+ airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER;
pressureMeasurementCount = airPressureSum = 0;
}
//int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
//int32_t windowedAirPressure = 0;
//uint8_t windowPtr;
windowedAirPressure += simpleAirPressure;
windowedAirPressure -= airPressureWindow[windowPtr];
airPressureWindow[windowPtr] = simpleAirPressure;
windowPtr = (windowPtr+1) % MAX_AIRPRESSURE_WINDOW_LENGTH;
}
}
 
/branches/dongfang_FC_rewrite/attitude.c
396,7 → 396,6
temp = filteredAcc[2] >> 3;
accVector += temp * temp;
debugOut.analog[18] = accVector;
debugOut.analog[19] = dynamicParams.maxAccVector;
}
 
/************************************************************************
/branches/dongfang_FC_rewrite/configuration.c
214,6 → 214,7
 
void setOtherDefaults(void) {
// Height Control
staticParams.airpressureAccZCorrection = 128+56;
staticParams.heightP = 10;
staticParams.heightD = 30;
staticParams.heightSetting = 251;
311,7 → 312,6
mixerMatrix.motor[i][MIX_ROLL] = 0;
mixerMatrix.motor[i][MIX_YAW] = 0;
}
/*
// default = Quadro
mixerMatrix.motor[0][MIX_PITCH] = +64;
mixerMatrix.motor[0][MIX_YAW] = +64;
322,8 → 322,8
mixerMatrix.motor[3][MIX_ROLL] = +64;
mixerMatrix.motor[3][MIX_YAW] = -64;
memcpy(mixerMatrix.name, "Quadro\0", 7);
*/
 
/*
// default = X
mixerMatrix.motor[0][MIX_PITCH] = +45;
mixerMatrix.motor[0][MIX_ROLL] = +45;
340,6 → 340,7
mixerMatrix.motor[3][MIX_PITCH] = -45;
mixerMatrix.motor[3][MIX_ROLL] = +45;
mixerMatrix.motor[3][MIX_YAW] = -64;
*/
 
memcpy(mixerMatrix.name, "X\0", 7);
}
/branches/dongfang_FC_rewrite/configuration.h
121,7 → 121,8
uint8_t bitConfig; // see upper defines for bitcoding
// Height Control
uint8_t airpressureFilter;
uint8_t airpressureFilter;
uint8_t airpressureAccZCorrection;
uint8_t heightP;
uint8_t heightD;
uint8_t heightSetting;
/branches/dongfang_FC_rewrite/flight.c
55,10 → 55,6
#include "flight.h"
#include "output.h"
 
// Only for debug. Remove.
//#include "analog.h"
//#include "rc.h"
 
// Necessary for external control and motor test
#include "uart0.h"
#include "twimaster.h"
182,7 → 178,7
/************************************************************************/
 
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
beepRCAlarm();
if (controlMixer_didReceiveSignal) beepRCAlarm();
if (emergencyFlightTime) {
// continue emergency flight
emergencyFlightTime--;
220,9 → 216,7
*/
if (isFlying < 256) {
IPart[PITCH] = IPart[ROLL] = 0;
// TODO: Don't stomp on other modules' variables!!!
// controlYaw = 0;
PDPartYaw = 0; // instead.
PDPartYaw = 0;
if (isFlying == 250) {
// HC_setGround();
updateCompassCourse = 1;
285,13 → 279,8
// The P-part is the P of the PID controller. That's the angle integrals (not rates).
for (axis = PITCH; axis <= ROLL; axis++) {
PDPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
/*
* Now blend in the D-part - proportional to the Differential of the integral = the rate.
* Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
* where pfactor is in [0..1].
*/
PDPart[axis] += (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING));
PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
}
 
334,7 → 323,6
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = throttleTerm / 2;
}
//CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
} else {
if (yawTerm < -MIN_YAWGAS / 2) {
debugOut.digital[0] |= DEBUG_CLIP;
343,10 → 331,8
debugOut.digital[0] |= DEBUG_CLIP;
yawTerm = MIN_YAWGAS / 2;
}
//CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
}
 
// FIXME: Throttle may exceed maxThrottle (there is no check no more).
tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
if (yawTerm < -(tmp_int - throttleTerm)) {
yawTerm = -(tmp_int - throttleTerm);
392,7 → 378,6
} else if (term[axis] > tmp_int) {
debugOut.digital[1] |= DEBUG_CLIP;
}
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/branches/dongfang_FC_rewrite/heightControl.c
65,7 → 65,7
}
} else {
// Switch is not activated; take the "max-height" as the target height.
targetHeight = (uint16_t) dynamicParams.heightSetting * 100;
targetHeight = (uint16_t) dynamicParams.heightSetting * 5L - 500L; // should be: 100 (or make a param out of it)
}
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
77,7 → 77,7
} else {
rampedTargetHeight = targetHeight;
}
} else if (rampedTargetHeight != targetHeight) {
} else {
// descending
if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
rampedTargetHeight -= staticParams.heightSlewRate;
99,7 → 99,7
uint16_t HC_getThrottle(uint16_t throttle) {
int32_t height = getHeight();
int32_t heightError = rampedTargetHeight - height;
 
static int32_t lastHeight;
int16_t dHeight = height - lastHeight;
111,10 → 111,10
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
// iHeight += heightError;
if (dHeight > 0) {
if (heightError > 0) {
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
} else if (dHeight < 0) {
} else if (heightError < 0) {
debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
}
126,15 → 126,18
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;
int16_t dThrottle = ((heightError * staticParams.heightP) >> 9)
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */-((dHeight * staticParams.heightD) >> 7);
// the "minGas" is now a limit for how much up / down the throttle can be varied
if (dThrottle > staticParams.heightControlMaxThrottleChange)
dThrottle = staticParams.heightControlMaxThrottleChange;
else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
dThrottle = -staticParams.heightControlMaxThrottleChange;
 
debugOut.analog[19] = rampedTargetHeight;
debugOut.analog[21] = dThrottle;
debugOut.analog[26] = height;
 
if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
|| (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
/branches/dongfang_FC_rewrite/invenSense.c
39,5 → 39,5
staticParams.driftCompDivider = 2;
staticParams.driftCompLimit = 5;
staticParams.zerothOrderCorrection = 1;
staticParams.imuReversedFlags = 8;
staticParams.imuReversedFlags = IMU_REVERSE_ACC_Z;
}
/branches/dongfang_FC_rewrite/makefile
23,18 → 23,18
#RC = DSL
#RC = SPECTRUM
 
GYRO=ENC-03_FC1.3
GYRO_HW_NAME=ENC
GYRO_HW_FACTOR=1.304f
#GYRO=ENC-03_FC1.3
#GYRO_HW_NAME=ENC
#GYRO_HW_FACTOR=1.304f
#GYRO_PITCHROLL_CORRECTION=1.0f
#GYRO_YAW_CORRECTION=1.0f
 
GYRO=ADXRS610_FC2.0
GYRO_HW_NAME=ADXR
GYRO_HW_FACTOR=1.2288f
GYRO_PITCHROLL_CORRECTION=1.0f
GYRO_YAW_CORRECTION=1.0f
 
#GYRO=ADXRS610_FC2.0
#GYRO_HW_NAME=ADXR
#GYRO_HW_FACTOR=1.2288f
#GYRO_PITCHROLL_CORRECTION=1.0f
#GYRO_YAW_CORRECTION=1.0f
 
#GYRO=invenSense
#GYRO_HW_NAME=Isense
#GYRO_HW_FACTOR=0.6827f
/branches/dongfang_FC_rewrite/rc.c
61,8 → 61,8
// The channel array is now 0-based.
volatile int16_t PPM_in[MAX_CHANNELS];
volatile int16_t PPM_diff[MAX_CHANNELS];
volatile uint8_t NewPpmData = 1;
volatile uint8_t RCQuality = 0;
volatile uint8_t newPPMData = 1;
volatile uint8_t RCQuality;
int16_t RC_PRTY[4];
uint8_t lastRCCommand = COMMAND_NONE;
uint8_t commandTimer = 0;
160,7 → 160,7
// if a sync gap happens and there where at least 4 channels decoded before
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
if (index >= 3) {
NewPpmData = 0; // Null means NewData for the first 4 channels
newPPMData = 0; // Null means NewData for the first 4 channels
}
// synchronize channel index
index = 0;
243,7 → 243,7
int16_t tmp1, tmp2;
if (RCQuality) {
RCQuality--;
if (NewPpmData-- == 0) {
if (newPPMData-- == 0) {
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP
+ RCDiff(CH_PITCH) * staticParams.stickD;
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP
274,8 → 274,6
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
= RC_PRTY[CONTROL_YAW] = 0;
}
debugOut.analog[21] = RCQuality;
}
 
/*
/branches/dongfang_FC_rewrite/timer0.c
170,8 → 170,7
beepModulation = BEEP_MODULATION_NONE;
}
 
// Beeper remains silent till the first time a control signal was received.
if (beeper_On & controlMixer_didReceiveSignal) {
if (beeper_On) {
// set speaker port to high.
if (boardRelease == 10)
PORTD |= (1 << PORTD2); // Speaker at PD2
/branches/dongfang_FC_rewrite/uart0.c
140,14 → 140,14
"gyroP ",
"ControlAct/10 ",
"Acc. Vector ",
"Max. Acc. Vector",
"targetheight ",
"var0 ", //20
"rc signal ",
"dHeightThrottle ",
"M1 ",
"M2 ",
"M3 ",
"M4 ", //25
"ControlYaw ",
"Height ",
"Airpress. Range ",
"DriftCompPitch ",
"DriftCompRoll ",