51,6 → 51,7 |
* to be centered on zero. |
*/ |
sensorOffset_t gyroOffset; |
int16_t airpressureOffset; |
|
/* |
* In the MK coordinate system, nose-down is positive and left-roll is positive. |
111,7 → 112,6 |
* Airspeed |
*/ |
uint16_t simpleAirPressure; |
uint16_t airspeedVelocity; |
|
/* |
* Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
234,14 → 234,6 |
} |
} |
|
/* |
* Min.: 0 |
* Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type. |
*/ |
uint16_t getSimplePressure(int advalue) { |
return advalue; |
} |
|
void startAnalogConversionCycle(void) { |
analogDataReady = 0; |
|
359,10 → 351,15 |
} |
} |
|
void analog_updateAirPressure(void) { |
// probably wanna aim at 1/10 m/s/unit. |
#define LOG_AIRSPEED_FACTOR 2 |
|
void analog_updateAirspeed(void) { |
uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
simpleAirPressure = rawAirPressure; |
airspeedVelocity = isqrt16(simpleAirPressure); |
int16_t temp = rawAirPressure - airpressureOffset; |
if (temp<0) temp = 0; |
simpleAirPressure = temp; |
airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(simpleAirPressure)) >> LOG_AIRSPEED_FACTOR; |
} |
|
void analog_updateBatteryVoltage(void) { |
405,23 → 402,23 |
// gyroActivity = 0; |
} |
|
void analog_calibrateGyros(void) { |
#define GYRO_OFFSET_CYCLES 64 |
void analog_calibrate(void) { |
#define OFFSET_CYCLES 64 |
uint8_t i, axis; |
int32_t offsets[3] = { 0, 0, 0 }; |
int32_t offsets[4] = { 0, 0, 0, 0}; |
gyro_calibrate(); |
|
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
for (i = 0; i < OFFSET_CYCLES; i++) { |
delay_ms_with_adc_measurement(10, 1); |
for (axis = PITCH; axis <= YAW; axis++) { |
offsets[axis] += rawGyroValue(axis); |
} |
offsets[3] += sensorInputs[AD_AIRPRESSURE]; |
} |
|
for (axis = PITCH; axis <= YAW; axis++) { |
gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_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) |
428,6 → 425,13 |
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
} |
|
gyroOffset_writeToEEProm(); |
airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
int16_t min = 200; |
int16_t max = (1024-200) * 2; |
if(airpressure < min || airpressure > max) |
versionInfo.hardwareErrors[0] |= FC_ERROR0_PRESSURE; |
|
gyroOffset_writeToEEProm(); |
|
startAnalogConversionCycle(); |
} |