Subversion Repositories FlightCtrl

Rev

Go to most recent revision | 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
}