Diff: main.cpp
- Revision:
- 1:aca0c191fb1c
- Parent:
- 0:b61f317f452e
- Child:
- 2:78c3d0598819
--- a/main.cpp Mon Jan 30 08:02:30 2012 +0000
+++ b/main.cpp Tue Jan 31 08:13:04 2012 +0000
@@ -9,8 +9,8 @@
HMC5883L compass(p9, p10);
Serial pc(USBTX, USBRX);
-#define N 3
-#define pi 3.14159265
+#define N 3
+#define pi 3.14159265
double* RK4( double, double[N], double[N] );
double* func( double[N], double[N] );
@@ -30,10 +30,10 @@
double gyro_rad [N] = {0};
int mag [N] = {0};
- double ang_deg[N] = {0};
- double ang_rad[N] = {0};
-
- double* p_ang;
+ double ang_deg[N] = {0};
+ double ang_rad[N] = {0};
+
+ double* p_ang;
// *** Setting up accelerometer ***
// These are here to test whether any of the initialization fails. It will print the failure.
@@ -101,18 +101,18 @@
// Low pass filter for gyro
for ( int i=0;i<N;i++ ){ if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i]=0; } }
- for ( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
+ for ( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
- // RK4 (Prediction)
- p_ang = RK4(dt,ang_rad,gyro_rad);
- for ( int i=0;i<N;i++ ){ ang_rad[i] = *p_ang; p_ang = p_ang+1; }
+ // RK4 (Prediction)
+ p_ang = RK4(dt,ang_rad,gyro_rad);
+ for ( int i=0;i<N;i++ ){ ang_rad[i] = *p_ang; p_ang = p_ang+1; }
- for ( int i=0;i<N;i++ ){ ang_deg[i] = ang_rad[i]*180/pi; }
+ for ( int i=0;i<N;i++ ){ ang_deg[i] = ang_rad[i]*180/pi; }
//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2]);
- pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
-
- wait(dt);
+ pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
+
+ wait(dt);
t += dt;
}
@@ -120,49 +120,50 @@
double* RK4( double dt, double y[N], double x[N] ){
-
- double yBuf[N] = {0};
- double k[N][4] = {0};
+
+ double yBuf[N] = {0};
+ double k[N][4] = {0};
- double* p_y = y;
+ double* p_y = y;
- double* pk1;
- double* pk2;
- double* pk3;
- double* pk4;
+ double* pk1;
+ double* pk2;
+ double* pk3;
+ double* pk4;
- for ( int i=0;i<N;i++){ yBuf[i] = y[i]; }
- pk1 = func (yBuf,x);
- for ( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]; }
+ pk1 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
- for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
- pk2 = func (yBuf,x);
- for ( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
+ pk2 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
- for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
- pk3 = func (yBuf,x);
- for ( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
+ pk3 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
- for ( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
- pk4 = func (yBuf,x);
- for ( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
+ pk4 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
- for ( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; }
+ for ( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; }
- return p_y;
+ return p_y;
}
double* func( double y[N], double x[N] ){
- double f[N] = {0};
- double* p_f = f;
-
- f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
- f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
- f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
+ double f[N] = {0};
+ double* p_f = f;
+ double a = 0;
+
+ f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
+ f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
+ f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
- return p_f;
+ return p_f;
}
\ No newline at end of file
hige dura