Diff: main.cpp

Revision:

59:1ef9ce8622cd

Parent:

58:f1b47b30f7c4

Child:

60:3ba75014e3fc

--- a/main.cpp	Wed Jun 08 09:23:36 2022 +0200
+++ b/main.cpp	Wed Jun 08 16:38:04 2022 +0200
@@ -6,6 +6,7 @@
 #include "QMC5883L.h"
 #include "LinearCharacteristics.h"
 #include "NEOM9N.h"
+#include "AvgFilter.h"
 
 // logical variable main task
 bool do_execute_main_task = true;  // this variable will be toggled via the user button (blue button) to or not to execute the main task
@@ -19,7 +20,7 @@
 int main()
 {
     // while loop gets executed every main_task_period_ms milliseconds
-    const int main_task_period_ms = 50;  // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
+    const int main_task_period_ms = 40;  // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
     Timer main_task_timer;               // create Timer object which we use to run the main task every main task period time in ms
     main_task_timer.start();
     
@@ -40,7 +41,17 @@
     
     // create object for GNSS Sensor NEO-M9N
     NEOM9N neom9n(PA_9, PA_10); // UART1
-    
+    bool isGNSSInit = false;
+
+    /*
+    bool is_first_readout = true;
+    uint8_t Navg = 25;
+    AvgFilter* pos_ecef_avg[3];
+    pos_ecef_avg[0] = new AvgFilter(Navg);
+    pos_ecef_avg[1] = new AvgFilter(Navg);
+    pos_ecef_avg[2] = new AvgFilter(Navg);
+    */
+
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
     user_button.rise(&user_button_released_fcn);   
@@ -60,6 +71,18 @@
         // update GNSS
         neom9n.update();
 
+        if (!isGNSSInit && std::chrono::duration_cast<std::chrono::milliseconds>(run_timer.elapsed_time()).count() >= 1000) {
+            neom9n.init();
+            isGNSSInit = true;
+        }
+
+        /*
+        if (is_first_readout) {
+            is_first_readout = false;
+            pos_ecef_avg[0]->reset()
+        }
+        */
+
         if (do_execute_main_task) {
 
         } else {
@@ -69,25 +92,23 @@
         user_led = !user_led;
 
         // do only output via serial what's really necessary (this makes your code slow)
-        printf("%0.3f, %0.3f, %0.3f, ",        //  1: 3 : scaled with 10.0f
-            mag_val[0] * 10.0f, mag_val[1] * 10.0f, mag_val[2] * 10.0f);
-        printf("%0.3f, %0.3f, %0.3f, ",        //  4: 6 : scaled with 10.0f
-            neom9n.getVelN() * 10.0f, neom9n.getVelE() * 10.0f, neom9n.getVelD() * 10.0f);
-        printf("%0.3f, %0.3f, %0.3f, %0.3f, ", //  7:10 : scaled with 10.0f
-            neom9n.getGroundSpeed() * 10.0f, neom9n.getSpeedAcc() * 10.0f, neom9n.getHeading() * 10.0f, neom9n.getHeadAcc() * 10.0f);
-        //printf("%0.6f, %0.6f, %0.3f, %0.3f, ", // 11:14 : scaled with 10.0f
-        //    neom9n.getLon() * 10.0f, neom9n.getLat() * 10.0f, neom9n.getHeight() * 10.0f, neom9n.getHeightMSL() * 10.0f);
-        printf("%d, %d, %d, %d, ", // 11:14 : raw data
-            neom9n.actualPVT.lon, neom9n.actualPVT.lat, neom9n.actualPVT.height, neom9n.actualPVT.heightMSL);
-        printf("%0.3f, %d, ",                  // 15:16 : no scaling
+        printf("%0.3f, %0.3f, %0.3f, ",        //  1: 3
+            mag_val[0], mag_val[1], mag_val[2]);
+        printf("%0.3f, %0.3f, %0.3f, %0.3f, ", //  4: 7
+            neom9n.getGroundSpeed(), neom9n.getSpeedAcc(), neom9n.getHeading(), neom9n.getHeadAcc());
+        printf("%d, %d, %d, %d, ",             //  8:11 see manual for scaling information
+            neom9n.m_actualPVT.lon, neom9n.m_actualPVT.lat, neom9n.m_actualPVT.height, neom9n.m_actualPVT.heightMSL);
+        printf("%0.3f, %d, ",                  // 12:13
             neom9n.getSatTime(), neom9n.getNumSat());
-        float x, y, z, p;
-        neom9n.getPosECEF(x, y, z, p);
-        printf("%0.3f, %0.3f, %0.3f,  %0.3f, ",// 17:20: no scaling
-            x, y, z, p);
-        neom9n.getVelECEF(x, y, z, p);
-        printf("%0.3f, %0.3f, %0.3f,  %0.3f, ",// 21:24: no scaling
-            x, y, z, p);
+        Eigen::Vector3f pos_ecef = neom9n.getPosECEF();
+        printf("%0.3f, %0.3f, %0.3f, ",        // 14:16
+            pos_ecef.x(), pos_ecef.y(), pos_ecef.z());
+        Eigen::Vector3f pos_enu = neom9n.getPosENU();
+        printf("%0.3f, %0.3f, %0.3f, ",        // 17:19
+            pos_enu.x(), pos_enu.y(), pos_enu.z());
+        Eigen::Vector3f vel_enu = neom9n.getVelENU();
+        printf("%0.3f, %0.3f, %0.3f, ",        // 20:22
+            vel_enu.x(), vel_enu.y(), vel_enu.z());
 
         int run_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(run_timer.elapsed_time()).count();
         printf("%d\r\n", run_elapsed_time_ms);