Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2108 → Rev 2109

/branches/dongfang_FC_fixedwing/arduino_atmega328/analog.c
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;