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@69:2cda20c51989, 2022-06-10 (annotated)

Committer:
pmic
Date:
Fri Jun 10 18:41:40 2022 +0200
Revision:
69:2cda20c51989
Parent:
68:0c0212d6239d
Child:
70:cfeea856d624
Final tests

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