Rev 2104 | Rev 2106 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2104 | Rev 2105 | ||
---|---|---|---|
Line 49... | Line 49... | ||
49 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
49 | * 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 |
50 | * standing still. They are used for adjusting the gyro and acc. meter values |
51 | * to be centered on zero. |
51 | * to be centered on zero. |
52 | */ |
52 | */ |
53 | sensorOffset_t gyroOffset; |
53 | sensorOffset_t gyroOffset; |
- | 54 | int16_t airpressureOffset; |
|
Line 54... | Line 55... | ||
54 | 55 | ||
55 | /* |
56 | /* |
56 | * In the MK coordinate system, nose-down is positive and left-roll is positive. |
57 | * In the MK coordinate system, nose-down is positive and left-roll is positive. |
57 | * If a sensor is used in an orientation where one but not both of the axes has |
58 | * If a sensor is used in an orientation where one but not both of the axes has |
Line 109... | Line 110... | ||
109 | 110 | ||
110 | /* |
111 | /* |
111 | * Airspeed |
112 | * Airspeed |
112 | */ |
113 | */ |
113 | uint16_t simpleAirPressure; |
- | |
Line 114... | Line 114... | ||
114 | uint16_t airspeedVelocity; |
114 | uint16_t simpleAirPressure; |
115 | 115 | ||
116 | /* |
116 | /* |
117 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
117 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
Line 232... | Line 232... | ||
232 | } else { |
232 | } else { |
233 | *noiseMeasurement = 0; |
233 | *noiseMeasurement = 0; |
234 | } |
234 | } |
235 | } |
235 | } |
Line 236... | Line -... | ||
236 | - | ||
237 | /* |
- | |
238 | * Min.: 0 |
- | |
239 | * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type. |
- | |
240 | */ |
- | |
241 | uint16_t getSimplePressure(int advalue) { |
- | |
242 | return advalue; |
- | |
243 | } |
- | |
244 | 236 | ||
245 | void startAnalogConversionCycle(void) { |
237 | void startAnalogConversionCycle(void) { |
Line 246... | Line 238... | ||
246 | analogDataReady = 0; |
238 | analogDataReady = 0; |
247 | 239 | ||
Line 357... | Line 349... | ||
357 | if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
349 | if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
358 | gyroDWindowIdx = 0; |
350 | gyroDWindowIdx = 0; |
359 | } |
351 | } |
360 | } |
352 | } |
Line -... | Line 353... | ||
- | 353 | ||
- | 354 | // probably wanna aim at 1/10 m/s/unit. |
|
- | 355 | #define LOG_AIRSPEED_FACTOR 2 |
|
361 | 356 | ||
362 | void analog_updateAirPressure(void) { |
357 | void analog_updateAirspeed(void) { |
- | 358 | uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
|
- | 359 | int16_t temp = rawAirPressure - airpressureOffset; |
|
363 | uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
360 | if (temp<0) temp = 0; |
364 | simpleAirPressure = rawAirPressure; |
361 | simpleAirPressure = temp; |
365 | airspeedVelocity = isqrt16(simpleAirPressure); |
362 | airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(simpleAirPressure)) >> LOG_AIRSPEED_FACTOR; |
Line 366... | Line 363... | ||
366 | } |
363 | } |
367 | 364 | ||
368 | void analog_updateBatteryVoltage(void) { |
365 | void analog_updateBatteryVoltage(void) { |
Line 403... | Line 400... | ||
403 | delay_ms_with_adc_measurement(100, 0); |
400 | delay_ms_with_adc_measurement(100, 0); |
Line 404... | Line 401... | ||
404 | 401 | ||
405 | // gyroActivity = 0; |
402 | // gyroActivity = 0; |
Line 406... | Line 403... | ||
406 | } |
403 | } |
407 | 404 | ||
408 | void analog_calibrateGyros(void) { |
405 | void analog_calibrate(void) { |
409 | #define GYRO_OFFSET_CYCLES 64 |
406 | #define OFFSET_CYCLES 64 |
410 | uint8_t i, axis; |
407 | uint8_t i, axis; |
Line 411... | Line 408... | ||
411 | int32_t offsets[3] = { 0, 0, 0 }; |
408 | int32_t offsets[4] = { 0, 0, 0, 0}; |
412 | gyro_calibrate(); |
409 | gyro_calibrate(); |
413 | 410 | ||
414 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
411 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
415 | for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
412 | for (i = 0; i < OFFSET_CYCLES; i++) { |
416 | delay_ms_with_adc_measurement(10, 1); |
413 | delay_ms_with_adc_measurement(10, 1); |
- | 414 | for (axis = PITCH; axis <= YAW; axis++) { |
|
417 | for (axis = PITCH; axis <= YAW; axis++) { |
415 | offsets[axis] += rawGyroValue(axis); |
Line 418... | Line 416... | ||
418 | offsets[axis] += rawGyroValue(axis); |
416 | } |
419 | } |
417 | offsets[3] += sensorInputs[AD_AIRPRESSURE]; |
420 | } |
- | |
421 | 418 | } |
|
422 | for (axis = PITCH; axis <= YAW; axis++) { |
419 | |
423 | gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
420 | for (axis = PITCH; axis <= YAW; axis++) { |
424 | 421 | gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
|
425 | int16_t min = (512-200) * GYRO_OVERSAMPLING; |
422 | int16_t min = (512-200) * GYRO_OVERSAMPLING; |
Line -... | Line 423... | ||
- | 423 | int16_t max = (512+200) * GYRO_OVERSAMPLING; |
|
- | 424 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
|
- | 425 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
|
- | 426 | } |
|
- | 427 | ||
- | 428 | airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
|
426 | int16_t max = (512+200) * GYRO_OVERSAMPLING; |
429 | int16_t min = 200; |
- | 430 | int16_t max = (1024-200) * 2; |
|
427 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
431 | if(airpressure < min || airpressure > max) |
428 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
432 | versionInfo.hardwareErrors[0] |= FC_ERROR0_PRESSURE; |