14,12 → 14,16 |
#include "fc.h" |
#include "printf_P.h" |
#include "eeprom.h" |
#include "twimaster.h" |
|
volatile int16_t Current_AccZ = 0; |
volatile int16_t UBat = 100; |
volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0; |
uint8_t AnalogOffsetNick = 115, AnalogOffsetRoll = 115, AnalogOffsetYaw = 115; |
uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
volatile int16_t AdValueAccRoll = 0, AdValueAccNick = 0, AdValueAccTop = 0; |
volatile int32_t AirPressure = 32000; |
volatile uint8_t average_pressure = 0; |
volatile int16_t StartAirPressure; |
volatile uint16_t ReadingAirPressure = 1023; |
uint8_t PressureSensorOffset; |
77,6 → 81,35 |
} |
|
|
void SearchGyroOffset(void) |
{ |
uint8_t i, ready = 0; |
|
GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
for(i = 140; i != 0; i--) |
{ |
if(ready == 3 && i > 10) i = 9; |
ready = 0; |
if(AdValueGyrNick < 1020) AnalogOffsetNick--; else if(AdValueGyrNick > 1030) AnalogOffsetNick++; else ready++; |
if(AdValueGyrRoll < 1020) AnalogOffsetRoll--; else if(AdValueGyrRoll > 1030) AnalogOffsetRoll++; else ready++; |
if(AdValueGyrYaw < 1020) AnalogOffsetYaw-- ; else if(AdValueGyrYaw > 1030) AnalogOffsetYaw++ ; else ready++; |
twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset |
I2C_Start(); // initiate data transmission |
if(AnalogOffsetNick < 10) { GyroDefectNick = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefectNick = 1; AnalogOffsetNick = 245;}; |
if(AnalogOffsetRoll < 10) { GyroDefectRoll = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefectRoll = 1; AnalogOffsetRoll = 245;}; |
if(AnalogOffsetYaw < 10) { GyroDefectYaw = 1; AnalogOffsetYaw = 10;}; if(AnalogOffsetYaw > 245) { GyroDefectYaw = 1; AnalogOffsetYaw = 245;}; |
while(twi_state); // wait for end of data transmission |
average_pressure = 0; |
ADC_Enable(); |
while(average_pressure == 0); |
if(i < 10) Delay_ms_Mess(10); |
} |
Delay_ms_Mess(70); |
} |
|
|
|
|
/*****************************************************/ |
/* Interrupt Service Routine for ADC */ |
/*****************************************************/ |
101,7 → 134,6 |
{ |
static uint8_t adc_channel = 0, state = 0; |
static uint16_t yaw1, roll1, nick1; |
static uint8_t average_pressure = 0; |
static int16_t tmpAirPressure = 0; |
// disable further AD conversion |
ADC_Disable(); |