mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
e0d63103ec
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1302 ebee16cc-31ac-478f-84a7-5cbb03baadba
31 lines
763 B
C
31 lines
763 B
C
#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;
|
|
}
|