Rev 2108 | Rev 2129 | 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) |