44,8 → 44,8 |
*/ |
uint32_t airpressure; |
uint16_t airspeedVelocity = 0; |
int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH]; |
uint8_t airpressureWindowIdx = 0; |
//int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH]; |
//uint8_t airpressureWindowIdx = 0; |
|
/* |
* Offset values. These are the raw gyro and acc. meter sums when the copter is |
212,8 → 212,6 |
} |
|
void startAnalogConversionCycle(void) { |
sensorDataReady = 0; |
|
// Stop the sampling. Cycle is over. |
for (uint8_t i = 0; i<8; i++) { |
ADSensorInputs[i] = 0; |
224,6 → 222,7 |
ADMUX = (ADMUX & 0xE0) | adChannel; |
startADC(); |
twimaster_startCycle(); |
sensorDataReady = 0; |
} |
|
/***************************************************** |
308,17 → 307,25 |
void analog_updateAirspeed(void) { |
uint16_t rawAirpressure = ADSensorInputs[AD_AIRPRESSURE]; |
int16_t temp = rawAirpressure - airpressureOffset; |
airpressure -= airpressureWindow[airpressureWindowIdx]; |
airpressure += temp; |
airpressureWindow[airpressureWindowIdx] = temp; |
airpressureWindowIdx++; |
if (airpressureWindowIdx == AIRPRESSURE_WINDOW_LENGTH) { |
airpressureWindowIdx = 0; |
} |
// airpressure -= airpressureWindow[airpressureWindowIdx]; |
// airpressure += temp; |
// airpressureWindow[airpressureWindowIdx] = temp; |
// airpressureWindowIdx++; |
// if (airpressureWindowIdx == AIRPRESSURE_WINDOW_LENGTH) { |
// airpressureWindowIdx = 0; |
// } |
|
if (temp<0) temp = 0; |
airspeedVelocity = (staticParams.airspeedCorrection * isqrt32(airpressure)) >> LOG_AIRSPEED_FACTOR; |
isFlying = (airspeedVelocity >= staticParams.isFlyingThreshold); |
#define AIRPRESSURE_FILTER 16 |
airpressure = ((int32_t)airpressure * (AIRPRESSURE_FILTER-1) + (AIRPRESSURE_FILTER/2) + temp) / AIRPRESSURE_FILTER; |
|
uint16_t p2 = (airpressure<0) ? 0 : airpressure; |
airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(p2)) >> LOG_AIRSPEED_FACTOR; |
|
debugOut.analog[17] = airpressure; |
debugOut.analog[18] = airpressureOffset; |
debugOut.analog[19] = airspeedVelocity; |
|
isFlying = 0; //(airspeedVelocity >= staticParams.isFlyingThreshold); |
} |
|
void analog_updateBatteryVoltage(void) { |
355,10 → 362,6 |
} |
} |
|
for (uint8_t i=0; i<AIRPRESSURE_WINDOW_LENGTH; i++) { |
airpressureWindow[i] = 0; |
} |
|
// Setting offset values has an influence in the analog.c ISR |
// Therefore run measurement for 100ms to achive stable readings |
delay_ms_with_adc_measurement(100, 0); |
367,7 → 370,7 |
} |
|
void analog_calibrate(void) { |
#define OFFSET_CYCLES 64 |
#define OFFSET_CYCLES 32 |
uint8_t i, axis; |
int32_t offsets[4] = { 0, 0, 0, 0}; |
|
382,15 → 385,12 |
|
for (axis = PITCH; axis <= YAW; axis++) { |
gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
int16_t min = (512-200) * GYRO_OVERSAMPLING; |
int16_t max = (512+200) * GYRO_OVERSAMPLING; |
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
} |
|
airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
int16_t min = 200; |
int16_t max = (1024-200) * 2; |
int16_t max = 1024-200; |
|
if(airpressureOffset < min || airpressureOffset > max) |
versionInfo.hardwareErrors[0] |= FC_ERROR0_PRESSURE; |
|