0,0 → 1,47 |
#include <stdlib.h> |
#include <avr/io.h> |
#include <stdio.h> |
|
#include "attitude.h" |
#include "configuration.h" |
#include <avr/interrupt.h> |
#include "timer0.h" |
#include "debug.h" |
|
// where our main data flow comes from. |
#include "analog.h" |
#include "AP_AHRS_DCM.h" |
#include "AP_GPS.h" |
|
int16_t attitude[3]; |
|
//AP_Compass_HIL compass; |
GPS* gps; |
AP_AHRS_DCM ahrs(gps); |
|
uint8_t imu_sequence = 0; //incremented on each call to imu_update |
|
/************************************************************************ |
* Neutral Readings |
************************************************************************/ |
void attitude_setNeutral(void) { |
analog_setNeutral(); |
ahrs.reset(); |
} |
|
void attitude_update(void) { |
static uint32_t timestampJiffies; |
if (analog_attitudeDataStatus == ATTITUDE_SENSOR_DATA_READY) { |
J3TOGGLE; |
// OOPS: The attitude data might get added to while this is running... |
// debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
analog_updateAttitudeData(); |
cli(); |
float jiffies = jiffiesClock - timestampJiffies; |
timestampJiffies = jiffiesClock; |
sei(); |
ahrs.update(attitude, jiffies * T_TIMER0IRQ); |
} else { |
// debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
} |
} |