HAPSRG / GPSUBX_UART

GPS/GNSS UBX library for UART

Dependencies:   Vector3

Dependents:   GPS_0002

GPSUBX_UART.hpp@17:2e6147aeba54, 2021-12-10 (annotated)

Committer:
cocorlow
Date:
Fri Dec 10 10:41:56 2021 +0000
Revision:
17:2e6147aeba54
Parent:
13:facd8e54f2eb 
Eigen Dense

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 0:cf7c726ec8a1 1 #ifndef __GPSUBX_UART_HPP__
cocorlow 0:cf7c726ec8a1 2 #define __GPSUBX_UART_HPP__
cocorlow 0:cf7c726ec8a1 3
cocorlow 0:cf7c726ec8a1 4 #include "mbed.h"
cocorlow 17:2e6147aeba54 5 // Eigen
cocorlow 17:2e6147aeba54 6 #include <Eigen/Dense.h>
cocorlow 17:2e6147aeba54 7 //#include <Eigen/Core.h>
cocorlow 17:2e6147aeba54 8 //#include <Eigen/LU.h>
cocorlow 17:2e6147aeba54 9 //#include <Eigen/Geometry.h>
cocorlow 17:2e6147aeba54 10 //using namespace std;
cocorlow 17:2e6147aeba54 11 using namespace Eigen;
cocorlow 0:cf7c726ec8a1 12
cocorlow 0:cf7c726ec8a1 13 #define POSECEF_LEN 20
cocorlow 0:cf7c726ec8a1 14 #define POSLLH_LEN 28
NaotoMorita 13:facd8e54f2eb 15 #define STATUS_LEN 16
cocorlow 0:cf7c726ec8a1 16 #define TIMEUTC_LEN 20
cocorlow 0:cf7c726ec8a1 17 #define VELECEF_LEN 20
cocorlow 0:cf7c726ec8a1 18 #define VELNED_LEN 36
cocorlow 0:cf7c726ec8a1 19
cocorlow 0:cf7c726ec8a1 20 #define UBX_SYNC 0x62 << 8 | 0xb5
cocorlow 0:cf7c726ec8a1 21
cocorlow 1:71f5168e48c8 22 #define RECEIVE_SIZE 1024
cocorlow 0:cf7c726ec8a1 23 #define SENTENCE_SIZE 64
cocorlow 0:cf7c726ec8a1 24
cocorlow 17:2e6147aeba54 25 #ifndef M_PI_F
cocorlow 17:2e6147aeba54 26 #define M_PI_F 3.141592f
cocorlow 17:2e6147aeba54 27 #endif
cocorlow 17:2e6147aeba54 28
NaotoMorita 12:2ffb2fcaac23 29 //extern Serial twelite;
cocorlow 0:cf7c726ec8a1 30
cocorlow 0:cf7c726ec8a1 31 //// 0x01 0x01
cocorlow 0:cf7c726ec8a1 32 //union POSECEF
cocorlow 0:cf7c726ec8a1 33 //{
cocorlow 0:cf7c726ec8a1 34 // char byte_data[POSECEF_LEN+8];
cocorlow 0:cf7c726ec8a1 35 // struct
cocorlow 0:cf7c726ec8a1 36 // {
cocorlow 0:cf7c726ec8a1 37 // unsigned short sync;
cocorlow 0:cf7c726ec8a1 38 // char m_class;
cocorlow 0:cf7c726ec8a1 39 // char m_id;
cocorlow 0:cf7c726ec8a1 40 // unsigned short pay_len;
cocorlow 0:cf7c726ec8a1 41 // unsigned int iTOW;
cocorlow 0:cf7c726ec8a1 42 // int ecefX;
cocorlow 0:cf7c726ec8a1 43 // int ecefY;
cocorlow 0:cf7c726ec8a1 44 // int ecefZ;
cocorlow 0:cf7c726ec8a1 45 // unsigned int pAcc;
cocorlow 0:cf7c726ec8a1 46 // char check_a;
cocorlow 0:cf7c726ec8a1 47 // char check_b;
cocorlow 0:cf7c726ec8a1 48 // } data;
cocorlow 0:cf7c726ec8a1 49 //};
cocorlow 0:cf7c726ec8a1 50
cocorlow 0:cf7c726ec8a1 51 // 0x01 0x02
cocorlow 0:cf7c726ec8a1 52 union POSLLH
cocorlow 0:cf7c726ec8a1 53 {
cocorlow 0:cf7c726ec8a1 54 char byte_data[POSLLH_LEN];
cocorlow 0:cf7c726ec8a1 55 struct
cocorlow 0:cf7c726ec8a1 56 {
cocorlow 0:cf7c726ec8a1 57 unsigned int iTOW;
cocorlow 0:cf7c726ec8a1 58 int lon;
cocorlow 0:cf7c726ec8a1 59 int lat;
cocorlow 0:cf7c726ec8a1 60 int height;
cocorlow 0:cf7c726ec8a1 61 int hMSL;
cocorlow 0:cf7c726ec8a1 62 unsigned int hAcc;
cocorlow 0:cf7c726ec8a1 63 unsigned int vAcc;
cocorlow 0:cf7c726ec8a1 64 } data;
cocorlow 0:cf7c726ec8a1 65 };
cocorlow 0:cf7c726ec8a1 66
NaotoMorita 13:facd8e54f2eb 67 // 0x01 0x03
NaotoMorita 13:facd8e54f2eb 68 union STATUS
cocorlow 10:a90d07e4c34d 69 {
NaotoMorita 13:facd8e54f2eb 70 char byte_data[STATUS_LEN];
cocorlow 10:a90d07e4c34d 71 struct
cocorlow 10:a90d07e4c34d 72 {
cocorlow 10:a90d07e4c34d 73 unsigned int iTOW;
NaotoMorita 13:facd8e54f2eb 74 unsigned char gpsFix;
NaotoMorita 13:facd8e54f2eb 75 unsigned char flags;
NaotoMorita 13:facd8e54f2eb 76 unsigned char fixStat;
NaotoMorita 13:facd8e54f2eb 77 unsigned char flags2;
NaotoMorita 13:facd8e54f2eb 78 unsigned int ttff;
NaotoMorita 13:facd8e54f2eb 79 unsigned int msss;
cocorlow 10:a90d07e4c34d 80 } data;
cocorlow 10:a90d07e4c34d 81 };
cocorlow 10:a90d07e4c34d 82
cocorlow 0:cf7c726ec8a1 83 // 0x01 0x21
cocorlow 0:cf7c726ec8a1 84 union TIMEUTC
cocorlow 0:cf7c726ec8a1 85 {
cocorlow 0:cf7c726ec8a1 86 char byte_data[TIMEUTC_LEN];
cocorlow 0:cf7c726ec8a1 87 struct
cocorlow 0:cf7c726ec8a1 88 {
cocorlow 0:cf7c726ec8a1 89 unsigned int iTOW;
cocorlow 0:cf7c726ec8a1 90 unsigned int tAcc;
cocorlow 0:cf7c726ec8a1 91 int nano;
cocorlow 0:cf7c726ec8a1 92 unsigned short year;
cocorlow 0:cf7c726ec8a1 93 unsigned char month;
cocorlow 0:cf7c726ec8a1 94 unsigned char day;
cocorlow 0:cf7c726ec8a1 95 unsigned char hour;
cocorlow 0:cf7c726ec8a1 96 unsigned char min;
cocorlow 0:cf7c726ec8a1 97 unsigned char sec;
cocorlow 0:cf7c726ec8a1 98 unsigned char valid;
cocorlow 0:cf7c726ec8a1 99 } data;
cocorlow 0:cf7c726ec8a1 100 };
cocorlow 0:cf7c726ec8a1 101
cocorlow 0:cf7c726ec8a1 102 //// 0x01 0x11
cocorlow 0:cf7c726ec8a1 103 //union VELECEF
cocorlow 0:cf7c726ec8a1 104 //{
cocorlow 0:cf7c726ec8a1 105 // char byte_data[VELECEF_LEN];
cocorlow 0:cf7c726ec8a1 106 // struct
cocorlow 0:cf7c726ec8a1 107 // {
cocorlow 0:cf7c726ec8a1 108 // unsigned int iTOW;
cocorlow 0:cf7c726ec8a1 109 // int ecefVX;
cocorlow 0:cf7c726ec8a1 110 // int ecefVY;
cocorlow 0:cf7c726ec8a1 111 // int ecefVZ;
cocorlow 0:cf7c726ec8a1 112 // unsigned int sAcc;
cocorlow 0:cf7c726ec8a1 113 // } data;
cocorlow 0:cf7c726ec8a1 114 //};
cocorlow 0:cf7c726ec8a1 115
cocorlow 0:cf7c726ec8a1 116 // 0x01 0x12
cocorlow 0:cf7c726ec8a1 117 union VELNED
cocorlow 0:cf7c726ec8a1 118 {
cocorlow 0:cf7c726ec8a1 119 char byte_data[VELNED_LEN];
cocorlow 0:cf7c726ec8a1 120 struct
cocorlow 0:cf7c726ec8a1 121 {
cocorlow 0:cf7c726ec8a1 122 unsigned int iTOW;
cocorlow 0:cf7c726ec8a1 123 int velN;
cocorlow 0:cf7c726ec8a1 124 int velE;
cocorlow 0:cf7c726ec8a1 125 int velD;
cocorlow 0:cf7c726ec8a1 126 unsigned int speed;
cocorlow 0:cf7c726ec8a1 127 unsigned int gSpeed;
cocorlow 0:cf7c726ec8a1 128 signed int heading;
cocorlow 0:cf7c726ec8a1 129 unsigned int sAcc;
cocorlow 0:cf7c726ec8a1 130 unsigned int cAcc;
cocorlow 0:cf7c726ec8a1 131 } data;
cocorlow 0:cf7c726ec8a1 132 };
cocorlow 4:59f0f651296c 133
cocorlow 0:cf7c726ec8a1 134
cocorlow 5:a92850395dcb 135 /**GPS/GNSS library for UART (UBX protocol)
cocorlow 5:a92850395dcb 136 * RX pin is required.
cocorlow 8:1eb5f701a0d0 137 @code
cocorlow 8:1eb5f701a0d0 138 #include "mbed.h"
cocorlow 8:1eb5f701a0d0 139 #include "GPSUBX_UART.cpp"
cocorlow 8:1eb5f701a0d0 140
cocorlow 8:1eb5f701a0d0 141 Serial pc(USBTX, USBRX, 115200);
cocorlow 8:1eb5f701a0d0 142
cocorlow 8:1eb5f701a0d0 143 GPSUBX_UART gps(PD_1, PD_0); // mbed Nucleo 767
cocorlow 8:1eb5f701a0d0 144
cocorlow 8:1eb5f701a0d0 145 void Display()
cocorlow 8:1eb5f701a0d0 146 {
cocorlow 8:1eb5f701a0d0 147 pc.printf("POSLLH: %d, %f, %f, %f\r\n", gps.iTOW_POSLLH, gps.Longitude, gps.Latitude, gps.Height);
cocorlow 8:1eb5f701a0d0 148 pc.printf("VELNED: %d, %f, %f, %f\r\n", gps.iTOW_VELNED, gps.VelocityNED.x, gps.VelocityNED.y, gps.VelocityNED.z);
cocorlow 8:1eb5f701a0d0 149 pc.printf("TIMEUTC: %4d/%2d/%2d %2d:%2d %2d\r\n", gps.Year, gps.Month, gps.Day, gps.Hours, gps.Minutes, gps.Seconds);
cocorlow 8:1eb5f701a0d0 150 }
cocorlow 8:1eb5f701a0d0 151
cocorlow 8:1eb5f701a0d0 152 int main()
cocorlow 8:1eb5f701a0d0 153 {
cocorlow 8:1eb5f701a0d0 154 gps.Attach();
cocorlow 8:1eb5f701a0d0 155 int timer = 0;
cocorlow 8:1eb5f701a0d0 156 while (1)
cocorlow 8:1eb5f701a0d0 157 {
cocorlow 8:1eb5f701a0d0 158 gps.Update();
cocorlow 8:1eb5f701a0d0 159 wait(0.1);
cocorlow 8:1eb5f701a0d0 160 timer++;
cocorlow 8:1eb5f701a0d0 161 if (timer >= 5)
cocorlow 8:1eb5f701a0d0 162 {
cocorlow 8:1eb5f701a0d0 163 Display();
cocorlow 8:1eb5f701a0d0 164 timer = 0;
cocorlow 8:1eb5f701a0d0 165 }
cocorlow 8:1eb5f701a0d0 166 }
cocorlow 8:1eb5f701a0d0 167 }
cocorlow 8:1eb5f701a0d0 168 @endcode
cocorlow 5:a92850395dcb 169 */
cocorlow 0:cf7c726ec8a1 170 class GPSUBX_UART
cocorlow 0:cf7c726ec8a1 171 {
cocorlow 0:cf7c726ec8a1 172 private:
cocorlow 2:6218fe8e54f4 173
cocorlow 1:71f5168e48c8 174 RawSerial serial;
cocorlow 0:cf7c726ec8a1 175 char receive_buffer[RECEIVE_SIZE];
cocorlow 0:cf7c726ec8a1 176 char sentence_buffer[SENTENCE_SIZE];
cocorlow 0:cf7c726ec8a1 177 static const float Radius = 6378136.6f;
cocorlow 2:6218fe8e54f4 178 volatile int receive_index;
cocorlow 0:cf7c726ec8a1 179
cocorlow 1:71f5168e48c8 180 void Receive();
cocorlow 1:71f5168e48c8 181 void Decode(char buffer[], int mc, int mi);
cocorlow 0:cf7c726ec8a1 182
cocorlow 0:cf7c726ec8a1 183 public:
cocorlow 2:6218fe8e54f4 184
cocorlow 2:6218fe8e54f4 185 int dummy; // Compiler bug? If "dummy" does not exist, the value of "Year" (under "public:") may include bugs.
cocorlow 5:a92850395dcb 186 /**Year*/
cocorlow 2:6218fe8e54f4 187 volatile int Year;
cocorlow 5:a92850395dcb 188 /**Month*/
cocorlow 2:6218fe8e54f4 189 volatile int Month;
cocorlow 5:a92850395dcb 190 /**Day*/
cocorlow 2:6218fe8e54f4 191 volatile int Day;
cocorlow 5:a92850395dcb 192 /**Hours*/
cocorlow 2:6218fe8e54f4 193 volatile int Hours;
cocorlow 5:a92850395dcb 194 /**Minutes*/
cocorlow 2:6218fe8e54f4 195 volatile int Minutes;
cocorlow 5:a92850395dcb 196 /**Seconds*/
cocorlow 2:6218fe8e54f4 197 volatile int Seconds;
cocorlow 5:a92850395dcb 198 /**Longitude (deg)*/
cocorlow 5:a92850395dcb 199 volatile float Longitude;
cocorlow 5:a92850395dcb 200 /**Latitude (deg)*/
cocorlow 2:6218fe8e54f4 201 volatile float Latitude;
cocorlow 5:a92850395dcb 202 /**Height [m]*/
cocorlow 2:6218fe8e54f4 203 volatile float Height;
cocorlow 5:a92850395dcb 204 /**iTOW of POSLLH [ms]*/
cocorlow 2:6218fe8e54f4 205 volatile int iTOW_POSLLH;
cocorlow 9:874d7633e358 206 /**Velocity NED [m/s]*/
cocorlow 17:2e6147aeba54 207 Vector3f VelocityNED;
cocorlow 5:a92850395dcb 208 /**Speed [m/s]*/
cocorlow 2:6218fe8e54f4 209 volatile float Speed;
cocorlow 5:a92850395dcb 210 /**Ground speed [m/s]*/
cocorlow 2:6218fe8e54f4 211 volatile float GroundSpeed;
cocorlow 5:a92850395dcb 212 /**Heading angle (deg) */
cocorlow 2:6218fe8e54f4 213 volatile float Heading;
cocorlow 5:a92850395dcb 214 /**iTOW of VELNED [ms]*/
cocorlow 2:6218fe8e54f4 215 volatile int iTOW_VELNED;
cocorlow 1:71f5168e48c8 216
NaotoMorita 13:facd8e54f2eb 217 /**iTOW of STATUS [ms]*/
NaotoMorita 13:facd8e54f2eb 218 volatile int iTOW_STATUS;
NaotoMorita 13:facd8e54f2eb 219 /**gpsFix Status*/
NaotoMorita 13:facd8e54f2eb 220 volatile char gpsFix;
cocorlow 10:a90d07e4c34d 221
cocorlow 10:a90d07e4c34d 222
cocorlow 5:a92850395dcb 223 /**Time zone*/
cocorlow 2:6218fe8e54f4 224 volatile int TimeZone;
cocorlow 0:cf7c726ec8a1 225
cocorlow 17:2e6147aeba54 226 Vector3f UniversalZeroPosition;
cocorlow 17:2e6147aeba54 227 Vector3f UniversalZeroUnitN;
cocorlow 17:2e6147aeba54 228 Vector3f UniversalZeroUnitE;
cocorlow 17:2e6147aeba54 229 Vector3f UniversalZeroUnitD;
cocorlow 17:2e6147aeba54 230 Vector3f UniversalPosition;
cocorlow 5:a92850395dcb 231 /**Position NED [m]*/
cocorlow 17:2e6147aeba54 232 Vector3f PositionNED;
cocorlow 3:6c8715574bb8 233
cocorlow 5:a92850395dcb 234 /**Constructor
cocorlow 3:6c8715574bb8 235 *@param tx UART TX pin
cocorlow 3:6c8715574bb8 236 *@param rx UART RX pin
cocorlow 3:6c8715574bb8 237 *@param baud UART baud rate (default: 38400)
cocorlow 3:6c8715574bb8 238 *@param timezone Time zone (default: +9)
cocorlow 3:6c8715574bb8 239 */
cocorlow 3:6c8715574bb8 240 GPSUBX_UART(PinName tx, PinName rx, int baud = 38400, int timezone = +9);
cocorlow 5:a92850395dcb 241 /**call Update() regularly in the main loop
cocorlow 5:a92850395dcb 242 */
cocorlow 1:71f5168e48c8 243 void Update();
cocorlow 5:a92850395dcb 244 /**call Attach() once before measuring position
cocorlow 5:a92850395dcb 245 */
cocorlow 0:cf7c726ec8a1 246 void Attach();
cocorlow 0:cf7c726ec8a1 247
cocorlow 17:2e6147aeba54 248 Vector3f ToUniversalUnit();
cocorlow 17:2e6147aeba54 249 Vector3f ToUniversal();
cocorlow 0:cf7c726ec8a1 250 void CalculateUnit();
cocorlow 5:a92850395dcb 251 /**call Calculate() before calculating PositionNED
cocorlow 5:a92850395dcb 252 */
cocorlow 0:cf7c726ec8a1 253 void Calculate();
cocorlow 17:2e6147aeba54 254 Vector3f Calculate(Vector3f position);
cocorlow 0:cf7c726ec8a1 255
cocorlow 0:cf7c726ec8a1 256 static void Checksum(char payload[], int n, char* ck_a, char* ck_b);
cocorlow 0:cf7c726ec8a1 257 };
cocorlow 0:cf7c726ec8a1 258 #endif