Diff: main.cpp

Revision:

18:ba523ddf9a89

Parent:

13:fc873da5b445

Child:

19:428ad790bdb3

--- a/main.cpp	Wed Sep 27 15:48:21 2017 +0000
+++ b/main.cpp	Mon May 13 11:45:43 2019 +0000
@@ -39,6 +39,7 @@
 /* Includes */
 #include "mbed.h"
 #include "XNucleoIKS01A2.h"
+#include "MagdwickAHRS.h"
 
 /* Instantiate the expansion board */
 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
@@ -92,6 +93,11 @@
   float value1, value2;
   char buffer1[32], buffer2[32];
   int32_t axes[3];
+  int32_t mag[3];
+  int32_t gyr[3];
+  int32_t ac[3];
+  float pitch, roll, yaw;
+  
   
   /* Enable all sensors */
   hum_temp->enable();
@@ -103,42 +109,53 @@
   
   printf("\r\n--- Starting new run ---\r\n");
 
-  hum_temp->read_id(&id);
-  printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
-  press_temp->read_id(&id);
-  printf("LPS22HB  pressure & temperature   = 0x%X\r\n", id);
   magnetometer->read_id(&id);
-  printf("LSM303AGR magnetometer            = 0x%X\r\n", id);
+ // printf("LSM303AGR magnetometer            = 0x%X\r\n", id);
   accelerometer->read_id(&id);
-  printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
+//  printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
   acc_gyro->read_id(&id);
-  printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
+ // printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
+ 
+ 
+ Magdwick magdwick_filter;
+ magdwick_filter::begin(sampleFreqDef); 
+ 
  
   while(1) {
     printf("\r\n");
-
-    hum_temp->get_temperature(&value1);
-    hum_temp->get_humidity(&value2);
-    printf("HTS221: [temp] %7s C,   [hum] %s%%\r\n", print_double(buffer1, value1), print_double(buffer2, value2));
+    
+    magnetometer->get_m_axes(axes);-
+    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]);
     
-    press_temp->get_temperature(&value1);
-    press_temp->get_pressure(&value2);
-    printf("LPS22HB: [temp] %7s C, [press] %s mbar\r\n", print_double(buffer1, value1), print_double(buffer2, value2));
-
-    printf("---\r\n");
+  //  printf("mag1 is at 0x%08X\r\n", &mag[0]); it's how to get an adress for use in stm studio
+  
+    accelerometer->get_x_axes(axes);
+    ac[0] = axes[0];
+    ac[1] = axes[1];
+    ac[2] = axes[2];
+    printf("LSM303AGR [acc/mg]:  %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]);    
+    
+    acc_gyro->get_g_axes(axes);
+    gyr[0] = axes[0];
+    gyr[1] = axes[1];
+    gyr[2] = axes[2];
+    printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", gyr[0], gyr[1], gyr[2]);
 
-    magnetometer->get_m_axes(axes);
-    printf("LSM303AGR [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
+    magdwick_filter::update(gyr[0], gyr[1], gyr[2], ac[0], ac[1], ac[2], mag[0], mag[1], mag[2]);
+    magdwick_filter::computeAngles();
     
-    accelerometer->get_x_axes(axes);
-    printf("LSM303AGR [acc/mg]:  %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-
-    acc_gyro->get_x_axes(axes);
-    printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-
-    acc_gyro->get_g_axes(axes);
-    printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-
-    wait(1.5);
+    pitch = magdwick_filter::getPitch();
+    roll = magdwick_filter::getRoll();
+    yaw = magdwick_filter::getYaw();
+    
+    printf(" Pitch: %.2f\n Roll: %.2f\n, Yaw: %.2f\n", pitch, roll, yaw);
+    
+    wait(0.5);
   }
 }