| 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(); |