Rev 2102 | Rev 2104 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2102 | Rev 2103 | ||
---|---|---|---|
Line 41... | Line 41... | ||
41 | * integration to angles. |
41 | * integration to angles. |
42 | */ |
42 | */ |
43 | int16_t gyro_PID[3]; |
43 | int16_t gyro_PID[3]; |
44 | int16_t gyro_ATT[3]; |
44 | int16_t gyro_ATT[3]; |
45 | int16_t gyroD[3]; |
45 | int16_t gyroD[3]; |
46 | int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH]; |
46 | int16_t gyroDWindow[3][GYRO_D_WINDOW_LENGTH]; |
47 | uint8_t gyroDWindowIdx = 0; |
47 | uint8_t gyroDWindowIdx = 0; |
48 | //int16_t dHeight; |
48 | //int16_t dHeight; |
49 | //uint32_t gyroActivity; |
49 | //uint32_t gyroActivity; |
Line 50... | Line 50... | ||
50 | 50 | ||
Line 117... | Line 117... | ||
117 | * Airspeed |
117 | * Airspeed |
118 | */ |
118 | */ |
119 | uint16_t simpleAirPressure; |
119 | uint16_t simpleAirPressure; |
120 | uint16_t airspeedVelocity; |
120 | uint16_t airspeedVelocity; |
Line 121... | Line -... | ||
121 | - | ||
122 | // Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
- | |
123 | // int32_t filteredAirPressure; |
- | |
124 | - | ||
125 | #define MAX_AIRPRESSURE_WINDOW_LENGTH 32 |
- | |
126 | int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH]; |
- | |
127 | int32_t windowedAirPressure; |
- | |
128 | uint8_t windowPtr = 0; |
- | |
129 | - | ||
130 | // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
- | |
131 | int32_t airPressureSum; |
- | |
132 | - | ||
133 | // The number of samples summed into airPressureSum so far. |
- | |
134 | uint8_t pressureMeasurementCount; |
- | |
135 | - | ||
136 | 121 | ||
137 | /* |
122 | /* |
138 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
123 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
139 | * That is divided by 3 below, for a final 10.34 per volt. |
124 | * That is divided by 3 below, for a final 10.34 per volt. |
140 | * So the initial value of 100 is for 9.7 volts. |
125 | * So the initial value of 100 is for 9.7 volts. |
Line 223... | Line 208... | ||
223 | ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); |
208 | ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); |
224 | //Set ADC Control and Status Register B |
209 | //Set ADC Control and Status Register B |
225 | //Trigger Source to Free Running Mode |
210 | //Trigger Source to Free Running Mode |
226 | ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0)); |
211 | ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0)); |
Line 227... | Line -... | ||
227 | - | ||
228 | for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) { |
- | |
229 | airPressureWindow[i] = 0; |
- | |
230 | } |
- | |
231 | windowedAirPressure = 0; |
- | |
232 | 212 | ||
Line 233... | Line 213... | ||
233 | startAnalogConversionCycle(); |
213 | startAnalogConversionCycle(); |
234 | 214 | ||
235 | // restore global interrupt flags |
215 | // restore global interrupt flags |
Line 320... | Line 300... | ||
320 | void analog_updateGyros(void) { |
300 | void analog_updateGyros(void) { |
321 | // for various filters... |
301 | // for various filters... |
322 | int16_t tempOffsetGyro[3], tempGyro; |
302 | int16_t tempOffsetGyro[3], tempGyro; |
Line 323... | Line 303... | ||
323 | 303 | ||
- | 304 | debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
|
324 | debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
305 | |
325 | for (uint8_t axis=0; axis<3; axis++) { |
306 | for (uint8_t axis=0; axis<3; axis++) { |
326 | tempGyro = rawGyroValue(axis); |
307 | tempGyro = rawGyroValue(axis); |
327 | /* |
308 | /* |
328 | * Process the gyro data for the PID controller. |
309 | * Process the gyro data for the PID controller. |
Line 374... | Line 355... | ||
374 | rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW); |
355 | rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW); |
Line 375... | Line 356... | ||
375 | 356 | ||
376 | // dampenGyroActivity(); |
357 | // dampenGyroActivity(); |
377 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
358 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
- | 359 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
|
Line 378... | Line 360... | ||
378 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
360 | gyro_ATT[YAW] = tempOffsetGyro[YAW]; |
379 | 361 | ||
380 | if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
362 | if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
381 | gyroDWindowIdx = 0; |
363 | gyroDWindowIdx = 0; |
Line 400... | Line 382... | ||
400 | analog_updateAirPressure(); |
382 | analog_updateAirPressure(); |
401 | analog_updateBatteryVoltage(); |
383 | analog_updateBatteryVoltage(); |
402 | #ifdef USE_MK3MAG |
384 | #ifdef USE_MK3MAG |
403 | magneticHeading = volatileMagneticHeading; |
385 | magneticHeading = volatileMagneticHeading; |
404 | #endif |
386 | #endif |
405 | - | ||
406 | } |
387 | } |
Line 407... | Line 388... | ||
407 | 388 | ||
408 | void analog_setNeutral() { |
389 | void analog_setNeutral() { |
Line 428... | Line 409... | ||
428 | 409 | ||
429 | // gyroActivity = 0; |
410 | // gyroActivity = 0; |
Line 430... | Line 411... | ||
430 | } |
411 | } |
431 | 412 | ||
432 | void analog_calibrateGyros(void) { |
413 | void analog_calibrateGyros(void) { |
433 | #define GYRO_OFFSET_CYCLES 32 |
414 | #define GYRO_OFFSET_CYCLES 64 |
434 | uint8_t i, axis; |
415 | uint8_t i, axis; |
Line 435... | Line 416... | ||
435 | int32_t offsets[3] = { 0, 0, 0 }; |
416 | int32_t offsets[3] = { 0, 0, 0 }; |