Diff: NEOM9N_thread.cpp

Revision:

71:4bc51407bff0

Parent:

70:cfeea856d624

Child:

72:b749014457b7

--- a/NEOM9N_thread.cpp	Thu Jun 16 14:03:22 2022 +0200
+++ b/NEOM9N_thread.cpp	Sat Jun 18 18:36:19 2022 +0200
@@ -1,9 +1,10 @@
 #include "NEOM9N_thread.h"
 #include <cstdint>
 
-NEOM9N::NEOM9N(PinName Tx, PinName Rx) : m_bufferedSerial(Tx, Rx), thread(osPriorityLow, 4096)
+NEOM9N::NEOM9N(PinName Tx, PinName Rx) :
+    m_bufferedSerial(Tx, Rx),
+    thread(osPriorityNormal, 4096)
 {
-    m_Ts = 0.005; // GNSS runs at 25 Hz := 0.04 sec, make sure that modulo(0.04 / m_Ts) = 0 e.g. m_Ts = 0.02, 0.005, 0.0025
     m_bufferedSerial.set_baud(38400);
     m_bufferedSerial.set_format(8, BufferedSerial::None, 1);
     m_bufferedSerial.set_blocking(false);
@@ -21,18 +22,19 @@
     ticker.detach();
 }
 
-void NEOM9N::StartThread()
+void NEOM9N::start_loop()
 {
     thread.start(callback(this, &NEOM9N::update));
     ticker.attach(callback(this, &NEOM9N::sendThreadFlag), std::chrono::microseconds{ static_cast<long int>( 1.0e6f * m_Ts ) });
 }
 
-void NEOM9N::ZeroLocal() 
+void NEOM9N::reset_local()
 {
     // copy so it can not be updated during calculations (maybe mutex this)
     ubxNavPVT_t ubxNavPVT = m_ubxNavPVT;
     m_pos_ecef_0 = transformWGS84ToECEF(ubxNavPVT);
     m_R_ecefToLocal_0 = getR_ECEFToLocal(ubxNavPVT);
+    printf("gps init values: %d, %f, %f, %f\r\n", GetNumSV(), m_pos_ecef_0(0), m_pos_ecef_0(1), m_pos_ecef_0(2));
 }
 
 NEOM9N::ubxNavPVT_t NEOM9N::GetUbxNavPVT()
@@ -40,15 +42,6 @@
     return m_ubxNavPVT;
 }
 
-bool NEOM9N::CheckAndToggleHasNewData()
-{
-    if (m_has_new_data) {
-        m_has_new_data = false;
-        return true;
-    }
-    return false;
-}
-
 // Position in ECEF in m
 Eigen::Vector3f NEOM9N::GetPosECEF()
 {
@@ -65,9 +58,9 @@
 Eigen::Vector3f NEOM9N::GetVelENU()
 {
     Eigen::Vector3f vel_enu;
-    vel_enu <<  static_cast<float>( m_ubxNavPVT.velE ) * 1e-3f, 
-                static_cast<float>( m_ubxNavPVT.velN ) * 1e-3f, 
-                -static_cast<float>( m_ubxNavPVT.velD ) * 1e-3f;
+    vel_enu << static_cast<float>( m_ubxNavPVT.velE ) * 1e-3f,
+               static_cast<float>( m_ubxNavPVT.velN ) * 1e-3f,
+              -static_cast<float>( m_ubxNavPVT.velD ) * 1e-3f;
     return vel_enu;
 }
 
@@ -121,15 +114,15 @@
 }
 
 void NEOM9N::update()
-{   
-    static char buffer[256];                // buffer to readout from the SerialBuffer    
+{
+    static char buffer[256];                // buffer to readout from the SerialBuffer
     static char check_buffer[4];            // buffer to check if a seuenz is starting
     const static int msg_buffer_len = 96;   // message length 94 + 2 (UBX_PAYLOAD_INDEX), do not know why +2, do not care
     static char msg_buffer[msg_buffer_len]; // buffer to decode message
     // ? | ? | PAYLOAD 92 | CK_A | CK_B
     static uint8_t msg_buffer_cntr = 0;
     static bool is_msg_sequenz = false;
-    
+
     while(true) {
 
         ThisThread::flags_wait_any(threadFlag);
@@ -147,13 +140,32 @@
                         m_ubxNavPVT.numSV = ubxNavPVT.numSV;
                         if(ubxNavPVT.fixType == 3 && ubxNavPVT.numSV >= M_MIN_SATS) {
                             m_ubxNavPVT = ubxNavPVT;
+                            static bool is_reset_local_internal = false;
+                            if (!is_reset_local_internal) {
+                                reset_local();
+                                is_reset_local_internal = true;
+                            }
                             m_pos_ecef = transformWGS84ToECEF(m_ubxNavPVT);
                             m_pos_enu = m_R_ecefToLocal_0 * ( m_pos_ecef - m_pos_ecef_0 );
-                            m_has_new_data = true;
-                        } else {
-                            m_has_new_data = false;
                         }
                         is_msg_sequenz = false;
+                        // HACK: update float sens_GPS[13];     // pos_enu(3), vel_enu(3), headMot, numSV, hAcc, vAcc, sAcc, headAcc, timestamp
+                        /*
+                        data.sens_GPS[0] = m_pos_enu(0);
+                        data.sens_GPS[1] = m_pos_enu(1);
+                        data.sens_GPS[2] = m_pos_enu(2);
+                        Eigen::Vector3f vel_enu = GetVelENU();
+                        data.sens_GPS[3] = vel_enu(0);
+                        data.sens_GPS[4] = vel_enu(1);
+                        data.sens_GPS[5] = vel_enu(2);
+                        data.sens_GPS[6] = GetHeadMot();
+                        data.sens_GPS[7] = (float)GetNumSV();
+                        data.sens_GPS[8] = GethAcc();
+                        data.sens_GPS[9] = GetvAcc();
+                        data.sens_GPS[10] = GetsAcc();
+                        data.sens_GPS[11] = GetHeadAcc();
+                        data.sens_GPS[12] = global_timer.read();
+                        */
 #if PRINT_FOR_DEBUG
                         printf("%d, %d, %d, %d\r\n", ubxNavPVT.numSV, ubxNavPVT.lon, ubxNavPVT.lat, ubxNavPVT.height);
 #endif
@@ -223,8 +235,8 @@
     return R_ECEFToLocal;
 }
 
-NEOM9N::ubxNavPVT_t NEOM9N::decodeUbxNavPVTmsg(const char *buf) {
-    
+NEOM9N::ubxNavPVT_t NEOM9N::decodeUbxNavPVTmsg(const char *buf)
+{
     static ubxNavPVT_t ubxNavPVT;
     static uint8_t index;
 
@@ -238,7 +250,6 @@
               0060  00 00 B9 53
     */
 
-
     // uint32_t iTOW;    //  0  -    -   GPS time of week of the navigation epoch
     index = UBX_PAYLOAD_INDEX + 0;
     ubxNavPVT.iTOW = buf[index++];
@@ -352,7 +363,6 @@
     ubxNavPVT.magAcc = buf[index++];
     ubxNavPVT.magAcc |= (buf[index++] << 8);
     */
-    
     return ubxNavPVT;
 }