Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2072 → Rev 2073

/branches/dongfang_FC_rewrite/analog.c
123,15 → 123,15
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
int32_t filteredAirPressure;
 
#define MAX_D_AIRPRESSURE_WINDOW_LENGTH 5
#define MAX_D_AIRPRESSURE_WINDOW_LENGTH 32
//int32_t lastFilteredAirPressure;
int16_t dAirPressureWindow[MAX_D_AIRPRESSURE_WINDOW_LENGTH];
uint8_t dWindowPtr;
uint8_t dWindowPtr = 0;
 
#define MAX_AIRPRESSURE_WINDOW_LENGTH 32
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
int32_t windowedAirPressure;
uint8_t windowPtr;
uint8_t windowPtr = 0;
 
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
int32_t airPressureSum;
488,12 → 488,12
windowedAirPressure += simpleAirPressure;
windowedAirPressure -= airPressureWindow[windowPtr];
airPressureWindow[windowPtr++] = simpleAirPressure;
if (windowPtr == staticParams.airpressureWindowLength) windowPtr = 0;
if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0;
filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
}
 
dAirPressureWindow[dWindowPtr++] = (int16_t)(filteredAirPressure - lastFilteredAirPressure);
if (dWindowPtr == staticParams.dAirpressureWindowLength) dWindowPtr = 0;
if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0;
 
pressureMeasurementCount = airPressureSum = 0;
}
624,10 → 624,11
 
int16_t analog_getDHeight(void) {
int16_t result = 0;
for (int i=0; i<staticParams.dAirpressureWindowLength; i++) {
result += dAirPressureWindow[i];
for (int i=0; i<staticParams.airpressureDWindowLength; i++) {
result -= dAirPressureWindow[i]; // minus pressure is plus height.
}
 
debugOut.analog[30] = result;
// dHeight = -dPressure, so here it is the old pressure minus the current, not opposite.
return result;
}
/branches/dongfang_FC_rewrite/configuration.c
195,7 → 195,6
staticParams.heightD = 30;
staticParams.heightSetting = 251;
staticParams.heightControlMaxThrottleChange = 10;
staticParams.heightSlewRate = 4;
// Control
staticParams.stickP = 8;
/branches/dongfang_FC_rewrite/configuration.h
179,15 → 179,15
// Height Control
uint8_t airpressureFilterConstant;
uint8_t airpressureWindowLength; // 0 means: Use filter.
uint8_t dAirpressureWindowLength; // values 1..5 are legal.
uint8_t airpressureDWindowLength; // values 1..5 are legal.
uint8_t airpressureAccZCorrection;
uint8_t heightP;
uint8_t heightI;
uint8_t heightD;
uint8_t heightSetting;
uint8_t heightControlMaxIntegral;
uint8_t heightControlMaxIntegralIn;
uint8_t heightControlMaxIntegralOut;
uint8_t heightControlMaxThrottleChange;
uint8_t heightSlewRate;
 
// Servos
uint8_t servoCount;
/branches/dongfang_FC_rewrite/controlMixer.c
71,8 → 71,6
else if (variables[i] > 0 && variables[i] > targetvalue)
variables[i]--;
}
 
debugOut.analog[31] = variables[0];
}
 
uint8_t controlMixer_getSignalQuality(void) {
121,6 → 119,9
// Decode commands.
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
: COMMAND_NONE;
 
debugOut.analog[31] = rcCommand;
 
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
: COMMAND_NONE;
 
184,7 → 185,6
updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);
updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);
dampenControlActivity();
//debugOut.analog[17] = controlActivity/10;
// We can safely do this even with a bad signal - the variables will not have been updated then.
configuration_applyVariablesToParams();
/branches/dongfang_FC_rewrite/failsafeControl.c
15,7 → 15,7
 
debugOut.analog[25] = emergencyFlightTime;
 
debugOut.analog[30] = controlMixer_getSignalQuality();
// debugOut.analog[30] = controlMixer_getSignalQuality();
 
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
if (controlMixer_didReceiveSignal)
/branches/dongfang_FC_rewrite/flight.c
278,8 → 278,10
CHECK_MIN_MAX(tmp, 1, 255);
throttle = tmp;
 
/*
if (i < 4)
debugOut.analog[10 + i] = throttle;
*/
 
if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
motor[i].throttle = throttle;
/branches/dongfang_FC_rewrite/heightControl.c
96,7 → 96,7
HC_periodicTask();
int16_t throttle = PRTY[CONTROL_THROTTLE];
int32_t height = analog_getHeight();
int32_t heightError = /*ramped*/ targetHeight - height;
int32_t heightError = targetHeight - height;
int16_t dHeight = analog_getDHeight();
debugOut.analog[22] = height/10L;
114,27 → 114,37
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
}
 
int32_t heightErrorForIntegral = heightError;
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << 10;
 
if (heightErrorForIntegral > heightErrorForIntegralLimit) {
heightErrorForIntegral = heightErrorForIntegralLimit;
} else if (heightErrorForIntegral < -heightErrorForIntegralLimit) {
heightErrorForIntegral =- heightErrorForIntegralLimit;
}
 
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
iHeight += heightError;
iHeight += heightErrorForIntegral;
 
#define IHEIGHT_SCALE 24
// dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
 
if (dThrottleI > staticParams.heightControlMaxIntegral) {
dThrottleI = staticParams.heightControlMaxIntegral;
iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
} else if (dThrottleI < -staticParams.heightControlMaxIntegral) {
dThrottleI = -staticParams.heightControlMaxIntegral;
iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
if (dThrottleI > staticParams.heightControlMaxIntegralOut) {
dThrottleI = staticParams.heightControlMaxIntegralOut;
iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
} else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) {
dThrottleI = -staticParams.heightControlMaxIntegralOut;
iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
}
 
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
 
//debugOut.analog[24] = dThrottleP;
//debugOut.analog[25] = dThrottleI;
//debugOut.analog[26] = dThrottleD;
debugOut.analog[10] = dThrottleP;
debugOut.analog[11] = dThrottleI;
debugOut.analog[12] = dThrottleD;
debugOut.analog[13] = heightError/10;
 
//debugOut.analog[27] = dynamicParams.heightP;
//debugOut.analog[28] = dynamicParams.heightI;
/branches/dongfang_FC_rewrite/rc.c
111,7 → 111,7
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
if ((signal > 250) && (signal < 687)) {
// shift signal to zero symmetric range -154 to 159
signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
// check for stable signal
if (abs(signal - PPM_in[index]) < 6) {
if (RCQuality < 200)
131,7 → 131,7
tmp = signal;
// calculate signal difference on good signal level
if (RCQuality >= 195)
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
PPM_diff[index] = signal - PPM_in[index]; //((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
else
PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
/branches/dongfang_FC_rewrite/uart0.c
98,16 → 98,16
"RollTerm ",
"ThrottleTerm ",
"YawTerm ",
"M1 ", //10
"M2 ",
"M3 ",
"M4 ",
"heightP ", //10
"heightI ",
"heightD ",
"heightErr/10 ",
"controlActivity ",
"accVector ", //15
"RC Stick P ",
"RC Stick R ",
"RC Stick T ",
"RC Stick Y ",
"ppm 0 ",
"ppm 1 ",
"ppm 2 ",
"ppm 3 ",
"control act wghd", //20
"acc vector wghd ",
"Height[dm] ",
118,8 → 118,8
"naviRoll ",
"i part contrib ",
"Acc Z ",
"Signal Qual ", //30
"var0 "
"Height D ", //30
"RCCommand "
};
 
/****************************************************************/