Diff: NEOM9N_thread.h

Revision:

62:c9571e4d9005

Child:

66:4057e8e5c248

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NEOM9N_thread.h	Thu Jun 09 07:35:21 2022 +0200
@@ -0,0 +1,82 @@
+#ifndef NEOM9N_THREAD_H
+#define NEOM9N_THREAD_H
+
+#include <mbed.h>
+
+#include "Eigen/Dense.h"
+#include "ThreadFlag.h"
+
+/**
+ * notes:
+ * - you need to configure neo-m9n unit according to one note notes. basically flash 20220602_M9N_config_00.txt via u-center and save
+ * - unit runs at 25 Hz, do not readout faster than the set update rate (config above) 
+ */
+
+#define PRINT_FOR_DEBUG false
+
+#define M_PI 3.14159265358979323846
+
+#define UBX_PAYLOAD_INDEX 2
+#define UBX_PVT_HEADER_0  0xb5
+#define UBX_PVT_HEADER_1  0x62
+#define UBX_PVT_CLASS     0x01
+#define UBX_PVT_ID        0x07
+
+class NEOM9N
+{   
+    public:
+        NEOM9N(PinName TX, PinName RX, float Ts);
+        virtual ~NEOM9N();
+
+        typedef struct ubxNavPVT_s {
+            uint8_t fixType;  // 20  -    -   GNSSfix Type
+            uint8_t numSV;    // 23  -    -   Number of satellites used in Nav Solution
+            int32_t lon;      // 24 1e-7 deg  Longitude
+            int32_t lat;      // 28 1e-7 deg  Latitude
+            int32_t height;   // 32  -   mm   Height above ellipsoid
+            uint32_t hAcc;    // 40  -   mm   Horizontal accuracy estimate
+            uint32_t vAcc;    // 44  -   mm   Vertical accuracy estimate
+            int32_t velN;     // 48  -   mm/s NED north velocity
+            int32_t velE;     // 52  -   mm/s NED east velocity
+            int32_t velD;     // 56  -   mm/s NED down velocity
+            int32_t gSpeed;   // 60  -   mm/s Ground Speed (2-D)
+            int32_t headMot;  // 64 1e-5 deg  Heading of motion (2-D)
+            uint32_t sAcc;    // 68 -    mm/s Speed accuracy estimate
+            uint32_t headAcc; // 72 1e-5 deg  Heading accuracy estimate (both motion and vehicle)
+            int16_t magDec;   // 88 1e-2 deg  Magnetic declination.
+            uint16_t magAcc;  // 90 1e-2 deg  Magnetic declination accuracy
+        } ubxNavPVT_t;
+
+        void StartThread();
+        void ZeroLocal();
+        ubxNavPVT_t GetUbxNavPVT();
+        Eigen::Vector3f GetPosECEF();
+        Eigen::Vector3f GetPosENU();
+        Eigen::Vector3f GetVelENU();
+        uint8_t GetFixType();
+        uint8_t GetNumSV();
+
+    private:
+        float m_Ts;
+        BufferedSerial m_bufferedSerial;
+        bool m_has_fix = false;
+        ubxNavPVT_t m_ubxNavPVT;
+#if PRINT_FOR_DEBUG
+        Timer m_run_timer;
+#endif
+        Eigen::Vector3d m_pos_ecef_0;
+        Eigen::Matrix3d m_R_ecefToLocal_0;
+
+        ThreadFlag threadFlag;
+        Thread thread;
+        Ticker ticker;
+
+        void update();
+        Eigen::Vector3d transformWGS84ToECEF(const ubxNavPVT_t& ubxNavPVT);
+        Eigen::Matrix3d getR_ECEFToLocal(const ubxNavPVT_t& ubxNavPVT);
+        ubxNavPVT_t decodeUbxNavPVTmsg(const char *buf);
+        void sendThreadFlag();
+};
+
+#endif /* NEOM9N_THREAD_H */
+