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;
}