Michael Ernst Peter / Mbed OS Test_GPS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Eigen

NEOM9N_thread.cpp@72:b749014457b7, 2022-06-18 (annotated)

Committer:
pmic
Date:
Sat Jun 18 21:39:32 2022 +0200
Revision:
72:b749014457b7
Parent:
71:4bc51407bff0 
Included checksum

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 62:c9571e4d9005 1 #include "NEOM9N_thread.h"
pmic 66:4057e8e5c248 2 #include <cstdint>
pmic 62:c9571e4d9005 3
pmic 71:4bc51407bff0 4 NEOM9N::NEOM9N(PinName Tx, PinName Rx) :
pmic 71:4bc51407bff0 5 m_bufferedSerial(Tx, Rx),
pmic 71:4bc51407bff0 6 thread(osPriorityNormal, 4096)
pmic 62:c9571e4d9005 7 {
pmic 62:c9571e4d9005 8 m_bufferedSerial.set_baud(38400);
pmic 62:c9571e4d9005 9 m_bufferedSerial.set_format(8, BufferedSerial::None, 1);
pmic 62:c9571e4d9005 10 m_bufferedSerial.set_blocking(false);
pmic 62:c9571e4d9005 11 #if PRINT_FOR_DEBUG
pmic 62:c9571e4d9005 12 m_run_timer.start();
pmic 62:c9571e4d9005 13 #endif
pmic 62:c9571e4d9005 14 m_pos_ecef_0.setZero();
pmic 69:2cda20c51989 15 m_pos_ecef.setZero();
pmic 69:2cda20c51989 16 m_pos_enu.setZero();
pmic 62:c9571e4d9005 17 m_R_ecefToLocal_0.setIdentity();
pmic 62:c9571e4d9005 18 }
pmic 62:c9571e4d9005 19
pmic 62:c9571e4d9005 20 NEOM9N::~NEOM9N()
pmic 62:c9571e4d9005 21 {
pmic 62:c9571e4d9005 22 ticker.detach();
pmic 62:c9571e4d9005 23 }
pmic 62:c9571e4d9005 24
pmic 71:4bc51407bff0 25 void NEOM9N::start_loop()
pmic 62:c9571e4d9005 26 {
pmic 62:c9571e4d9005 27 thread.start(callback(this, &NEOM9N::update));
pmic 69:2cda20c51989 28 ticker.attach(callback(this, &NEOM9N::sendThreadFlag), std::chrono::microseconds{ static_cast<long int>( 1.0e6f * m_Ts ) });
pmic 62:c9571e4d9005 29 }
pmic 62:c9571e4d9005 30
pmic 71:4bc51407bff0 31 void NEOM9N::reset_local()
pmic 62:c9571e4d9005 32 {
pmic 69:2cda20c51989 33 // copy so it can not be updated during calculations (maybe mutex this)
pmic 67:1cac5dca9045 34 ubxNavPVT_t ubxNavPVT = m_ubxNavPVT;
pmic 67:1cac5dca9045 35 m_pos_ecef_0 = transformWGS84ToECEF(ubxNavPVT);
pmic 67:1cac5dca9045 36 m_R_ecefToLocal_0 = getR_ECEFToLocal(ubxNavPVT);
pmic 71:4bc51407bff0 37 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));
pmic 62:c9571e4d9005 38 }
pmic 62:c9571e4d9005 39
pmic 62:c9571e4d9005 40 NEOM9N::ubxNavPVT_t NEOM9N::GetUbxNavPVT()
pmic 62:c9571e4d9005 41 {
pmic 62:c9571e4d9005 42 return m_ubxNavPVT;
pmic 62:c9571e4d9005 43 }
pmic 62:c9571e4d9005 44
pmic 69:2cda20c51989 45 // Position in ECEF in m
pmic 62:c9571e4d9005 46 Eigen::Vector3f NEOM9N::GetPosECEF()
pmic 62:c9571e4d9005 47 {
pmic 69:2cda20c51989 48 return m_pos_ecef.cast<float>();
pmic 62:c9571e4d9005 49 }
pmic 62:c9571e4d9005 50
pmic 69:2cda20c51989 51 // Position in East-North-Up in m
pmic 62:c9571e4d9005 52 Eigen::Vector3f NEOM9N::GetPosENU()
pmic 62:c9571e4d9005 53 {
pmic 69:2cda20c51989 54 return m_pos_enu.cast<float>();
pmic 62:c9571e4d9005 55 }
pmic 62:c9571e4d9005 56
pmic 69:2cda20c51989 57 // Velocity in East-North-Up in m
pmic 62:c9571e4d9005 58 Eigen::Vector3f NEOM9N::GetVelENU()
pmic 62:c9571e4d9005 59 {
pmic 62:c9571e4d9005 60 Eigen::Vector3f vel_enu;
pmic 71:4bc51407bff0 61 vel_enu << static_cast<float>( m_ubxNavPVT.velE ) * 1e-3f,
pmic 71:4bc51407bff0 62 static_cast<float>( m_ubxNavPVT.velN ) * 1e-3f,
pmic 71:4bc51407bff0 63 -static_cast<float>( m_ubxNavPVT.velD ) * 1e-3f;
pmic 62:c9571e4d9005 64 return vel_enu;
pmic 62:c9571e4d9005 65 }
pmic 62:c9571e4d9005 66
pmic 69:2cda20c51989 67 // 20 - - GNSSfix Type
pmic 62:c9571e4d9005 68 uint8_t NEOM9N::GetFixType()
pmic 62:c9571e4d9005 69 {
pmic 62:c9571e4d9005 70 return m_ubxNavPVT.fixType;
pmic 62:c9571e4d9005 71 }
pmic 62:c9571e4d9005 72
pmic 69:2cda20c51989 73 // 23 - - Number of satellites used in Nav Solution
pmic 62:c9571e4d9005 74 uint8_t NEOM9N::GetNumSV()
pmic 62:c9571e4d9005 75 {
pmic 62:c9571e4d9005 76 return m_ubxNavPVT.numSV;
pmic 62:c9571e4d9005 77 }
pmic 62:c9571e4d9005 78
pmic 69:2cda20c51989 79 // 0 - ms GPS time of week of the navigation epoch
pmic 69:2cda20c51989 80 uint8_t NEOM9N::GetGPSTimeOfWeek()
pmic 69:2cda20c51989 81 {
pmic 69:2cda20c51989 82 return m_ubxNavPVT.iTOW;
pmic 69:2cda20c51989 83 }
pmic 69:2cda20c51989 84
pmic 69:2cda20c51989 85 // Heading of motion (2-D) in rad wrapped to (-pi, pi)
pmic 69:2cda20c51989 86 float NEOM9N::GetHeadMot()
pmic 69:2cda20c51989 87 {
pmic 69:2cda20c51989 88 float headMot = static_cast<float>( m_ubxNavPVT.headMot ) * 1e-5f * M_PI / 180.0f;
pmic 69:2cda20c51989 89 return atan2f( sinf(headMot), cosf(headMot) );
pmic 69:2cda20c51989 90 }
pmic 69:2cda20c51989 91
pmic 69:2cda20c51989 92 // 72 Heading accuracy estimate in rad (both motion and vehicle)
pmic 69:2cda20c51989 93 float NEOM9N::GetHeadAcc()
pmic 69:2cda20c51989 94 {
pmic 69:2cda20c51989 95 return static_cast<float>( m_ubxNavPVT.headAcc ) * 1e-5f * M_PI / 180.0f;
pmic 69:2cda20c51989 96 }
pmic 69:2cda20c51989 97
pmic 69:2cda20c51989 98 // 40 Horizontal accuracy estimate in m
pmic 69:2cda20c51989 99 float NEOM9N::GethAcc()
pmic 69:2cda20c51989 100 {
pmic 69:2cda20c51989 101 return static_cast<float>( m_ubxNavPVT.hAcc ) * 1e-3f;
pmic 69:2cda20c51989 102 }
pmic 69:2cda20c51989 103
pmic 69:2cda20c51989 104 // 44 Vertical accuracy estimate in m
pmic 69:2cda20c51989 105 float NEOM9N::GetvAcc()
pmic 69:2cda20c51989 106 {
pmic 69:2cda20c51989 107 return static_cast<float>( m_ubxNavPVT.vAcc ) * 1e-3f;
pmic 69:2cda20c51989 108 }
pmic 69:2cda20c51989 109
pmic 69:2cda20c51989 110 // 68 Speed accuracy estimate in m/s
pmic 69:2cda20c51989 111 float NEOM9N::GetsAcc()
pmic 69:2cda20c51989 112 {
pmic 69:2cda20c51989 113 return static_cast<float>( m_ubxNavPVT.sAcc ) * 1e-3f;
pmic 69:2cda20c51989 114 }
pmic 69:2cda20c51989 115
pmic 62:c9571e4d9005 116 void NEOM9N::update()
pmic 71:4bc51407bff0 117 {
pmic 71:4bc51407bff0 118 static char buffer[256]; // buffer to readout from the SerialBuffer
pmic 62:c9571e4d9005 119 static char check_buffer[4]; // buffer to check if a seuenz is starting
pmic 72:b749014457b7 120 const static int msg_buffer_len = 100; // message length 94 + 2 (UBX_PAYLOAD_INDEX), do not know why +2, do not care
pmic 62:c9571e4d9005 121 static char msg_buffer[msg_buffer_len]; // buffer to decode message
pmic 66:4057e8e5c248 122 // ? | ? | PAYLOAD 92 | CK_A | CK_B
pmic 62:c9571e4d9005 123 static uint8_t msg_buffer_cntr = 0;
pmic 62:c9571e4d9005 124 static bool is_msg_sequenz = false;
pmic 71:4bc51407bff0 125
pmic 62:c9571e4d9005 126 while(true) {
pmic 62:c9571e4d9005 127
pmic 62:c9571e4d9005 128 ThisThread::flags_wait_any(threadFlag);
pmic 62:c9571e4d9005 129
pmic 62:c9571e4d9005 130 if (m_bufferedSerial.readable()) {
pmic 62:c9571e4d9005 131
pmic 66:4057e8e5c248 132 uint32_t read_len = m_bufferedSerial.read(buffer, sizeof(buffer));
pmic 66:4057e8e5c248 133 for (uint32_t i = 0; i < read_len; i++) {
pmic 62:c9571e4d9005 134 if (is_msg_sequenz) {
pmic 62:c9571e4d9005 135 msg_buffer[msg_buffer_cntr++] = buffer[i];
pmic 72:b749014457b7 136 if (msg_buffer_cntr == msg_buffer_len) {
pmic 72:b749014457b7 137 // checksum, we need to set those values because B5 62 01 07 are at the end of the message and we also need 01 and 07
pmic 72:b749014457b7 138 uint8_t CK_A = 0x08; // 0x01 + 0x07
pmic 72:b749014457b7 139 uint8_t CK_B = 0x09; // 0x01 + 0x01 + 0x07;
pmic 72:b749014457b7 140 for (int i = 0; i < msg_buffer_len - 2 - 4; i++) {
pmic 72:b749014457b7 141 CK_A += msg_buffer[i];
pmic 72:b749014457b7 142 CK_B += CK_A;
pmic 62:c9571e4d9005 143 }
pmic 72:b749014457b7 144 // proceed if checksum is valid
pmic 72:b749014457b7 145 if (CK_A == msg_buffer[msg_buffer_len - 2 - 4] && CK_B == msg_buffer[msg_buffer_len - 1 - 4]) {
pmic 72:b749014457b7 146 ubxNavPVT_t ubxNavPVT = decodeUbxNavPVTmsg(msg_buffer);
pmic 72:b749014457b7 147 // update fixType and numSV always
pmic 72:b749014457b7 148 m_ubxNavPVT.fixType = ubxNavPVT.fixType;
pmic 72:b749014457b7 149 m_ubxNavPVT.numSV = ubxNavPVT.numSV;
pmic 72:b749014457b7 150 // update if fixtype 3 and numSV sufficent
pmic 72:b749014457b7 151 if(ubxNavPVT.fixType == 3 && ubxNavPVT.numSV >= M_MIN_SATS) {
pmic 72:b749014457b7 152 m_ubxNavPVT = ubxNavPVT;
pmic 72:b749014457b7 153 static bool is_reset_local_internal = false;
pmic 72:b749014457b7 154 if (!is_reset_local_internal) {
pmic 72:b749014457b7 155 reset_local();
pmic 72:b749014457b7 156 is_reset_local_internal = true;
pmic 72:b749014457b7 157 }
pmic 72:b749014457b7 158 m_pos_ecef = transformWGS84ToECEF(m_ubxNavPVT);
pmic 72:b749014457b7 159 m_pos_enu = m_R_ecefToLocal_0 * ( m_pos_ecef - m_pos_ecef_0 );
pmic 72:b749014457b7 160 }
pmic 72:b749014457b7 161 is_msg_sequenz = false;
pmic 72:b749014457b7 162 // HACK: update float sens_GPS[13]; // pos_enu(3), vel_enu(3), headMot, numSV, hAcc, vAcc, sAcc, headAcc, timestamp
pmic 72:b749014457b7 163 /*
pmic 72:b749014457b7 164 data.sens_GPS[0] = m_pos_enu(0);
pmic 72:b749014457b7 165 data.sens_GPS[1] = m_pos_enu(1);
pmic 72:b749014457b7 166 data.sens_GPS[2] = m_pos_enu(2);
pmic 72:b749014457b7 167 Eigen::Vector3f vel_enu = GetVelENU();
pmic 72:b749014457b7 168 data.sens_GPS[3] = vel_enu(0);
pmic 72:b749014457b7 169 data.sens_GPS[4] = vel_enu(1);
pmic 72:b749014457b7 170 data.sens_GPS[5] = vel_enu(2);
pmic 72:b749014457b7 171 data.sens_GPS[6] = GetHeadMot();
pmic 72:b749014457b7 172 data.sens_GPS[7] = (float)GetNumSV();
pmic 72:b749014457b7 173 data.sens_GPS[8] = GethAcc();
pmic 72:b749014457b7 174 data.sens_GPS[9] = GetvAcc();
pmic 72:b749014457b7 175 data.sens_GPS[10] = GetsAcc();
pmic 72:b749014457b7 176 data.sens_GPS[11] = GetHeadAcc();
pmic 72:b749014457b7 177 data.sens_GPS[12] = global_timer.read();
pmic 72:b749014457b7 178 */
pmic 62:c9571e4d9005 179 #if PRINT_FOR_DEBUG
pmic 72:b749014457b7 180 printf("%d, %d, %d, %d\r\n", ubxNavPVT.numSV, ubxNavPVT.lon, ubxNavPVT.lat, ubxNavPVT.height);
pmic 72:b749014457b7 181 #endif
pmic 72:b749014457b7 182 }
pmic 62:c9571e4d9005 183 }
pmic 62:c9571e4d9005 184 }
pmic 62:c9571e4d9005 185 // shift past readings and check if a message starts
pmic 72:b749014457b7 186 check_buffer[3] = check_buffer[2]; // UBX_PVT_HEADER_0: 0xB5
pmic 72:b749014457b7 187 check_buffer[2] = check_buffer[1]; // UBX_PVT_HEADER_1: 0x62
pmic 72:b749014457b7 188 check_buffer[1] = check_buffer[0]; // UBX_PVT_CLASS : 0x01
pmic 72:b749014457b7 189 check_buffer[0] = buffer[i]; // UBX_PVT_ID : 0x07
pmic 62:c9571e4d9005 190 if(check_buffer[3] == UBX_PVT_HEADER_0 && check_buffer[2] == UBX_PVT_HEADER_1 && check_buffer[1] == UBX_PVT_CLASS && check_buffer[0] == UBX_PVT_ID) {
pmic 62:c9571e4d9005 191 #if PRINT_FOR_DEBUG
pmic 62:c9571e4d9005 192 int run_time = std::chrono::duration_cast<std::chrono::microseconds>(m_run_timer.elapsed_time()).count();
pmic 62:c9571e4d9005 193 m_run_timer.reset();
pmic 66:4057e8e5c248 194 printf("Time since last received package: %d, MSG len: %d\r\n", run_time, msg_buffer_cntr);
pmic 62:c9571e4d9005 195 #endif
pmic 62:c9571e4d9005 196 is_msg_sequenz = true;
pmic 62:c9571e4d9005 197 msg_buffer_cntr = 0;
pmic 62:c9571e4d9005 198 }
pmic 62:c9571e4d9005 199 }
pmic 62:c9571e4d9005 200 }
pmic 62:c9571e4d9005 201 }
pmic 62:c9571e4d9005 202 }
pmic 62:c9571e4d9005 203
pmic 62:c9571e4d9005 204 Eigen::Vector3d NEOM9N::transformWGS84ToECEF(const ubxNavPVT_t& ubxNavPVT)
pmic 62:c9571e4d9005 205 {
pmic 62:c9571e4d9005 206 // persistent within this function
pmic 62:c9571e4d9005 207 const static double a = 6378137.0; // WGS | 84 Earth semimajor axis
pmic 62:c9571e4d9005 208 const static double e = 8.1819191 * 1e-2; // eccentricity
pmic 62:c9571e4d9005 209
pmic 62:c9571e4d9005 210 double lon = static_cast<double>( ubxNavPVT.lon ) * 1e-7 * M_PI / 180.0;
pmic 62:c9571e4d9005 211 double lat = static_cast<double>( ubxNavPVT.lat ) * 1e-7 * M_PI / 180.0;
pmic 62:c9571e4d9005 212 double h = static_cast<double>( ubxNavPVT.height ) * 1e-3;
pmic 62:c9571e4d9005 213
pmic 62:c9571e4d9005 214 double sin_lat = sin(lat);
pmic 62:c9571e4d9005 215 double cos_lat = cos(lat);
pmic 62:c9571e4d9005 216 double N = a / sqrt(1 - e * e * sin_lat * sin_lat);
pmic 62:c9571e4d9005 217
pmic 62:c9571e4d9005 218 Eigen::Vector3d pos_ecef;
pmic 62:c9571e4d9005 219 pos_ecef << (h + N) * cos_lat * cos(lon),
pmic 62:c9571e4d9005 220 (h + N) * cos_lat * sin(lon),
pmic 62:c9571e4d9005 221 (h + (1.0 - e*e)*N) * sin_lat;
pmic 62:c9571e4d9005 222 return pos_ecef;
pmic 62:c9571e4d9005 223 }
pmic 62:c9571e4d9005 224
pmic 62:c9571e4d9005 225 Eigen::Matrix3d NEOM9N::getR_ECEFToLocal(const ubxNavPVT_t& ubxNavPVT)
pmic 62:c9571e4d9005 226 {
pmic 62:c9571e4d9005 227 double lon = static_cast<double>(ubxNavPVT.lon) * 1e-7 * M_PI / 180.0;
pmic 62:c9571e4d9005 228 double lat = static_cast<double>(ubxNavPVT.lat) * 1e-7 * M_PI / 180.0;
pmic 62:c9571e4d9005 229
pmic 62:c9571e4d9005 230 double sin_lat = sin(lat);
pmic 62:c9571e4d9005 231 double cos_lat = cos(lat);
pmic 62:c9571e4d9005 232 double sin_lon = sin(lon);
pmic 62:c9571e4d9005 233 double cos_lon = cos(lon);
pmic 62:c9571e4d9005 234
pmic 62:c9571e4d9005 235 Eigen::Matrix3d R_ECEFToLocal;
pmic 62:c9571e4d9005 236 R_ECEFToLocal << -sin_lon, cos_lon, 0,
pmic 62:c9571e4d9005 237 -cos_lon*sin_lat, -sin_lat*sin_lon, cos_lat,
pmic 62:c9571e4d9005 238 cos_lat*cos_lon, cos_lat*sin_lon, sin_lat;
pmic 62:c9571e4d9005 239 return R_ECEFToLocal;
pmic 62:c9571e4d9005 240 }
pmic 62:c9571e4d9005 241
pmic 71:4bc51407bff0 242 NEOM9N::ubxNavPVT_t NEOM9N::decodeUbxNavPVTmsg(const char *buf)
pmic 71:4bc51407bff0 243 {
pmic 62:c9571e4d9005 244 static ubxNavPVT_t ubxNavPVT;
pmic 62:c9571e4d9005 245 static uint8_t index;
pmic 62:c9571e4d9005 246
pmic 66:4057e8e5c248 247 /* example from u-center: len = 6*16+4 = 100 = 4 + 94 + 2
pmic 66:4057e8e5c248 248 09:41:53 0000 B5 62 01 07 5C 00 C8 72 AE 16 E6 07 06 09 09 29 µb..\.Èr®.æ....)
pmic 66:4057e8e5c248 249 0010 35 F3 FF FF FF FF 00 84 D7 17 00 00 A4 00 00 00 5óÿÿÿÿ..×...¤...
pmic 66:4057e8e5c248 250 0020 00 00 00 00 00 00 00 00 00 00 98 BD FF FF FF FF ...........½ÿÿÿÿ
pmic 66:4057e8e5c248 251 0030 FF FF 00 34 F8 DF 00 00 00 00 00 00 00 00 00 00 ÿÿ.4øß..........
pmic 66:4057e8e5c248 252 0040 00 00 00 00 00 00 00 00 00 00 58 3E 0F 00 80 A8 ..........X>...¨
pmic 66:4057e8e5c248 253 0050 12 01 0F 27 00 00 EE 13 4F 2F 00 00 00 00 00 00 ...'..î.O/......
pmic 66:4057e8e5c248 254 0060 00 00 B9 53
pmic 66:4057e8e5c248 255 */
pmic 66:4057e8e5c248 256
pmic 69:2cda20c51989 257 // uint32_t iTOW; // 0 - - GPS time of week of the navigation epoch
pmic 69:2cda20c51989 258 index = UBX_PAYLOAD_INDEX + 0;
pmic 69:2cda20c51989 259 ubxNavPVT.iTOW = buf[index++];
pmic 69:2cda20c51989 260 ubxNavPVT.iTOW |= (buf[index++] << 8);
pmic 69:2cda20c51989 261 ubxNavPVT.iTOW |= (buf[index++] << 16);
pmic 69:2cda20c51989 262 ubxNavPVT.iTOW |= (buf[index++] << 24);
pmic 69:2cda20c51989 263 // uint16_t year; // 4 - - Year (UTC)
pmic 69:2cda20c51989 264 index = UBX_PAYLOAD_INDEX + 4;
pmic 69:2cda20c51989 265 ubxNavPVT.year = buf[index++];
pmic 69:2cda20c51989 266 ubxNavPVT.year |= (buf[index++] << 8);
pmic 69:2cda20c51989 267 // uint8_t month; // 6 - - Month, range 1..12 (UTC)
pmic 69:2cda20c51989 268 index = UBX_PAYLOAD_INDEX + 6;
pmic 69:2cda20c51989 269 ubxNavPVT.month = buf[index++];
pmic 69:2cda20c51989 270 // uint8_t day; // 7 - - Day of month, range 1..31 (UTC)
pmic 69:2cda20c51989 271 index = UBX_PAYLOAD_INDEX + 7;
pmic 69:2cda20c51989 272 ubxNavPVT.day = buf[index++];
pmic 69:2cda20c51989 273 // uint8_t hour; // 8 - - Hour of day, range 0..23 (UTC)
pmic 69:2cda20c51989 274 index = UBX_PAYLOAD_INDEX + 8;
pmic 69:2cda20c51989 275 ubxNavPVT.hour = buf[index++];
pmic 69:2cda20c51989 276 // uint8_t min; // 9 - - Minute of hour, range 0..59 (UTC)
pmic 69:2cda20c51989 277 index = UBX_PAYLOAD_INDEX + 9;
pmic 69:2cda20c51989 278 ubxNavPVT.min = buf[index++];
pmic 69:2cda20c51989 279 // uint8_t sec; // 10 - - Seconds of minute, range 0..60 (UTC)
pmic 69:2cda20c51989 280 index = UBX_PAYLOAD_INDEX + 10;
pmic 69:2cda20c51989 281 ubxNavPVT.sec = buf[index++];
pmic 62:c9571e4d9005 282 // uint8_t fixType; // 20 - - GNSSfix Type
pmic 62:c9571e4d9005 283 index = UBX_PAYLOAD_INDEX + 20;
pmic 62:c9571e4d9005 284 ubxNavPVT.fixType = buf[index];
pmic 62:c9571e4d9005 285 // uint8_t numSV; // 23 - - Number of satellites used in Nav Solution
pmic 62:c9571e4d9005 286 index = UBX_PAYLOAD_INDEX + 23;
pmic 62:c9571e4d9005 287 ubxNavPVT.numSV = buf[index];
pmic 62:c9571e4d9005 288 // int32_t lon; // 24 1e-7 deg Longitude
pmic 62:c9571e4d9005 289 index = UBX_PAYLOAD_INDEX + 24;
pmic 62:c9571e4d9005 290 ubxNavPVT.lon = buf[index++];
pmic 62:c9571e4d9005 291 ubxNavPVT.lon |= (buf[index++] << 8);
pmic 62:c9571e4d9005 292 ubxNavPVT.lon |= (buf[index++] << 16);
pmic 62:c9571e4d9005 293 ubxNavPVT.lon |= (buf[index++] << 24);
pmic 62:c9571e4d9005 294 // int32_t lat; // 28 1e-7 deg Latitude
pmic 62:c9571e4d9005 295 index = UBX_PAYLOAD_INDEX + 28;
pmic 62:c9571e4d9005 296 ubxNavPVT.lat = buf[index++];
pmic 62:c9571e4d9005 297 ubxNavPVT.lat |= (buf[index++] << 8);
pmic 62:c9571e4d9005 298 ubxNavPVT.lat |= (buf[index++] << 16);
pmic 62:c9571e4d9005 299 ubxNavPVT.lat |= (buf[index++] << 24);
pmic 62:c9571e4d9005 300 // int32_t height; // 32 - mm Height above ellipsoid
pmic 62:c9571e4d9005 301 index = UBX_PAYLOAD_INDEX + 32;
pmic 62:c9571e4d9005 302 ubxNavPVT.height = buf[index++];
pmic 62:c9571e4d9005 303 ubxNavPVT.height |= (buf[index++] << 8);
pmic 62:c9571e4d9005 304 ubxNavPVT.height |= (buf[index++] << 16);
pmic 62:c9571e4d9005 305 ubxNavPVT.height |= (buf[index++] << 24);
pmic 62:c9571e4d9005 306 // uint32_t hAcc; // 40 - mm Horizontal accuracy estimate
pmic 62:c9571e4d9005 307 index = UBX_PAYLOAD_INDEX + 40;
pmic 62:c9571e4d9005 308 ubxNavPVT.hAcc = buf[index++];
pmic 62:c9571e4d9005 309 ubxNavPVT.hAcc |= (buf[index++] << 8);
pmic 62:c9571e4d9005 310 ubxNavPVT.hAcc |= (buf[index++] << 16);
pmic 62:c9571e4d9005 311 ubxNavPVT.hAcc |= (buf[index++] << 24);
pmic 62:c9571e4d9005 312 // uint32_t vAcc; // 44 - mm Vertical accuracy estimate
pmic 62:c9571e4d9005 313 index = UBX_PAYLOAD_INDEX + 44;
pmic 62:c9571e4d9005 314 ubxNavPVT.vAcc = buf[index++];
pmic 62:c9571e4d9005 315 ubxNavPVT.vAcc |= (buf[index++] << 8);
pmic 62:c9571e4d9005 316 ubxNavPVT.vAcc |= (buf[index++] << 16);
pmic 62:c9571e4d9005 317 ubxNavPVT.vAcc |= (buf[index++] << 24);
pmic 62:c9571e4d9005 318 // int32_t velN; // 48 - mm/s NED north velocity
pmic 62:c9571e4d9005 319 index = UBX_PAYLOAD_INDEX + 48;
pmic 62:c9571e4d9005 320 ubxNavPVT.velN = buf[index++];
pmic 62:c9571e4d9005 321 ubxNavPVT.velN |= (buf[index++] << 8);
pmic 62:c9571e4d9005 322 ubxNavPVT.velN |= (buf[index++] << 16);
pmic 62:c9571e4d9005 323 ubxNavPVT.velN |= (buf[index++] << 24);
pmic 62:c9571e4d9005 324 // int32_t velE; // 52 - mm/s NED east velocity
pmic 62:c9571e4d9005 325 index = UBX_PAYLOAD_INDEX + 52;
pmic 62:c9571e4d9005 326 ubxNavPVT.velE = buf[index++];
pmic 62:c9571e4d9005 327 ubxNavPVT.velE |= (buf[index++] << 8);
pmic 62:c9571e4d9005 328 ubxNavPVT.velE |= (buf[index++] << 16);
pmic 62:c9571e4d9005 329 ubxNavPVT.velE |= (buf[index++] << 24);
pmic 62:c9571e4d9005 330 // int32_t velD; // 56 - mm/s NED down velocity
pmic 62:c9571e4d9005 331 index = UBX_PAYLOAD_INDEX + 56;
pmic 62:c9571e4d9005 332 ubxNavPVT.velD = buf[index++];
pmic 62:c9571e4d9005 333 ubxNavPVT.velD |= (buf[index++] << 8);
pmic 62:c9571e4d9005 334 ubxNavPVT.velD |= (buf[index++] << 16);
pmic 62:c9571e4d9005 335 ubxNavPVT.velD |= (buf[index++] << 24);
pmic 62:c9571e4d9005 336 // int32_t gSpeed; // 60 - mm/s Ground Speed (2-D)
pmic 62:c9571e4d9005 337 index = UBX_PAYLOAD_INDEX + 60;
pmic 62:c9571e4d9005 338 ubxNavPVT.gSpeed = buf[index++];
pmic 62:c9571e4d9005 339 ubxNavPVT.gSpeed |= (buf[index++] << 8);
pmic 62:c9571e4d9005 340 ubxNavPVT.gSpeed |= (buf[index++] << 16);
pmic 62:c9571e4d9005 341 ubxNavPVT.gSpeed |= (buf[index++] << 24);
pmic 62:c9571e4d9005 342 // int32_t headMot; // 64 1e-5 deg Heading of motion (2-D)
pmic 62:c9571e4d9005 343 index = UBX_PAYLOAD_INDEX + 64;
pmic 62:c9571e4d9005 344 ubxNavPVT.headMot = buf[index++];
pmic 62:c9571e4d9005 345 ubxNavPVT.headMot |= (buf[index++] << 8);
pmic 62:c9571e4d9005 346 ubxNavPVT.headMot |= (buf[index++] << 16);
pmic 62:c9571e4d9005 347 ubxNavPVT.headMot |= (buf[index++] << 24);
pmic 62:c9571e4d9005 348 // uint32_t sAcc; // 68 - mm/s Speed accuracy estimate
pmic 62:c9571e4d9005 349 index = UBX_PAYLOAD_INDEX + 68;
pmic 62:c9571e4d9005 350 ubxNavPVT.sAcc = buf[index++];
pmic 62:c9571e4d9005 351 ubxNavPVT.sAcc |= (buf[index++] << 8);
pmic 62:c9571e4d9005 352 ubxNavPVT.sAcc |= (buf[index++] << 16);
pmic 62:c9571e4d9005 353 ubxNavPVT.sAcc |= (buf[index++] << 24);
pmic 62:c9571e4d9005 354 // uint32_t headAcc; // 72 1e-5 deg Heading accuracy estimate (both motion and vehicle)
pmic 62:c9571e4d9005 355 index = UBX_PAYLOAD_INDEX + 72;
pmic 62:c9571e4d9005 356 ubxNavPVT.headAcc = buf[index++];
pmic 62:c9571e4d9005 357 ubxNavPVT.headAcc |= (buf[index++] << 8);
pmic 62:c9571e4d9005 358 ubxNavPVT.headAcc |= (buf[index++] << 16);
pmic 62:c9571e4d9005 359 ubxNavPVT.headAcc |= (buf[index++] << 24);
pmic 66:4057e8e5c248 360 /*
pmic 62:c9571e4d9005 361 // int16_t magDec; // 88 1e-2 deg Magnetic declination.
pmic 62:c9571e4d9005 362 index = UBX_PAYLOAD_INDEX + 88;
pmic 62:c9571e4d9005 363 ubxNavPVT.magDec = buf[index++];
pmic 62:c9571e4d9005 364 ubxNavPVT.magDec |= (buf[index++] << 8);
pmic 62:c9571e4d9005 365 // uint16_t magAcc; // 90 1e-2 deg Magnetic declination accuracy
pmic 62:c9571e4d9005 366 index = UBX_PAYLOAD_INDEX + 90;
pmic 62:c9571e4d9005 367 ubxNavPVT.magAcc = buf[index++];
pmic 62:c9571e4d9005 368 ubxNavPVT.magAcc |= (buf[index++] << 8);
pmic 66:4057e8e5c248 369 */
pmic 62:c9571e4d9005 370 return ubxNavPVT;
pmic 62:c9571e4d9005 371 }
pmic 62:c9571e4d9005 372
pmic 62:c9571e4d9005 373 void NEOM9N::sendThreadFlag()
pmic 62:c9571e4d9005 374 {
pmic 62:c9571e4d9005 375 thread.flags_set(threadFlag);
pmic 62:c9571e4d9005 376 }