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@67:1cac5dca9045, 2022-06-09 (annotated)

Committer:
pmic
Date:
Thu Jun 09 13:36:50 2022 +0200
Revision:
67:1cac5dca9045
Parent:
66:4057e8e5c248
Child:
68:0c0212d6239d
Simplified logic, leave it to the user

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