Diff: AHRS.cpp

Revision:

7:bfde7bd5fe31

Parent:

6:5824bd96b6cf

Child:

8:51062bb877f0

--- a/AHRS.cpp	Fri Jun 14 07:05:20 2019 +0000
+++ b/AHRS.cpp	Wed Jun 26 14:19:01 2019 +0000
@@ -20,15 +20,18 @@
     LinearCharacteristics raw_gz2gz(-32767,32768,-500*PI/180.0, 500*PI/180.0);
     LinearCharacteristics int2magx( -32767,32768,100.0,-100.0);     // x-axis reversed
     LinearCharacteristics int2magy( -32767,32768,100.0,-100.0);     // y-axis reversed*/
-    LinearCharacteristics raw_ax2ax( 1.0,0.0);       // use gain and offset here
-    LinearCharacteristics raw_ay2ay(-1.0,0.0);      // y-axis reversed
-    LinearCharacteristics raw_az2az( 1.0,0.0);
-    LinearCharacteristics raw_gx2gx( 1.0,0.0);
-    LinearCharacteristics raw_gy2gy(-1.0,0.0);  // y-axis reversed (lefthanded system)
-    LinearCharacteristics raw_gz2gz( 1.0,0.0);
-    LinearCharacteristics int2magx( -1.0,0.0);     // x-axis reversed
-    LinearCharacteristics int2magy( -1.0,0.0);     // y-axis reversed
-    LinearCharacteristics int2magz(  1.0,0.0);
+    raw_ax2ax.setup( 1.0,-.31);       // use gain and offset here
+    raw_ay2ay.setup(-1.0,-0.31);      // y-axis reversed // was -1,0.0
+    raw_az2az.setup( 1.0,0.0);
+    raw_gx2gx.setup( 1.0,0.0);
+    raw_gy2gy.setup(-1.0,0.0);  // y-axis reversed (lefthanded system)
+    raw_gz2gz.setup( 1.0,0.0);
+    int2magx.setup(  1.0,0.0);     // x-axis reversed
+    int2magy.setup(  1.0,0.0);     // y-axis reversed
+    int2magz.setup(  1.0,0.0);
+    local_time = 0.0;
+    //
+    
     //matrix measurement(6,1,0.0);
     while (!imu.begin()) {
         wait(1);
@@ -41,11 +44,11 @@
 
 void AHRS::update(void)
 {
-    while(1){
+    while(1){   
         thread.signal_wait(signal);
         imu.readAccel();
         //imu.readMag_calibrated();
-        imu.readMag();
+        //imu.readMag();
         imu.readGyro();
         matrix measurement(6,1,0.0);
         //Perform filter update
@@ -57,15 +60,27 @@
         measurement.put_entry(3,0,raw_ax2ax(imu.accX));
         measurement.put_entry(4,0,raw_ay2ay(imu.accY));
         RPY_filter.loop(&measurement);
+        //measurement.put_entry(0,0,10*sin(local_time));   my_logger.data_vector[8]=10*sin(local_time);    measurement.put_entry(1,0,-5*cos(local_time));    my_logger.data_vector[9]=-5*cos(local_time);   measurement.put_entry(3,0,sin(2*local_time));  measurement.put_entry(4,0,cos(3*local_time));
+        //if(local_time<=2.0) RPY_filter.loop(&measurement); if(local_time==0.0) printf("\r\n"); if(local_time<=0.2){ printf("%1.2f xk: ",local_time); for(int k=0;k<6;k++) printf("%1.5f ",RPY_filter.get_est_state(k)); printf("\r\n");}
+        //local_time += 0.01;
         my_logger.data_vector[0] = raw_gx2gx(imu.gyroX);
         my_logger.data_vector[1] = raw_gy2gy(imu.gyroY);
         my_logger.data_vector[2] = raw_gz2gz(imu.gyroZ);
         my_logger.data_vector[3] = raw_ax2ax(imu.accX);
         my_logger.data_vector[4] = raw_ay2ay(imu.accY);
         my_logger.data_vector[5] = raw_az2az(imu.accZ);
-        my_logger.data_vector[6] = RPY_filter.get_est_state(1);
-        my_logger.data_vector[7] = RPY_filter.get_est_state(2);
+        my_logger.data_vector[6] = RPY_filter.get_est_state(0);
+        my_logger.data_vector[7] = RPY_filter.get_est_state(1);
+        my_logger.data_vector[8] = RPY_filter.get_est_state(4);
+        my_logger.data_vector[9] = RPY_filter.get_est_state(5);
+        
+        //printf("R %1.5f P %1.5f gx: %1.5f gy: %1.5f\r\n",my_logger.data_vector[6],my_logger.data_vector[7],my_logger.data_vector[0],my_logger.data_vector[1]); 
+        //printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\r\n",my_logger.data_vector[6],my_logger.data_vector[7],raw_gx2gx(imu.gyroX),raw_gy2gy(imu.gyroY),raw_gz2gz(imu.gyroZ),raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); 
+        //printf("ax: %1.4f ay: %1.4f az: %1.4f\r\n",raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); 
+        //printf("gx: %1.4f gy: %1.4f gz: %1.4f\r\n",raw_gx2gx(imu.gyroX),raw_gy2gy(imu.gyroY),raw_gz2gz(imu.gyroZ)); 
+        //printf("ax: %1.4f ay: %1.4f az: %1.4f\r\n",raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); 
         }       // while(1) (the thread)
+        
 }
 void AHRS::sendSignal() {