Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2189 | - | 1 | #include <stdlib.h> |
| 2 | #include <avr/io.h> |
||
| 3 | #include <stdio.h> |
||
| 4 | |||
| 5 | #include "attitude.h" |
||
| 6 | #include "configuration.h" |
||
| 7 | #include <avr/interrupt.h> |
||
| 8 | #include "timer0.h" |
||
| 9 | #include "debug.h" |
||
| 10 | |||
| 11 | // where our main data flow comes from. |
||
| 12 | #include "analog.h" |
||
| 13 | #include "AP_AHRS_DCM.h" |
||
| 14 | #include "AP_GPS.h" |
||
| 15 | |||
| 16 | int16_t attitude[3]; |
||
| 17 | |||
| 18 | //AP_Compass_HIL compass; |
||
| 19 | GPS* gps; |
||
| 20 | AP_AHRS_DCM ahrs(gps); |
||
| 21 | |||
| 22 | uint8_t imu_sequence = 0; //incremented on each call to imu_update |
||
| 23 | |||
| 24 | /************************************************************************ |
||
| 25 | * Neutral Readings |
||
| 26 | ************************************************************************/ |
||
| 27 | void attitude_setNeutral(void) { |
||
| 28 | analog_setNeutral(); |
||
| 29 | ahrs.reset(); |
||
| 30 | } |
||
| 31 | |||
| 32 | void attitude_update(void) { |
||
| 33 | static uint32_t timestampJiffies; |
||
| 34 | if (analog_attitudeDataStatus == ATTITUDE_SENSOR_DATA_READY) { |
||
| 35 | J3TOGGLE; |
||
| 36 | // OOPS: The attitude data might get added to while this is running... |
||
| 37 | // debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
||
| 38 | analog_updateAttitudeData(); |
||
| 39 | cli(); |
||
| 40 | float jiffies = jiffiesClock - timestampJiffies; |
||
| 41 | timestampJiffies = jiffiesClock; |
||
| 42 | sei(); |
||
| 43 | ahrs.update(attitude, jiffies * T_TIMER0IRQ); |
||
| 44 | } else { |
||
| 45 | // debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
||
| 46 | } |
||
| 47 | } |