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@66:4057e8e5c248, 2022-06-09 (annotated)

Committer:
pmic
Date:
Thu Jun 09 13:14:26 2022 +0200
Revision:
66:4057e8e5c248
Parent:
65:73f3639c1a0f
Child:
67:1cac5dca9045
Final checks before testing, did not manage to use checksum, just leave it for now

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 66:4057e8e5c248 6 m_Ts = 0.0025; // 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 62:c9571e4d9005 14 m_R_ecefToLocal_0.setIdentity();
pmic 62:c9571e4d9005 15 }
pmic 62:c9571e4d9005 16
pmic 62:c9571e4d9005 17 NEOM9N::~NEOM9N()
pmic 62:c9571e4d9005 18 {
pmic 62:c9571e4d9005 19 ticker.detach();
pmic 62:c9571e4d9005 20 }
pmic 62:c9571e4d9005 21
pmic 62:c9571e4d9005 22 void NEOM9N::StartThread()
pmic 62:c9571e4d9005 23 {
pmic 62:c9571e4d9005 24 thread.start(callback(this, &NEOM9N::update));
pmic 62:c9571e4d9005 25 ticker.attach(callback(this, &NEOM9N::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * m_Ts)});
pmic 62:c9571e4d9005 26 }
pmic 62:c9571e4d9005 27
pmic 62:c9571e4d9005 28 void NEOM9N::ZeroLocal()
pmic 62:c9571e4d9005 29 {
pmic 62:c9571e4d9005 30 if (m_has_fix) {
pmic 62:c9571e4d9005 31 // copy so it can not be updated during calculations
pmic 62:c9571e4d9005 32 ubxNavPVT_t ubxNavPVT = m_ubxNavPVT;
pmic 62:c9571e4d9005 33 m_pos_ecef_0 = transformWGS84ToECEF(ubxNavPVT);
pmic 62:c9571e4d9005 34 m_R_ecefToLocal_0 = getR_ECEFToLocal(ubxNavPVT);
pmic 62:c9571e4d9005 35 }
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 66:4057e8e5c248 45 if (m_has_fix) {
pmic 66:4057e8e5c248 46 if (m_has_new_data) {
pmic 66:4057e8e5c248 47 m_has_new_data = false;
pmic 66:4057e8e5c248 48 return true;
pmic 66:4057e8e5c248 49 }
pmic 66:4057e8e5c248 50 }
pmic 66:4057e8e5c248 51 return false;
pmic 66:4057e8e5c248 52 }
pmic 66:4057e8e5c248 53
pmic 62:c9571e4d9005 54 Eigen::Vector3f NEOM9N::GetPosECEF()
pmic 62:c9571e4d9005 55 {
pmic 62:c9571e4d9005 56 Eigen::Vector3d pos_ecef;
pmic 62:c9571e4d9005 57 if (m_has_fix) {
pmic 62:c9571e4d9005 58 pos_ecef = transformWGS84ToECEF(m_ubxNavPVT);
pmic 62:c9571e4d9005 59 }
pmic 62:c9571e4d9005 60 return pos_ecef.cast<float>();
pmic 62:c9571e4d9005 61 }
pmic 62:c9571e4d9005 62
pmic 62:c9571e4d9005 63 Eigen::Vector3f NEOM9N::GetPosENU()
pmic 62:c9571e4d9005 64 {
pmic 62:c9571e4d9005 65 Eigen::Vector3d pos_enu;
pmic 62:c9571e4d9005 66 if (m_has_fix) {
pmic 62:c9571e4d9005 67 pos_enu = m_R_ecefToLocal_0 * ( transformWGS84ToECEF(m_ubxNavPVT) - m_pos_ecef_0 );
pmic 62:c9571e4d9005 68 }
pmic 62:c9571e4d9005 69 return pos_enu.cast<float>();
pmic 62:c9571e4d9005 70 }
pmic 62:c9571e4d9005 71
pmic 62:c9571e4d9005 72 Eigen::Vector3f NEOM9N::GetVelENU()
pmic 62:c9571e4d9005 73 {
pmic 62:c9571e4d9005 74 Eigen::Vector3f vel_enu;
pmic 65:73f3639c1a0f 75 if (m_has_fix) {
pmic 65:73f3639c1a0f 76 vel_enu << static_cast<float>( m_ubxNavPVT.velE ) * 1e-3f,
pmic 65:73f3639c1a0f 77 static_cast<float>( m_ubxNavPVT.velN ) * 1e-3f,
pmic 65:73f3639c1a0f 78 -static_cast<float>( m_ubxNavPVT.velD ) * 1e-3f;
pmic 65:73f3639c1a0f 79 }
pmic 62:c9571e4d9005 80 return vel_enu;
pmic 62:c9571e4d9005 81 }
pmic 62:c9571e4d9005 82
pmic 62:c9571e4d9005 83 uint8_t NEOM9N::GetFixType()
pmic 62:c9571e4d9005 84 {
pmic 62:c9571e4d9005 85 return m_ubxNavPVT.fixType;
pmic 62:c9571e4d9005 86 }
pmic 62:c9571e4d9005 87
pmic 62:c9571e4d9005 88 uint8_t NEOM9N::GetNumSV()
pmic 62:c9571e4d9005 89 {
pmic 62:c9571e4d9005 90 return m_ubxNavPVT.numSV;
pmic 62:c9571e4d9005 91 }
pmic 62:c9571e4d9005 92
pmic 62:c9571e4d9005 93 void NEOM9N::update()
pmic 62:c9571e4d9005 94 {
pmic 62:c9571e4d9005 95 static char buffer[256]; // buffer to readout from the SerialBuffer
pmic 62:c9571e4d9005 96 static char check_buffer[4]; // buffer to check if a seuenz is starting
pmic 66:4057e8e5c248 97 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 98 static char msg_buffer[msg_buffer_len]; // buffer to decode message
pmic 66:4057e8e5c248 99 // ? | ? | PAYLOAD 92 | CK_A | CK_B
pmic 62:c9571e4d9005 100 static uint8_t msg_buffer_cntr = 0;
pmic 62:c9571e4d9005 101 static bool is_msg_sequenz = false;
pmic 66:4057e8e5c248 102
pmic 62:c9571e4d9005 103 while(true) {
pmic 62:c9571e4d9005 104
pmic 62:c9571e4d9005 105 ThisThread::flags_wait_any(threadFlag);
pmic 62:c9571e4d9005 106
pmic 62:c9571e4d9005 107 if (m_bufferedSerial.readable()) {
pmic 62:c9571e4d9005 108
pmic 66:4057e8e5c248 109 uint32_t read_len = m_bufferedSerial.read(buffer, sizeof(buffer));
pmic 66:4057e8e5c248 110 for (uint32_t i = 0; i < read_len; i++) {
pmic 62:c9571e4d9005 111 if (is_msg_sequenz) {
pmic 62:c9571e4d9005 112 msg_buffer[msg_buffer_cntr++] = buffer[i];
pmic 62:c9571e4d9005 113 if (msg_buffer_cntr == msg_buffer_len) {
pmic 62:c9571e4d9005 114 ubxNavPVT_t ubxNavPVT_candidate = decodeUbxNavPVTmsg(msg_buffer);
pmic 62:c9571e4d9005 115 if(ubxNavPVT_candidate.fixType == 3) {
pmic 62:c9571e4d9005 116 m_ubxNavPVT = ubxNavPVT_candidate;
pmic 62:c9571e4d9005 117 m_has_fix = true;
pmic 66:4057e8e5c248 118 m_has_new_data = true;
pmic 62:c9571e4d9005 119 } else {
pmic 62:c9571e4d9005 120 ubxNavPVT_t ubxNavPVT_empty;
pmic 62:c9571e4d9005 121 m_ubxNavPVT = ubxNavPVT_empty;
pmic 62:c9571e4d9005 122 m_has_fix = false;
pmic 66:4057e8e5c248 123 m_has_new_data = false;
pmic 62:c9571e4d9005 124 }
pmic 62:c9571e4d9005 125 is_msg_sequenz = false;
pmic 62:c9571e4d9005 126 #if PRINT_FOR_DEBUG
pmic 62:c9571e4d9005 127 printf("%d, %d, %d, %d\r\n", m_ubxNavPVT.lon, m_ubxNavPVT.lat, m_ubxNavPVT.height, m_ubxNavPVT.numSV);
pmic 62:c9571e4d9005 128 #endif
pmic 62:c9571e4d9005 129 }
pmic 62:c9571e4d9005 130 }
pmic 62:c9571e4d9005 131 // shift past readings and check if a message starts
pmic 62:c9571e4d9005 132 check_buffer[3] = check_buffer[2]; // UBX_PVT_HEADER_0
pmic 62:c9571e4d9005 133 check_buffer[2] = check_buffer[1]; // UBX_PVT_HEADER_1
pmic 62:c9571e4d9005 134 check_buffer[1] = check_buffer[0]; // UBX_PVT_CLASS
pmic 62:c9571e4d9005 135 check_buffer[0] = buffer[i]; // UBX_PVT_ID
pmic 62:c9571e4d9005 136 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 137 #if PRINT_FOR_DEBUG
pmic 62:c9571e4d9005 138 int run_time = std::chrono::duration_cast<std::chrono::microseconds>(m_run_timer.elapsed_time()).count();
pmic 62:c9571e4d9005 139 m_run_timer.reset();
pmic 66:4057e8e5c248 140 printf("Time since last received package: %d, MSG len: %d\r\n", run_time, msg_buffer_cntr);
pmic 66:4057e8e5c248 141 /*
pmic 66:4057e8e5c248 142 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 143 msg_buffer[msg_buffer_len-3],
pmic 66:4057e8e5c248 144 msg_buffer[msg_buffer_len-2],
pmic 66:4057e8e5c248 145 msg_buffer[msg_buffer_len-1]);
pmic 66:4057e8e5c248 146 */
pmic 62:c9571e4d9005 147 #endif
pmic 62:c9571e4d9005 148 is_msg_sequenz = true;
pmic 62:c9571e4d9005 149 msg_buffer_cntr = 0;
pmic 62:c9571e4d9005 150 //memset(msg_buffer, 0, sizeof(msg_buffer));
pmic 62:c9571e4d9005 151 }
pmic 62:c9571e4d9005 152 }
pmic 62:c9571e4d9005 153 }
pmic 62:c9571e4d9005 154 }
pmic 62:c9571e4d9005 155 }
pmic 62:c9571e4d9005 156
pmic 62:c9571e4d9005 157 Eigen::Vector3d NEOM9N::transformWGS84ToECEF(const ubxNavPVT_t& ubxNavPVT)
pmic 62:c9571e4d9005 158 {
pmic 62:c9571e4d9005 159 // persistent within this function
pmic 62:c9571e4d9005 160 const static double a = 6378137.0; // WGS | 84 Earth semimajor axis
pmic 62:c9571e4d9005 161 const static double e = 8.1819191 * 1e-2; // eccentricity
pmic 62:c9571e4d9005 162
pmic 62:c9571e4d9005 163 double lon = static_cast<double>( ubxNavPVT.lon ) * 1e-7 * M_PI / 180.0;
pmic 62:c9571e4d9005 164 double lat = static_cast<double>( ubxNavPVT.lat ) * 1e-7 * M_PI / 180.0;
pmic 62:c9571e4d9005 165 double h = static_cast<double>( ubxNavPVT.height ) * 1e-3;
pmic 62:c9571e4d9005 166
pmic 62:c9571e4d9005 167 double sin_lat = sin(lat);
pmic 62:c9571e4d9005 168 double cos_lat = cos(lat);
pmic 62:c9571e4d9005 169 double N = a / sqrt(1 - e * e * sin_lat * sin_lat);
pmic 62:c9571e4d9005 170
pmic 62:c9571e4d9005 171 Eigen::Vector3d pos_ecef;
pmic 62:c9571e4d9005 172 pos_ecef << (h + N) * cos_lat * cos(lon),
pmic 62:c9571e4d9005 173 (h + N) * cos_lat * sin(lon),
pmic 62:c9571e4d9005 174 (h + (1.0 - e*e)*N) * sin_lat;
pmic 62:c9571e4d9005 175 return pos_ecef;
pmic 62:c9571e4d9005 176 }
pmic 62:c9571e4d9005 177
pmic 62:c9571e4d9005 178 Eigen::Matrix3d NEOM9N::getR_ECEFToLocal(const ubxNavPVT_t& ubxNavPVT)
pmic 62:c9571e4d9005 179 {
pmic 62:c9571e4d9005 180 double lon = static_cast<double>(ubxNavPVT.lon) * 1e-7 * M_PI / 180.0;
pmic 62:c9571e4d9005 181 double lat = static_cast<double>(ubxNavPVT.lat) * 1e-7 * M_PI / 180.0;
pmic 62:c9571e4d9005 182
pmic 62:c9571e4d9005 183 double sin_lat = sin(lat);
pmic 62:c9571e4d9005 184 double cos_lat = cos(lat);
pmic 62:c9571e4d9005 185 double sin_lon = sin(lon);
pmic 62:c9571e4d9005 186 double cos_lon = cos(lon);
pmic 62:c9571e4d9005 187
pmic 62:c9571e4d9005 188 Eigen::Matrix3d R_ECEFToLocal;
pmic 62:c9571e4d9005 189 R_ECEFToLocal << -sin_lon, cos_lon, 0,
pmic 62:c9571e4d9005 190 -cos_lon*sin_lat, -sin_lat*sin_lon, cos_lat,
pmic 62:c9571e4d9005 191 cos_lat*cos_lon, cos_lat*sin_lon, sin_lat;
pmic 62:c9571e4d9005 192 return R_ECEFToLocal;
pmic 62:c9571e4d9005 193 }
pmic 62:c9571e4d9005 194
pmic 62:c9571e4d9005 195 NEOM9N::ubxNavPVT_t NEOM9N::decodeUbxNavPVTmsg(const char *buf) {
pmic 62:c9571e4d9005 196
pmic 62:c9571e4d9005 197 static ubxNavPVT_t ubxNavPVT;
pmic 62:c9571e4d9005 198 static uint8_t index;
pmic 62:c9571e4d9005 199
pmic 66:4057e8e5c248 200 /* example from u-center: len = 6*16+4 = 100 = 4 + 94 + 2
pmic 66:4057e8e5c248 201 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 202 0010 35 F3 FF FF FF FF 00 84 D7 17 00 00 A4 00 00 00 5óÿÿÿÿ..×...¤...
pmic 66:4057e8e5c248 203 0020 00 00 00 00 00 00 00 00 00 00 98 BD FF FF FF FF ...........½ÿÿÿÿ
pmic 66:4057e8e5c248 204 0030 FF FF 00 34 F8 DF 00 00 00 00 00 00 00 00 00 00 ÿÿ.4øß..........
pmic 66:4057e8e5c248 205 0040 00 00 00 00 00 00 00 00 00 00 58 3E 0F 00 80 A8 ..........X>...¨
pmic 66:4057e8e5c248 206 0050 12 01 0F 27 00 00 EE 13 4F 2F 00 00 00 00 00 00 ...'..î.O/......
pmic 66:4057e8e5c248 207 0060 00 00 B9 53
pmic 66:4057e8e5c248 208 */
pmic 66:4057e8e5c248 209
pmic 62:c9571e4d9005 210 // uint8_t fixType; // 20 - - GNSSfix Type
pmic 62:c9571e4d9005 211 index = UBX_PAYLOAD_INDEX + 20;
pmic 62:c9571e4d9005 212 ubxNavPVT.fixType = buf[index];
pmic 62:c9571e4d9005 213 // uint8_t numSV; // 23 - - Number of satellites used in Nav Solution
pmic 62:c9571e4d9005 214 index = UBX_PAYLOAD_INDEX + 23;
pmic 62:c9571e4d9005 215 ubxNavPVT.numSV = buf[index];
pmic 62:c9571e4d9005 216 // int32_t lon; // 24 1e-7 deg Longitude
pmic 62:c9571e4d9005 217 index = UBX_PAYLOAD_INDEX + 24;
pmic 62:c9571e4d9005 218 ubxNavPVT.lon = buf[index++];
pmic 62:c9571e4d9005 219 ubxNavPVT.lon |= (buf[index++] << 8);
pmic 62:c9571e4d9005 220 ubxNavPVT.lon |= (buf[index++] << 16);
pmic 62:c9571e4d9005 221 ubxNavPVT.lon |= (buf[index++] << 24);
pmic 62:c9571e4d9005 222 // int32_t lat; // 28 1e-7 deg Latitude
pmic 62:c9571e4d9005 223 index = UBX_PAYLOAD_INDEX + 28;
pmic 62:c9571e4d9005 224 ubxNavPVT.lat = buf[index++];
pmic 62:c9571e4d9005 225 ubxNavPVT.lat |= (buf[index++] << 8);
pmic 62:c9571e4d9005 226 ubxNavPVT.lat |= (buf[index++] << 16);
pmic 62:c9571e4d9005 227 ubxNavPVT.lat |= (buf[index++] << 24);
pmic 62:c9571e4d9005 228 // int32_t height; // 32 - mm Height above ellipsoid
pmic 62:c9571e4d9005 229 index = UBX_PAYLOAD_INDEX + 32;
pmic 62:c9571e4d9005 230 ubxNavPVT.height = buf[index++];
pmic 62:c9571e4d9005 231 ubxNavPVT.height |= (buf[index++] << 8);
pmic 62:c9571e4d9005 232 ubxNavPVT.height |= (buf[index++] << 16);
pmic 62:c9571e4d9005 233 ubxNavPVT.height |= (buf[index++] << 24);
pmic 62:c9571e4d9005 234 // uint32_t hAcc; // 40 - mm Horizontal accuracy estimate
pmic 62:c9571e4d9005 235 index = UBX_PAYLOAD_INDEX + 40;
pmic 62:c9571e4d9005 236 ubxNavPVT.hAcc = buf[index++];
pmic 62:c9571e4d9005 237 ubxNavPVT.hAcc |= (buf[index++] << 8);
pmic 62:c9571e4d9005 238 ubxNavPVT.hAcc |= (buf[index++] << 16);
pmic 62:c9571e4d9005 239 ubxNavPVT.hAcc |= (buf[index++] << 24);
pmic 62:c9571e4d9005 240 // uint32_t vAcc; // 44 - mm Vertical accuracy estimate
pmic 62:c9571e4d9005 241 index = UBX_PAYLOAD_INDEX + 44;
pmic 62:c9571e4d9005 242 ubxNavPVT.vAcc = buf[index++];
pmic 62:c9571e4d9005 243 ubxNavPVT.vAcc |= (buf[index++] << 8);
pmic 62:c9571e4d9005 244 ubxNavPVT.vAcc |= (buf[index++] << 16);
pmic 62:c9571e4d9005 245 ubxNavPVT.vAcc |= (buf[index++] << 24);
pmic 62:c9571e4d9005 246 // int32_t velN; // 48 - mm/s NED north velocity
pmic 62:c9571e4d9005 247 index = UBX_PAYLOAD_INDEX + 48;
pmic 62:c9571e4d9005 248 ubxNavPVT.velN = buf[index++];
pmic 62:c9571e4d9005 249 ubxNavPVT.velN |= (buf[index++] << 8);
pmic 62:c9571e4d9005 250 ubxNavPVT.velN |= (buf[index++] << 16);
pmic 62:c9571e4d9005 251 ubxNavPVT.velN |= (buf[index++] << 24);
pmic 62:c9571e4d9005 252 // int32_t velE; // 52 - mm/s NED east velocity
pmic 62:c9571e4d9005 253 index = UBX_PAYLOAD_INDEX + 52;
pmic 62:c9571e4d9005 254 ubxNavPVT.velE = buf[index++];
pmic 62:c9571e4d9005 255 ubxNavPVT.velE |= (buf[index++] << 8);
pmic 62:c9571e4d9005 256 ubxNavPVT.velE |= (buf[index++] << 16);
pmic 62:c9571e4d9005 257 ubxNavPVT.velE |= (buf[index++] << 24);
pmic 62:c9571e4d9005 258 // int32_t velD; // 56 - mm/s NED down velocity
pmic 62:c9571e4d9005 259 index = UBX_PAYLOAD_INDEX + 56;
pmic 62:c9571e4d9005 260 ubxNavPVT.velD = buf[index++];
pmic 62:c9571e4d9005 261 ubxNavPVT.velD |= (buf[index++] << 8);
pmic 62:c9571e4d9005 262 ubxNavPVT.velD |= (buf[index++] << 16);
pmic 62:c9571e4d9005 263 ubxNavPVT.velD |= (buf[index++] << 24);
pmic 62:c9571e4d9005 264 // int32_t gSpeed; // 60 - mm/s Ground Speed (2-D)
pmic 62:c9571e4d9005 265 index = UBX_PAYLOAD_INDEX + 60;
pmic 62:c9571e4d9005 266 ubxNavPVT.gSpeed = buf[index++];
pmic 62:c9571e4d9005 267 ubxNavPVT.gSpeed |= (buf[index++] << 8);
pmic 62:c9571e4d9005 268 ubxNavPVT.gSpeed |= (buf[index++] << 16);
pmic 62:c9571e4d9005 269 ubxNavPVT.gSpeed |= (buf[index++] << 24);
pmic 62:c9571e4d9005 270 // int32_t headMot; // 64 1e-5 deg Heading of motion (2-D)
pmic 62:c9571e4d9005 271 index = UBX_PAYLOAD_INDEX + 64;
pmic 62:c9571e4d9005 272 ubxNavPVT.headMot = buf[index++];
pmic 62:c9571e4d9005 273 ubxNavPVT.headMot |= (buf[index++] << 8);
pmic 62:c9571e4d9005 274 ubxNavPVT.headMot |= (buf[index++] << 16);
pmic 62:c9571e4d9005 275 ubxNavPVT.headMot |= (buf[index++] << 24);
pmic 62:c9571e4d9005 276 // uint32_t sAcc; // 68 - mm/s Speed accuracy estimate
pmic 62:c9571e4d9005 277 index = UBX_PAYLOAD_INDEX + 68;
pmic 62:c9571e4d9005 278 ubxNavPVT.sAcc = buf[index++];
pmic 62:c9571e4d9005 279 ubxNavPVT.sAcc |= (buf[index++] << 8);
pmic 62:c9571e4d9005 280 ubxNavPVT.sAcc |= (buf[index++] << 16);
pmic 62:c9571e4d9005 281 ubxNavPVT.sAcc |= (buf[index++] << 24);
pmic 62:c9571e4d9005 282 // uint32_t headAcc; // 72 1e-5 deg Heading accuracy estimate (both motion and vehicle)
pmic 62:c9571e4d9005 283 index = UBX_PAYLOAD_INDEX + 72;
pmic 62:c9571e4d9005 284 ubxNavPVT.headAcc = buf[index++];
pmic 62:c9571e4d9005 285 ubxNavPVT.headAcc |= (buf[index++] << 8);
pmic 62:c9571e4d9005 286 ubxNavPVT.headAcc |= (buf[index++] << 16);
pmic 62:c9571e4d9005 287 ubxNavPVT.headAcc |= (buf[index++] << 24);
pmic 66:4057e8e5c248 288 /*
pmic 62:c9571e4d9005 289 // int16_t magDec; // 88 1e-2 deg Magnetic declination.
pmic 62:c9571e4d9005 290 index = UBX_PAYLOAD_INDEX + 88;
pmic 62:c9571e4d9005 291 ubxNavPVT.magDec = buf[index++];
pmic 62:c9571e4d9005 292 ubxNavPVT.magDec |= (buf[index++] << 8);
pmic 62:c9571e4d9005 293 // uint16_t magAcc; // 90 1e-2 deg Magnetic declination accuracy
pmic 62:c9571e4d9005 294 index = UBX_PAYLOAD_INDEX + 90;
pmic 62:c9571e4d9005 295 ubxNavPVT.magAcc = buf[index++];
pmic 62:c9571e4d9005 296 ubxNavPVT.magAcc |= (buf[index++] << 8);
pmic 66:4057e8e5c248 297 */
pmic 62:c9571e4d9005 298
pmic 62:c9571e4d9005 299 return ubxNavPVT;
pmic 62:c9571e4d9005 300 }
pmic 62:c9571e4d9005 301
pmic 62:c9571e4d9005 302 void NEOM9N::sendThreadFlag()
pmic 62:c9571e4d9005 303 {
pmic 62:c9571e4d9005 304 thread.flags_set(threadFlag);
pmic 62:c9571e4d9005 305 }