Rev 2106 | Rev 2110 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2106 | Rev 2109 | ||
|---|---|---|---|
| Line 44... | Line 44... | ||
| 44 | int16_t gyroD[3]; |
44 | int16_t gyroD[3]; |
| 45 | int16_t gyroDWindow[3][GYRO_D_WINDOW_LENGTH]; |
45 | int16_t gyroDWindow[3][GYRO_D_WINDOW_LENGTH]; |
| 46 | uint8_t gyroDWindowIdx = 0; |
46 | uint8_t gyroDWindowIdx = 0; |
| Line 47... | Line 47... | ||
| 47 | 47 | ||
| - | 48 | /* |
|
| - | 49 | * Airspeed |
|
| - | 50 | */ |
|
| - | 51 | int16_t airpressure; |
|
| - | 52 | uint16_t airspeedVelocity = 0; |
|
| - | 53 | //int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH]; |
|
| - | 54 | //uint8_t airpressureWindowIdx = 0; |
|
| - | 55 | ||
| 48 | /* |
56 | /* |
| 49 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
57 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
| 50 | * standing still. They are used for adjusting the gyro and acc. meter values |
58 | * standing still. They are used for adjusting the gyro and acc. meter values |
| 51 | * to be centered on zero. |
59 | * to be centered on zero. |
| 52 | */ |
60 | */ |
| Line 107... | Line 115... | ||
| 107 | if (reverseYaw) |
115 | if (reverseYaw) |
| 108 | result[3] =-result[3]; |
116 | result[3] =-result[3]; |
| 109 | } |
117 | } |
| Line 110... | Line 118... | ||
| 110 | 118 | ||
| 111 | /* |
- | |
| 112 | * Airspeed |
- | |
| 113 | */ |
- | |
| 114 | uint16_t simpleAirPressure; |
- | |
| 115 | - | ||
| 116 | /* |
119 | /* |
| 117 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
120 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
| 118 | * That is divided by 3 below, for a final 10.34 per volt. |
121 | * That is divided by 3 below, for a final 10.34 per volt. |
| 119 | * So the initial value of 100 is for 9.7 volts. |
122 | * So the initial value of 100 is for 9.7 volts. |
| 120 | */ |
123 | */ |
| 121 | uint16_t UBat = 100; |
- | |
| Line 122... | Line 124... | ||
| 122 | uint16_t airspeedVelocity = 0; |
124 | uint16_t UBat = 100; |
| 123 | 125 | ||
| 124 | /* |
126 | /* |
| 125 | * Control and status. |
127 | * Control and status. |
| Line 266... | Line 268... | ||
| 266 | analogDataReady = 1; |
268 | analogDataReady = 1; |
| 267 | // do not restart ADC converter. |
269 | // do not restart ADC converter. |
| 268 | } |
270 | } |
| 269 | } |
271 | } |
| Line 270... | Line -... | ||
| 270 | - | ||
| 271 | /* |
- | |
| 272 | void measureGyroActivity(int16_t newValue) { |
- | |
| 273 | gyroActivity += (uint32_t)((int32_t)newValue * newValue); |
- | |
| 274 | } |
- | |
| 275 | - | ||
| 276 | #define GADAMPING 6 |
- | |
| 277 | void dampenGyroActivity(void) { |
- | |
| 278 | static uint8_t cnt = 0; |
- | |
| 279 | if (++cnt >= IMUConfig.gyroActivityDamping) { |
- | |
| 280 | cnt = 0; |
- | |
| 281 | gyroActivity *= (uint32_t)((1L<<GADAMPING)-1); |
- | |
| 282 | gyroActivity >>= GADAMPING; |
- | |
| 283 | } |
- | |
| 284 | } |
- | |
| 285 | */ |
- | |
| 286 | 272 | ||
| 287 | void analog_updateGyros(void) { |
273 | void analog_updateGyros(void) { |
| 288 | // for various filters... |
274 | // for various filters... |
| Line 289... | Line 275... | ||
| 289 | int16_t tempOffsetGyro[3], tempGyro; |
275 | int16_t tempOffsetGyro[3], tempGyro; |
| Line 350... | Line 336... | ||
| 350 | gyroDWindowIdx = 0; |
336 | gyroDWindowIdx = 0; |
| 351 | } |
337 | } |
| 352 | } |
338 | } |
| Line 353... | Line 339... | ||
| 353 | 339 | ||
| 354 | // probably wanna aim at 1/10 m/s/unit. |
340 | // probably wanna aim at 1/10 m/s/unit. |
| Line 355... | Line 341... | ||
| 355 | #define LOG_AIRSPEED_FACTOR 2 |
341 | #define LOG_AIRSPEED_FACTOR 0 |
| 356 | 342 | ||
| 357 | void analog_updateAirspeed(void) { |
343 | void analog_updateAirspeed(void) { |
| - | 344 | uint16_t rawAirpressure = sensorInputs[AD_AIRPRESSURE]; |
|
| 358 | uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
345 | int16_t temp = airpressureOffset - rawAirpressure; |
| - | 346 | //airpressure -= airpressureWindow[airpressureWindowIdx]; |
|
| 359 | int16_t temp = rawAirPressure - airpressureOffset; |
347 | //airpressure += temp; |
| - | 348 | //airpressureWindow[airpressureWindowIdx] = temp; |
|
| - | 349 | //airpressureWindowIdx++; |
|
| - | 350 | //if (airpressureWindowIdx == AIRPRESSURE_WINDOW_LENGTH) { |
|
| - | 351 | // airpressureWindowIdx = 0; |
|
| - | 352 | //} |
|
| - | 353 | ||
| - | 354 | #define AIRPRESSURE_FILTER 16 |
|
| - | 355 | airpressure = ((int32_t)airpressure * (AIRPRESSURE_FILTER-1) + (AIRPRESSURE_FILTER/2) + temp) / AIRPRESSURE_FILTER; |
|
| 360 | if (temp<0) temp = 0; |
356 | |
| - | 357 | uint16_t p2 = (airpressure<0) ? 0 : airpressure; |
|
| - | 358 | airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(p2)) >> LOG_AIRSPEED_FACTOR; |
|
| - | 359 | ||
| - | 360 | debugOut.analog[17] = airpressure; |
|
| - | 361 | debugOut.analog[18] = airpressureOffset; |
|
| - | 362 | debugOut.analog[19] = airspeedVelocity; |
|
| 361 | simpleAirPressure = temp; |
363 | |
| Line 362... | Line 364... | ||
| 362 | airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(simpleAirPressure)) >> LOG_AIRSPEED_FACTOR; |
364 | isFlying = 0; //(airspeedVelocity >= staticParams.isFlyingThreshold); |
| 363 | } |
365 | } |
| 364 | 366 | ||