Rev 2092 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2092 | Rev 2112 | ||
---|---|---|---|
1 | #include "invenSense.h" |
- | |
2 | #include "timer0.h" |
1 | #include "timer0.h" |
3 | #include "configuration.h" |
2 | #include "configuration.h" |
4 | 3 | ||
5 | #include <avr/io.h> |
4 | #include <avr/io.h> |
6 | 5 | ||
7 | /* |
6 | /* |
8 | * Configuration for my prototype board with InvenSense gyros. |
7 | * Configuration for my prototype board with InvenSense gyros. |
9 | * The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll. |
8 | * The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll. |
10 | */ |
9 | */ |
11 | 10 | ||
12 | // The special auto-zero feature of this gyro needs a port. |
11 | // The special auto-zero feature of this gyro needs a port. |
13 | #define AUTOZERO_PORT PORTD |
12 | #define AUTOZERO_PORT PORTD |
14 | #define AUTOZERO_DDR DDRD |
13 | #define AUTOZERO_DDR DDRD |
15 | #define AUTOZERO_BIT 5 |
14 | #define AUTOZERO_BIT 5 |
16 | 15 | ||
17 | void gyro_calibrate(void) { |
16 | void gyro_calibrate(void) { |
18 | // If port not already set to output and high, do it. |
17 | // If port not already set to output and high, do it. |
19 | if (!(AUTOZERO_DDR & (1 << AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1 << AUTOZERO_BIT))) { |
18 | if (!(AUTOZERO_DDR & (1 << AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1 << AUTOZERO_BIT))) { |
20 | AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
19 | AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
21 | AUTOZERO_DDR |= (1 << AUTOZERO_BIT); |
20 | AUTOZERO_DDR |= (1 << AUTOZERO_BIT); |
22 | delay_ms(100); |
21 | delay_ms(100); |
23 | } |
22 | } |
24 | 23 | ||
25 | // Make a pulse on the auto-zero output line. |
24 | // Make a pulse on the auto-zero output line. |
26 | AUTOZERO_PORT &= ~(1 << AUTOZERO_BIT); |
25 | AUTOZERO_PORT &= ~(1 << AUTOZERO_BIT); |
27 | delay_ms(1); |
26 | delay_ms(1); |
28 | AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
27 | AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
29 | // Delay_ms(10); |
28 | // Delay_ms(10); |
30 | delay_ms_with_adc_measurement(100, 0); |
29 | delay_ms_with_adc_measurement(100, 0); |
31 | } |
30 | } |
32 | 31 | ||
33 | void gyro_init() { |
32 | void gyro_init(void) { |
34 | gyro_calibrate(); |
33 | gyro_calibrate(); |
35 | } |
34 | } |
36 | 35 | ||
37 | void gyro_setDefaultParameters(void) { |
36 | void gyro_setDefaultParameters(void) { |
38 | staticParams.gyroD = 3; |
37 | staticParams.gyroD = 3; |
39 | IMUConfig.driftCompDivider = 2; |
38 | IMUConfig.driftCompDivider = 2; |
40 | IMUConfig.driftCompLimit = 5; |
39 | IMUConfig.driftCompLimit = 5; |
41 | IMUConfig.zerothOrderCorrection = 1; |
40 | IMUConfig.zerothOrderCorrection = 1; |
42 | IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_Z; |
41 | IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_Z; |
43 | } |
42 | } |
44 | 43 |