/branches/dongfang_FC_rewrite/analog.c |
---|
122,8 → 122,12 |
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
int32_t filteredAirPressure; |
int32_t lastFilteredAirPressure; |
#define MAX_D_AIRPRESSURE_WINDOW_LENGTH 5 |
//int32_t lastFilteredAirPressure; |
int16_t dAirPressureWindow[MAX_D_AIRPRESSURE_WINDOW_LENGTH]; |
uint8_t dWindowPtr; |
#define MAX_AIRPRESSURE_WINDOW_LENGTH 32 |
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH]; |
int32_t windowedAirPressure; |
474,9 → 478,8 |
// The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half. |
airPressureSum += simpleAirPressure >> 2; |
lastFilteredAirPressure = filteredAirPressure; |
uint32_t lastFilteredAirPressure = filteredAirPressure; |
if (!staticParams.airpressureWindowLength) { |
filteredAirPressure = (filteredAirPressure * (staticParams.airpressureFilterConstant - 1) |
+ airPressureSum + staticParams.airpressureFilterConstant / 2) / staticParams.airpressureFilterConstant; |
484,11 → 487,14 |
// use windowed. |
windowedAirPressure += simpleAirPressure; |
windowedAirPressure -= airPressureWindow[windowPtr]; |
airPressureWindow[windowPtr] = simpleAirPressure; |
windowPtr = (windowPtr+1) % staticParams.airpressureWindowLength; |
airPressureWindow[windowPtr++] = simpleAirPressure; |
if (windowPtr == staticParams.airpressureWindowLength) windowPtr = 0; |
filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength; |
} |
dAirPressureWindow[dWindowPtr++] = (int16_t)(filteredAirPressure - lastFilteredAirPressure); |
if (dWindowPtr == staticParams.dAirpressureWindowLength) dWindowPtr = 0; |
pressureMeasurementCount = airPressureSum = 0; |
} |
} |
617,6 → 623,11 |
} |
int16_t analog_getDHeight(void) { |
int16_t result = 0; |
for (int i=0; i<staticParams.dAirpressureWindowLength; i++) { |
result += dAirPressureWindow[i]; |
} |
// dHeight = -dPressure, so here it is the old pressure minus the current, not opposite. |
return lastFilteredAirPressure - filteredAirPressure; |
return result; |
} |
/branches/dongfang_FC_rewrite/configuration.h |
---|
179,6 → 179,7 |
// Height Control |
uint8_t airpressureFilterConstant; |
uint8_t airpressureWindowLength; // 0 means: Use filter. |
uint8_t dAirpressureWindowLength; // values 1..5 are legal. |
uint8_t airpressureAccZCorrection; |
uint8_t heightP; |
uint8_t heightI; |
/branches/dongfang_FC_rewrite/directGPSNaviControl.c |
---|
270,7 → 270,7 |
case INVALID: // invalid gps data |
navi_setNeutral(); |
if (flightMode != GPS_FLIGHT_MODE_FREE) { |
beep(100); // beep if signal is neccesary |
beep(1); // beep if signal is neccesary |
} |
break; |
case PROCESSED: // if gps data are already processed do nothing |
/branches/dongfang_FC_rewrite/heightControl.c |
---|
49,6 → 49,7 |
if (setHeightLatch == LATCH_TIME) { |
// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
/* rampedTargetHeight = */ targetHeight = height; |
iHeight = 0; |
} |
// Time not yet reached. |
setHeightLatch++; |
139,7 → 140,7 |
//debugOut.analog[28] = dynamicParams.heightI; |
//debugOut.analog[29] = dynamicParams.heightD; |
int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD; |
int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD; |
if (dThrottle > staticParams.heightControlMaxThrottleChange) |
dThrottle = staticParams.heightControlMaxThrottleChange; |
/branches/dongfang_FC_rewrite/rc.c |
---|
184,17 → 184,17 |
int16_t tmp1, tmp2; |
if (RCQuality) { |
RCQuality--; |
PRTY[CONTROL_PITCH] += RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
PRTY[CONTROL_ROLL] += RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120; |
// Negative throttle values are taken as zero. |
if (throttle > 0) |
PRTY[CONTROL_THROTTLE] += throttle; |
PRTY[CONTROL_THROTTLE] = throttle; |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
// exponential stick sensitivity in yawing rate |
tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2 |
tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
PRTY[CONTROL_YAW] += tmp2; |
PRTY[CONTROL_YAW] = tmp2; |
uint8_t command = RC_getStickCommand(); |
if (lastRCCommand == command) { |