#include "inc/insgps.h"
#include "stdio.h"
#include "math.h"

extern struct NavStruct Nav;
extern float X[13];

int main()
{
  float gyro[3]={2.47,-0.25,7.71}, accel[3]={-1.02,0.70,-10.11}, dT=0.04, mags[3]={-50,-180,-376};
  float Pos[3]={0,0,0}, Vel[3]={0,0,0}, BaroAlt=2.66, Speed=4.4, Heading=0;
  float yaw;
  int i,j;

  INSGPSInit();

  for(i=0;i<10000000;i++){
    INSPrediction(gyro, accel, dT);
    //MagCorrection(mags);
    FullCorrection(mags,Pos,Vel,BaroAlt);
    yaw = atan2( (float) 2 * (Nav.q[0] * Nav.q[3] + Nav.q[1] * Nav.q[2]),
                 (float) (1 - 2 * (Nav.q[2] * Nav.q[2] + Nav.q[3] * Nav.q[3]) ) ) * 180 / M_PI;

    printf("%0.3f ", yaw);
    for(j=0; j < 13; j++)
      printf("%f ", X[j]);
    printf("\r\n");
  }
  return 0;
}