Rev 1927 | Rev 2096 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1927 | Rev 2022 | ||
---|---|---|---|
Line 1... | Line -... | ||
1 | #include <avr/io.h> |
- | |
2 | #include <avr/interrupt.h> |
1 | #include <avr/interrupt.h> |
3 | #include <avr/pgmspace.h> |
2 | #include <avr/pgmspace.h> |
Line 4... | Line 3... | ||
4 | 3 | ||
5 | #include "analog.h" |
4 | #include "analog.h" |
Line 50... | Line 49... | ||
50 | 49 | ||
51 | volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 |
50 | volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 |
Line 52... | Line 51... | ||
52 | * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
51 | * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
53 | - | ||
54 | /* |
- | |
55 | * This allows some experimentation with the gyro filters. |
- | |
56 | * Should be replaced by #define's later on... |
- | |
57 | */ |
- | |
58 | volatile uint8_t GYROS_PID_FILTER; |
- | |
59 | volatile uint8_t GYROS_ATT_FILTER; |
- | |
60 | volatile uint8_t GYROS_D_FILTER; |
- | |
61 | volatile uint8_t ACC_FILTER; |
- | |
62 | 52 | ||
63 | /* |
53 | /* |
64 | * Air pressure |
54 | * Air pressure |
Line 65... | Line 55... | ||
65 | */ |
55 | */ |
Line 187... | Line 177... | ||
187 | */ |
177 | */ |
188 | uint16_t getSimplePressure(int advalue) { |
178 | uint16_t getSimplePressure(int advalue) { |
189 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
179 | return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
190 | } |
180 | } |
Line -... | Line 181... | ||
- | 181 | ||
- | 182 | /* |
|
- | 183 | * In the MK coordinate system, nose-down is positive and left-roll is positive. |
|
- | 184 | * If a sensor is used in an orientation where one but not both of the axes has |
|
- | 185 | * an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true). |
|
191 | 186 | */ |
|
192 | void transformPRGyro(int16_t* result) { |
187 | void transformPRGyro(int16_t* result) { |
- | 188 | static const uint8_t tab[] = {1,1,0,0-1,-1,-1,0,1}; |
|
193 | static const uint8_t tab[] = {1,1,0,0-1,-1,-1,0,1}; |
189 | // Pitch to Pitch part |
- | 190 | int8_t pp = PR_GYROS_ORIENTATION_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT]; |
|
194 | int8_t pp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT]; |
191 | // Pitch to Roll part |
- | 192 | int8_t pr = tab[(GYRO_QUADRANT+2)%8]; |
|
195 | int8_t pr = tab[(GYRO_QUADRANT+2)%8]; |
193 | // Roll to Roll part |
- | 194 | int8_t rp = PR_GYROS_ORIENTATION_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8]; |
|
196 | int8_t rp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8]; |
195 | // Roll to Roll part |
Line 197... | Line 196... | ||
197 | int8_t rr = tab[GYRO_QUADRANT]; |
196 | int8_t rr = tab[GYRO_QUADRANT]; |
- | 197 | ||
198 | 198 | int16_t pitchIn = result[PITCH]; |
|
199 | int16_t temp = result[0]; |
199 | |
200 | result[0] = pp*result[0] + pr*result[1]; |
200 | result[PITCH] = pp*result[PITCH] + pr*result[ROLL]; |
Line 201... | Line 201... | ||
201 | result[1] = rp*temp + rr*result[1]; |
201 | result[ROLL] = rp*pitchIn + rr*result[ROLL]; |
202 | } |
202 | } |
203 | 203 | ||
204 | /***************************************************** |
204 | /***************************************************** |
205 | * Interrupt Service Routine for ADC |
205 | * Interrupt Service Routine for ADC |
206 | * Runs at 312.5 kHz or 3.2 µs. When all states are |
206 | * Runs at 312.5 kHz or 3.2 us. When all states are |
207 | * processed the interrupt is disabled and further |
207 | * processed the interrupt is disabled and further |
208 | * AD conversions are stopped. |
208 | * AD conversions are stopped. |
Line 239... | Line 239... | ||
239 | 239 | ||
Line 240... | Line 240... | ||
240 | break; |
240 | break; |
241 | 241 | ||
242 | case 11: // yaw gyro |
242 | case 11: // yaw gyro |
243 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
243 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
244 | if (YAW_REVERSED) |
244 | if (YAW_GYRO_REVERSED) |
245 | tempOffsetGyro[0] = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
245 | tempOffsetGyro[0] = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
246 | else |
246 | else |
247 | tempOffsetGyro[0] = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
247 | tempOffsetGyro[0] = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
248 | gyroD[YAW] = (gyroD[YAW] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[0] - yawGyro)) / GYROS_D_FILTER; |
248 | gyroD[YAW] = (gyroD[YAW] * (staticParams.DGyroFilter - 1) + (tempOffsetGyro[0] - yawGyro)) / staticParams.DGyroFilter; |
249 | yawGyro = tempOffsetGyro[0]; |
249 | yawGyro = tempOffsetGyro[0]; |
250 | break; |
- | |
251 | case 13: // roll axis acc. |
- | |
252 | /* |
- | |
253 | for (axis=0; axis<2; axis++) { |
- | |
254 | if (ACC_REVERSED[axis]) |
- | |
255 | tempSensor[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
- | |
256 | else |
- | |
257 | tempSensor[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis]; |
- | |
258 | } |
- | |
259 | if (AXIS_TRANSFORM) { |
- | |
260 | acc[PITCH] = tempSensor[PITCH] + tempSensor[ROLL]; |
- | |
261 | acc[ROLL] = tempSensor[ROLL] - tempSensor[PITCH]; |
- | |
262 | } else { |
- | |
263 | acc[PITCH] = tempSensor[PITCH]; |
- | |
264 | acc[ROLL] = tempSensor[ROLL]; |
- | |
Line 265... | Line 250... | ||
265 | } |
250 | break; |
266 | */ |
251 | case 13: // roll axis acc. |
Line 267... | Line 252... | ||
267 | 252 | ||
268 | // We have no sensor installed... |
253 | // We have no sensor installed... |
269 | acc[PITCH] = acc[ROLL] = 0; |
254 | acc[PITCH] = acc[ROLL] = 0; |
270 | 255 | ||
271 | for (axis=0; axis<2; axis++) { |
256 | for (axis=0; axis<2; axis++) { |
272 | filteredAcc[axis] = |
257 | filteredAcc[axis] = |
Line 273... | Line 258... | ||
273 | (filteredAcc[axis] * (ACC_FILTER - 1) + acc[axis]) / ACC_FILTER; |
258 | (filteredAcc[axis] * (staticParams.accFilter - 1) + acc[axis]) / staticParams.accFilter; |
Line 377... | Line 362... | ||
377 | // 2.1: Transform axis if configured to. |
362 | // 2.1: Transform axis if configured to. |
378 | transformPRGyro(tempOffsetGyro); |
363 | transformPRGyro(tempOffsetGyro); |
Line 379... | Line 364... | ||
379 | 364 | ||
380 | // 3) Scale and filter. |
365 | // 3) Scale and filter. |
381 | for (axis=0; axis<2; axis++) { |
366 | for (axis=0; axis<2; axis++) { |
Line 382... | Line 367... | ||
382 | tempOffsetGyro[axis] = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro[axis]) / GYROS_PID_FILTER; |
367 | tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.PIDGyroFilter - 1) + tempOffsetGyro[axis]) / staticParams.PIDGyroFilter; |
383 | 368 | ||
Line 384... | Line 369... | ||
384 | // 4) Measure noise. |
369 | // 4) Measure noise. |
385 | measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
370 | measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
Line 386... | Line 371... | ||
386 | 371 | ||
387 | // 5) Differential measurement. |
372 | // 5) Differential measurement. |
388 | gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / GYROS_D_FILTER; |
373 | gyroD[axis] = (gyroD[axis] * (staticParams.DGyroFilter - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.DGyroFilter; |
Line 399... | Line 384... | ||
399 | } |
384 | } |
Line 400... | Line 385... | ||
400 | 385 | ||
Line 401... | Line 386... | ||
401 | transformPRGyro(tempOffsetGyro); |
386 | transformPRGyro(tempOffsetGyro); |
402 | 387 | ||
403 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
388 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
404 | gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[PITCH]) / GYROS_ATT_FILTER; |
389 | gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter; |
Line 405... | Line 390... | ||
405 | gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[ROLL]) / GYROS_ATT_FILTER; |
390 | gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |
406 | break; |
391 | break; |
407 | 392 | ||
Line 436... | Line 421... | ||
436 | void analog_calibrate(void) { |
421 | void analog_calibrate(void) { |
437 | #define GYRO_OFFSET_CYCLES 32 |
422 | #define GYRO_OFFSET_CYCLES 32 |
438 | uint8_t i, axis; |
423 | uint8_t i, axis; |
439 | int32_t deltaOffsets[3] = { 0, 0, 0 }; |
424 | int32_t deltaOffsets[3] = { 0, 0, 0 }; |
Line 440... | Line -... | ||
440 | - | ||
441 | // Set the filters... to be removed again, once some good settings are found. |
- | |
442 | GYROS_PID_FILTER = (staticParams.sensorFilterSettings & (0x7 & (1<<0))) + 1; |
- | |
443 | GYROS_ATT_FILTER = 1; |
- | |
444 | GYROS_D_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<4))) + 1; |
- | |
445 | ACC_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<6))) + 1; |
- | |
446 | 425 | ||
Line 447... | Line 426... | ||
447 | gyro_calibrate(); |
426 | gyro_calibrate(); |
448 | 427 | ||
449 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
428 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |