Diff: main.cpp

Revision:

21:c62412a679b0

Parent:

20:c27bce3cf63b

Child:

22:bbdb7b928ccc

--- a/main.cpp	Mon May 20 08:22:32 2019 +0000
+++ b/main.cpp	Mon May 20 11:58:29 2019 +0000
@@ -40,7 +40,7 @@
 #include "mbed.h"
 #include "XNucleoIKS01A2.h"
 #include "MadgwickAHRS.h"
-#define sampleFreqDef   1500.0f
+#define sampleFreqDef   2.0f
 /* Instantiate the expansion board */
 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
 
@@ -87,18 +87,42 @@
   return str;
 }
 
+// NMEA conversion
+
+void float_to_char(float my_float, char* tab){
+        tab[0] = (char)( (int)(my_float/100.0) + 48);
+    tab[1] = (char)( (int)((my_float - (tab[0]- '0')*100)/10.0) + 48);
+    tab[2] = (char)( (int)((my_float - (tab[0]- '0')*100 - (tab[1]- '0')*10 )) + 48);
+    tab[4] = (char)( (int)((my_float - (tab[0]- '0')*100 - (tab[1]- '0')*10  - (tab[2]- '0'))*10.0) + 48);
+    tab[5] = (char)( (int)((my_float - (tab[0]- '0')*100 - (tab[1]- '0')*10 -  (tab[2]- '0') - (tab[4]- '0')/10.0)*100.0) + 48);
+}
+
+char * get_nmea_sentence(float roll, float pitch, float yaw){
+    static char sentence[] = "$PASHR,hhmmss.sss,hhh.hh,T,rrr.rr,ppp.pp,xxx.xx,a.aaa,b.bbb,c.ccc,d,e*hh\n";
+    //float roll_deg =    roll *  360/(2*3.14);
+   // float pitch_deg =   pitch * 360/(2*3.14);
+   // float yaw_deg =     yaw *  360/(2*3.14);
+
+    float_to_char(roll, sentence + 27);
+    float_to_char(pitch, sentence + 34);
+    float_to_char(yaw, sentence + 18);
+
+    return sentence;
+}
+
+
 /* Simple main function */
 int main() {
   uint8_t id;
   float value1, value2;
   char buffer1[32], buffer2[32];
+  char* nmea;
   int32_t axes[3];
   int32_t mag[3];
   int32_t gyr[3];
   int32_t ac[3];
-  float offset[9] = {0};
+  float gyrf[3];
   float pitch, roll, yaw;
-  int offset_cnt = 0;
   
   
   /* Enable all sensors */
@@ -130,7 +154,7 @@
     mag[0] = axes[0];
     mag[1] = axes[1];
     mag[2] = axes[2];
-    printf("LSM303AGR [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", mag[0], mag[1], mag[2]);
+  //  printf("LSM303AGR [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", mag[0], mag[1], mag[2]);
     
   //  printf("mag1 is at 0x%08X\r\n", &mag[0]); it's how to get an adress for use in stm studio
   
@@ -138,7 +162,7 @@
     ac[0] = axes[0];
     ac[1] = axes[1];
     ac[2] = axes[2];
-    printf("LSM303AGR [acc/mg]:  %6ld, %6ld, %6ld\r\n", ac[0] - offset[3]*1000, ac[1] - offset[4]*1000, ac[2] - offset[5]*1000);
+ //   printf("LSM303AGR [acc/g]:  %6ld, %6ld, %6ld\r\n", ac[0], ac[1], ac[2]);
     
    //acc_gyro->get_x_axes(axes);
    //printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", ac[0], ac[1], ac[2]);    
@@ -147,42 +171,25 @@
     gyr[0] = axes[0];
     gyr[1] = axes[1];
     gyr[2] = axes[2];
-    printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", gyr[0]/1000 - offset[0], gyr[1]/1000 - offset[1], gyr[2]/1000 - offset[2]);
-    if (offset_cnt > 10)
-    {
-    madgwick_filter.update(gyr[0]/1000 - offset[6], gyr[1]/1000 - offset[7], gyr[2]/1000 - offset[8], ac[0]/1000 - offset[3], ac[1]/1000 - offset[4], ac[2]/1000- offset[5], mag[0]/1000, mag[1]/1000, mag[2]/1000);
-    //madgwick_filter.update(0,0,0,0,0,0,0,0,0);
+  //  printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", gyr[0], gyr[1], gyr[2]);
+    
+    gyrf[0] = 1.0*gyr[0];
+    gyrf[1] = 1.0*gyr[1];
+    gyrf[2] = 1.0*gyr[2];
+   // printf(" gx: %.2f gy: %.2f , gz: %.2f\n\r", gyrf[0], gyrf[1], gyrf[2]);
+    
+    madgwick_filter.update((float)gyr[0]/1000.0, (float)gyr[1]/1000.0, (float)gyr[2]/1000.0, (float)ac[0], (float)ac[1], (float)ac[2], (float)mag[0], (float)mag[1], (float)mag[2]);
+    //madgwick_filter.update(0,100,0,0,0,0,0,0,0);
     madgwick_filter.computeAngles();
-    }
     
     pitch = madgwick_filter.getPitch();
     roll = madgwick_filter.getRoll();
     yaw = madgwick_filter.getYaw();
     
-    if (offset_cnt < 10)
-    {
-    offset[0] += mag[0]/1000;
-    offset[1] += mag[1]/1000;
-    offset[2] += mag[2]/1000;
-    offset[3] += ac[0]/1000;
-    offset[4] += ac[1]/1000;
-    offset[5] += ac[2]/1000;
-    offset[6] += gyr[0]/1000;
-    offset[7] += gyr[1]/1000;
-    offset[8] += gyr[2]/1000;
-    }
-    if(offset_cnt == 10)
-    {
-        for(int i = 0; i<10;i++)    
-        {
-            offset[i] = offset[i]/10;
-            printf("Offset[%d]: %.2f/n",i,offset[i]);
-        }
-    }
-      
-    offset_cnt++;
-    printf(" Roll: %.2f\n Pitch: %.2f\n , Yaw: %.2f\n", roll, pitch, yaw);
-    printf("Hehehehe");
+   // printf(" Roll: %.2f Pitch: %.2f , Yaw: %.2f\n\r", roll, pitch, yaw);
+    nmea = get_nmea_sentence(roll,pitch,yaw);
+    printf("%s",nmea);
+   // printf("Hehehehe\n\r");
     wait(0.5);
   }
 }