Subversion Repositories FlightCtrl

#include "invenSense.h"

#include "timer0.h"

#include "configuration.h"

#include <avr/io.h>

#define AUTOZERO_PORT PORTD

#define AUTOZERO_DDR DDRD

#define AUTOZERO_BIT 5

const uint8_t GYROS_REVERSE[2] = {0,0};

void gyro_calibrate() {

  // If port not already set to output and high, do it.

  if (!(AUTOZERO_DDR & (1<<AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1<<AUTOZERO_BIT))) {

    AUTOZERO_PORT |= (1<<AUTOZERO_BIT);

    AUTOZERO_DDR |= (1<<AUTOZERO_BIT);

    Delay_ms(100);

  }

  // Make a pulse on the auto-zero output line.

  AUTOZERO_PORT &= ~(1<<AUTOZERO_BIT);

  Delay_ms(1);

  AUTOZERO_PORT |= (1<<AUTOZERO_BIT);

  // Delay_ms(10);

  Delay_ms_Mess(100);

}

void gyro_setDefaults(void) {

  staticParams.GyroD = 3;

  staticParams.GyroAccFactor = 1;

  staticParams.DriftComp = 1;

  // Not used.

  staticParams.AngleTurnOverPitch = 85;

  staticParams.AngleTurnOverRoll = 85;

}