Rev 2108 | Rev 2116 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2108 | Rev 2109 | ||
|---|---|---|---|
| Line 42... | Line 42... | ||
| 42 | /* |
42 | /* |
| 43 | * Airspeed |
43 | * Airspeed |
| 44 | */ |
44 | */ |
| 45 | uint32_t airpressure; |
45 | uint32_t airpressure; |
| 46 | uint16_t airspeedVelocity = 0; |
46 | uint16_t airspeedVelocity = 0; |
| 47 | int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH]; |
47 | //int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH]; |
| 48 | uint8_t airpressureWindowIdx = 0; |
48 | //uint8_t airpressureWindowIdx = 0; |
| Line 49... | Line 49... | ||
| 49 | 49 | ||
| 50 | /* |
50 | /* |
| 51 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
51 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
| 52 | * standing still. They are used for adjusting the gyro and acc. meter values |
52 | * standing still. They are used for adjusting the gyro and acc. meter values |
| Line 210... | Line 210... | ||
| 210 | *noiseMeasurement = 0; |
210 | *noiseMeasurement = 0; |
| 211 | } |
211 | } |
| 212 | } |
212 | } |
| Line 213... | Line 213... | ||
| 213 | 213 | ||
| 214 | void startAnalogConversionCycle(void) { |
- | |
| 215 | sensorDataReady = 0; |
- | |
| 216 | 214 | void startAnalogConversionCycle(void) { |
|
| 217 | // Stop the sampling. Cycle is over. |
215 | // Stop the sampling. Cycle is over. |
| 218 | for (uint8_t i = 0; i<8; i++) { |
216 | for (uint8_t i = 0; i<8; i++) { |
| 219 | ADSensorInputs[i] = 0; |
217 | ADSensorInputs[i] = 0; |
| Line 220... | Line 218... | ||
| 220 | } |
218 | } |
| 221 | 219 | ||
| 222 | adState = 0; |
220 | adState = 0; |
| 223 | adChannel = AD_AIRPRESSURE; |
221 | adChannel = AD_AIRPRESSURE; |
| 224 | ADMUX = (ADMUX & 0xE0) | adChannel; |
222 | ADMUX = (ADMUX & 0xE0) | adChannel; |
| - | 223 | startADC(); |
|
| 225 | startADC(); |
224 | twimaster_startCycle(); |
| Line 226... | Line 225... | ||
| 226 | twimaster_startCycle(); |
225 | sensorDataReady = 0; |
| 227 | } |
226 | } |
| 228 | 227 | ||
| Line 306... | Line 305... | ||
| 306 | #define LOG_AIRSPEED_FACTOR 2 |
305 | #define LOG_AIRSPEED_FACTOR 2 |
| Line 307... | Line 306... | ||
| 307 | 306 | ||
| 308 | void analog_updateAirspeed(void) { |
307 | void analog_updateAirspeed(void) { |
| 309 | uint16_t rawAirpressure = ADSensorInputs[AD_AIRPRESSURE]; |
308 | uint16_t rawAirpressure = ADSensorInputs[AD_AIRPRESSURE]; |
| 310 | int16_t temp = rawAirpressure - airpressureOffset; |
309 | int16_t temp = rawAirpressure - airpressureOffset; |
| 311 | airpressure -= airpressureWindow[airpressureWindowIdx]; |
310 | // airpressure -= airpressureWindow[airpressureWindowIdx]; |
| 312 | airpressure += temp; |
311 | // airpressure += temp; |
| 313 | airpressureWindow[airpressureWindowIdx] = temp; |
312 | // airpressureWindow[airpressureWindowIdx] = temp; |
| 314 | airpressureWindowIdx++; |
313 | // airpressureWindowIdx++; |
| 315 | if (airpressureWindowIdx == AIRPRESSURE_WINDOW_LENGTH) { |
314 | // if (airpressureWindowIdx == AIRPRESSURE_WINDOW_LENGTH) { |
| 316 | airpressureWindowIdx = 0; |
315 | // airpressureWindowIdx = 0; |
| - | 316 | // } |
|
| - | 317 | ||
| - | 318 | #define AIRPRESSURE_FILTER 16 |
|
| Line 317... | Line 319... | ||
| 317 | } |
319 | airpressure = ((int32_t)airpressure * (AIRPRESSURE_FILTER-1) + (AIRPRESSURE_FILTER/2) + temp) / AIRPRESSURE_FILTER; |
| 318 | 320 | ||
| - | 321 | uint16_t p2 = (airpressure<0) ? 0 : airpressure; |
|
| - | 322 | airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(p2)) >> LOG_AIRSPEED_FACTOR; |
|
| - | 323 | ||
| - | 324 | debugOut.analog[17] = airpressure; |
|
| - | 325 | debugOut.analog[18] = airpressureOffset; |
|
| 319 | if (temp<0) temp = 0; |
326 | debugOut.analog[19] = airspeedVelocity; |
| 320 | airspeedVelocity = (staticParams.airspeedCorrection * isqrt32(airpressure)) >> LOG_AIRSPEED_FACTOR; |
327 | |
| Line 321... | Line 328... | ||
| 321 | isFlying = (airspeedVelocity >= staticParams.isFlyingThreshold); |
328 | isFlying = 0; //(airspeedVelocity >= staticParams.isFlyingThreshold); |
| 322 | } |
329 | } |
| 323 | 330 | ||
| Line 353... | Line 360... | ||
| 353 | for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) { |
360 | for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) { |
| 354 | gyroDWindow[i][j] = 0; |
361 | gyroDWindow[i][j] = 0; |
| 355 | } |
362 | } |
| 356 | } |
363 | } |
| Line 357... | Line -... | ||
| 357 | - | ||
| 358 | for (uint8_t i=0; i<AIRPRESSURE_WINDOW_LENGTH; i++) { |
- | |
| 359 | airpressureWindow[i] = 0; |
- | |
| 360 | } |
- | |
| 361 | 364 | ||
| 362 | // Setting offset values has an influence in the analog.c ISR |
365 | // Setting offset values has an influence in the analog.c ISR |
| 363 | // Therefore run measurement for 100ms to achive stable readings |
366 | // Therefore run measurement for 100ms to achive stable readings |
| Line 364... | Line 367... | ||
| 364 | delay_ms_with_adc_measurement(100, 0); |
367 | delay_ms_with_adc_measurement(100, 0); |
| 365 | 368 | ||
| Line 366... | Line 369... | ||
| 366 | // gyroActivity = 0; |
369 | // gyroActivity = 0; |
| 367 | } |
370 | } |
| 368 | 371 | ||
| 369 | void analog_calibrate(void) { |
372 | void analog_calibrate(void) { |
| Line 370... | Line 373... | ||
| 370 | #define OFFSET_CYCLES 64 |
373 | #define OFFSET_CYCLES 32 |
| 371 | uint8_t i, axis; |
374 | uint8_t i, axis; |
| Line 380... | Line 383... | ||
| 380 | offsets[3] += ADSensorInputs[AD_AIRPRESSURE]; |
383 | offsets[3] += ADSensorInputs[AD_AIRPRESSURE]; |
| 381 | } |
384 | } |
| Line 382... | Line 385... | ||
| 382 | 385 | ||
| 383 | for (axis = PITCH; axis <= YAW; axis++) { |
386 | for (axis = PITCH; axis <= YAW; axis++) { |
| 384 | gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
- | |
| 385 | int16_t min = (512-200) * GYRO_OVERSAMPLING; |
- | |
| 386 | int16_t max = (512+200) * GYRO_OVERSAMPLING; |
- | |
| 387 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
- | |
| 388 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
387 | gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
| Line 389... | Line 388... | ||
| 389 | } |
388 | } |
| 390 | 389 | ||
| 391 | airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
390 | airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
| - | 391 | int16_t min = 200; |
|
| 392 | int16_t min = 200; |
392 | int16_t max = 1024-200; |
| 393 | int16_t max = (1024-200) * 2; |
393 | |
| Line 394... | Line 394... | ||
| 394 | if(airpressureOffset < min || airpressureOffset > max) |
394 | if(airpressureOffset < min || airpressureOffset > max) |