| /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) { |