Subversion Repositories FlightCtrl

Rev

Rev 1646 | Rev 1887 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1646 Rev 1821
1 
#include "invenSense.h"
 1 
#include "invenSense.h"
2 
#include "timer0.h"
 2 
#include "timer0.h"
3 
#include "configuration.h"
 3 
#include "configuration.h"
4 
 
 4 
 
5 
#include <avr/io.h>
 5 
#include <avr/io.h>
6 
 
 6 
 
7 
/*
 7 
/*
8 
 * Configuration for my prototype board with InvenSense gyros.
 8 
 * 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.
 9 
 * The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll.
10 
 */
 10 
 */
11 
const uint8_t GYRO_REVERSED[3] = {0,0,0};
 11 
const uint8_t GYRO_REVERSED[3] = { 0, 0, 0 };
12 
const uint8_t ACC_REVERSED[3] = {0,0,1};
 12 
const uint8_t ACC_REVERSED[3] = { 0, 0, 1 };
13 
 
 13 
 
14 
#define AUTOZERO_PORT PORTD
 14 
#define AUTOZERO_PORT PORTD
15 
#define AUTOZERO_DDR DDRD
 15 
#define AUTOZERO_DDR DDRD
16 
#define AUTOZERO_BIT 5
 16 
#define AUTOZERO_BIT 5
17 
 
 17 
 
18 
void gyro_calibrate() {
 18 
void gyro_calibrate() {
19 
  // If port not already set to output and high, do it.
 19 
        // If port not already set to output and high, do it.
20 
  if (!(AUTOZERO_DDR & (1<<AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1<<AUTOZERO_BIT))) {
 20 
        if (!(AUTOZERO_DDR & (1 << AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1
- 
 
 21 
                        << AUTOZERO_BIT))) {
21 
    AUTOZERO_PORT |= (1<<AUTOZERO_BIT);
 22 
                AUTOZERO_PORT |= (1 << AUTOZERO_BIT);
22 
    AUTOZERO_DDR |= (1<<AUTOZERO_BIT);
 23 
                AUTOZERO_DDR |= (1 << AUTOZERO_BIT);
23 
    Delay_ms(100);
 24 
                Delay_ms(100);
24 
  }
 25 
        }
25 
 
 26 
 
26 
  // Make a pulse on the auto-zero output line.
 27 
        // Make a pulse on the auto-zero output line.
27 
  AUTOZERO_PORT &= ~(1<<AUTOZERO_BIT);
 28 
        AUTOZERO_PORT &= ~(1 << AUTOZERO_BIT);
28 
  Delay_ms(1);
 29 
        Delay_ms(1);
29 
  AUTOZERO_PORT |= (1<<AUTOZERO_BIT);
 30 
        AUTOZERO_PORT |= (1 << AUTOZERO_BIT);
30 
  // Delay_ms(10);
 31 
        // Delay_ms(10);
31 
  Delay_ms_Mess(100);
 32 
        Delay_ms_Mess(100);
32 
}
 33 
}
33 
 
 34 
 
34 
void gyro_setDefaults(void) {
 35 
void gyro_setDefaults(void) {
35 
  staticParams.GyroD = 3;
 36 
        staticParams.GyroD = 3;
36 
  staticParams.GyroAccFactor = 1;
 37 
        staticParams.GyroAccFactor = 1;
37 
  staticParams.DriftComp = 10;  
 38 
        staticParams.DriftComp = 10;
38 
 
 39 
 
39 
  // Not used.
 40 
        // Not used.
40 
  staticParams.AngleTurnOverPitch = 85;
 41 
        staticParams.AngleTurnOverPitch = 85;
41 
  staticParams.AngleTurnOverRoll = 85;
 42 
        staticParams.AngleTurnOverRoll = 85;
42 
}
 43 
}
43 
 
 44