/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 " |
}; |
/****************************************************************/ |