Rev 1645 | Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | #include "invenSense.h" |
2 | #include "timer0.h" |
||
3 | #include "configuration.h" |
||
4 | |||
5 | #include <avr/io.h> |
||
6 | |||
7 | #define AUTOZERO_PORT PORTD |
||
8 | #define AUTOZERO_DDR DDRD |
||
9 | #define AUTOZERO_BIT 5 |
||
10 | |||
11 | void gyro_calibrate() { |
||
12 | // If port not already set to output and high, do it. |
||
13 | if (!(AUTOZERO_DDR & (1<<AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1<<AUTOZERO_BIT))) { |
||
14 | AUTOZERO_PORT |= (1<<AUTOZERO_BIT); |
||
15 | AUTOZERO_DDR |= (1<<AUTOZERO_BIT); |
||
16 | Delay_ms(100); |
||
17 | } |
||
18 | |||
19 | // Make a pulse on the auto-zero output line. |
||
20 | AUTOZERO_PORT &= ~(1<<AUTOZERO_BIT); |
||
21 | Delay_ms(1); |
||
22 | AUTOZERO_PORT |= (1<<AUTOZERO_BIT); |
||
23 | // Delay_ms(10); |
||
24 | Delay_ms_Mess(100); |
||
25 | } |
||
26 | |||
27 | void gyro_setDefaults(void) { |
||
28 | staticParams.GyroD = 3; |
||
29 | staticParams.GyroAccFactor = 1; |
||
30 | staticParams.DriftComp = 1; |
||
31 | |||
32 | // Not used. |
||
33 | staticParams.AngleTurnOverPitch = 85; |
||
34 | staticParams.AngleTurnOverRoll = 85; |
||
35 | } |