Rev 1077 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1077 | Rev 1078 | ||
|---|---|---|---|
| Line 24... | Line 24... | ||
| 24 | volatile int16_t AdValueAccRoll = 0, AdValueAccNick = 0, AdValueAccTop = 0; |
24 | volatile int16_t AdValueAccRoll = 0, AdValueAccNick = 0, AdValueAccTop = 0; |
| 25 | volatile int32_t AirPressure = 32000; |
25 | volatile int32_t AirPressure = 32000; |
| 26 | volatile uint8_t average_pressure = 0; |
26 | volatile uint8_t average_pressure = 0; |
| 27 | volatile int16_t StartAirPressure; |
27 | volatile int16_t StartAirPressure; |
| 28 | volatile uint16_t ReadingAirPressure = 1023; |
28 | volatile uint16_t ReadingAirPressure = 1023; |
| - | 29 | int8_t ExpandBaro = 0; |
|
| 29 | uint8_t PressureSensorOffset; |
30 | uint8_t PressureSensorOffset; |
| 30 | volatile int16_t HeightD = 0; |
31 | volatile int16_t HeightD = 0; |
| 31 | volatile uint16_t MeasurementCounter = 0; |
32 | volatile uint16_t MeasurementCounter = 0; |
| Line 32... | Line 33... | ||
| 32 | 33 | ||
| Line 64... | Line 65... | ||
| 64 | { |
65 | { |
| 65 | uint8_t off; |
66 | uint8_t off; |
| 66 | off = GetParamByte(PID_PRESSURE_OFFSET); |
67 | off = GetParamByte(PID_PRESSURE_OFFSET); |
| 67 | if(off > 20) off -= 10; |
68 | if(off > 20) off -= 10; |
| 68 | OCR0A = off; |
69 | OCR0A = off; |
| - | 70 | ExpandBaro = 0; |
|
| 69 | Delay_ms_Mess(100); |
71 | Delay_ms_Mess(100); |
| 70 | if(ReadingAirPressure < 850) off = 0; |
72 | if(ReadingAirPressure < 850) off = 0; |
| 71 | for(; off < 250;off++) |
73 | for(; off < 250;off++) |
| 72 | { |
74 | { |
| 73 | OCR0A = off; |
75 | OCR0A = off; |
| 74 | Delay_ms_Mess(50); |
76 | Delay_ms_Mess(50); |
| 75 | printf("."); |
77 | printf("."); |
| 76 | if(ReadingAirPressure < 900) break; |
78 | if(ReadingAirPressure < 850) break; |
| 77 | } |
79 | } |
| 78 | SetParamByte(PID_PRESSURE_OFFSET, off); |
80 | SetParamByte(PID_PRESSURE_OFFSET, off); |
| 79 | PressureSensorOffset = off; |
81 | PressureSensorOffset = off; |
| 80 | Delay_ms_Mess(300); |
82 | Delay_ms_Mess(300); |
| 81 | } |
83 | } |
| Line 167... | Line 169... | ||
| 167 | adc_channel = 0; // set next channel to ADC7 = YAW GYRO |
169 | adc_channel = 0; // set next channel to ADC7 = YAW GYRO |
| 168 | break; |
170 | break; |
| 169 | case 6: |
171 | case 6: |
| 170 | // average over two samples to create current AdValueGyrYaw |
172 | // average over two samples to create current AdValueGyrYaw |
| 171 | if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2; |
173 | if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2; |
| - | 174 | else if (BoardRelease == 20) AdValueGyrYaw = 1023 - (ADC + yaw1); |
|
| 172 | else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1 |
175 | else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1 |
| 173 | adc_channel = 1; // set next channel to ADC7 = ROLL GYRO |
176 | adc_channel = 1; // set next channel to ADC7 = ROLL GYRO |
| 174 | break; |
177 | break; |
| 175 | case 7: |
178 | case 7: |
| 176 | // average over two samples to create current ADValueGyrRoll |
179 | // average over two samples to create current ADValueGyrRoll |
| Line 212... | Line 215... | ||
| 212 | case 10: |
215 | case 10: |
| 213 | tmpAirPressure += ADC; // sum vadc values |
216 | tmpAirPressure += ADC; // sum vadc values |
| 214 | if(++average_pressure >= 5) // if 5 values are summerized for averaging |
217 | if(++average_pressure >= 5) // if 5 values are summerized for averaging |
| 215 | { |
218 | { |
| 216 | ReadingAirPressure = ADC; // update measured air pressure |
219 | ReadingAirPressure = ADC; // update measured air pressure |
| 217 | HeightD = (7 * HeightD + (int16_t)FCParam.Height_D * (int16_t)(StartAirPressure - tmpAirPressure - ReadingHeight))/8; // D-Part = CurrentValue - OldValue |
220 | HeightD = (7 * HeightD + (int16_t)FCParam.Height_D * (int16_t)(255 * ExpandBaro + StartAirPressure - tmpAirPressure - ReadingHeight))/8; // D-Part = CurrentValue - OldValue |
| 218 | AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history |
221 | AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history |
| 219 | ReadingHeight = StartAirPressure - AirPressure; |
222 | ReadingHeight = 255 * ExpandBaro + StartAirPressure - AirPressure; |
| 220 | average_pressure = 0; // reset air pressure measurement counter |
223 | average_pressure = 0; // reset air pressure measurement counter |
| 221 | tmpAirPressure = 0; |
224 | tmpAirPressure = 0; |
| 222 | } |
225 | } |
| 223 | adc_channel = 0; // set next channel to ADC0 = GIER GYRO |
226 | adc_channel = 0; // set next channel to ADC0 = GIER GYRO |
| 224 | state = 0; // reset state machine |
227 | state = 0; // reset state machine |