NEOM9N_thread.cpp

00001 #include "NEOM9N_thread.h"
00002 #include <cstdint>
00003 
00004 NEOM9N::NEOM9N(PinName Tx, PinName Rx) :
00005     m_bufferedSerial(Tx, Rx),
00006     thread(osPriorityNormal, 4096)
00007 {
00008     m_bufferedSerial.set_baud(38400);
00009     m_bufferedSerial.set_format(8, BufferedSerial::None, 1);
00010     m_bufferedSerial.set_blocking(false);
00011 #if PRINT_FOR_DEBUG
00012     m_run_timer.start();
00013 #endif
00014     m_pos_ecef_0.setZero();
00015     m_pos_ecef.setZero();
00016     m_pos_enu.setZero();
00017     m_R_ecefToLocal_0.setIdentity();
00018 }
00019 
00020 NEOM9N::~NEOM9N()
00021 {
00022     ticker.detach();
00023 }
00024 
00025 void NEOM9N::start_loop()
00026 {
00027     thread.start(callback(this, &NEOM9N::update));
00028     ticker.attach(callback(this, &NEOM9N::sendThreadFlag), std::chrono::microseconds{ static_cast<long int>( 1.0e6f * m_Ts ) });
00029 }
00030 
00031 void NEOM9N::reset_local()
00032 {
00033     // copy so it can not be updated during calculations (maybe mutex this)
00034     ubxNavPVT_t ubxNavPVT = m_ubxNavPVT;
00035     m_pos_ecef_0 = transformWGS84ToECEF(ubxNavPVT);
00036     m_R_ecefToLocal_0 = getR_ECEFToLocal(ubxNavPVT);
00037     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));
00038 }
00039 
00040 NEOM9N::ubxNavPVT_t NEOM9N::GetUbxNavPVT()
00041 {
00042     return m_ubxNavPVT;
00043 }
00044 
00045 // Position in ECEF in m
00046 Eigen::Vector3f NEOM9N::GetPosECEF()
00047 {
00048     return m_pos_ecef.cast<float>();
00049 }
00050 
00051 // Position in East-North-Up in m
00052 Eigen::Vector3f NEOM9N::GetPosENU()
00053 {
00054     return m_pos_enu.cast<float>();
00055 }
00056 
00057 // Velocity in East-North-Up in m
00058 Eigen::Vector3f NEOM9N::GetVelENU()
00059 {
00060     Eigen::Vector3f vel_enu;
00061     vel_enu << static_cast<float>( m_ubxNavPVT.velE ) * 1e-3f,
00062                static_cast<float>( m_ubxNavPVT.velN ) * 1e-3f,
00063               -static_cast<float>( m_ubxNavPVT.velD ) * 1e-3f;
00064     return vel_enu;
00065 }
00066 
00067 // 20  -    -   GNSSfix Type
00068 uint8_t NEOM9N::GetFixType()
00069 {
00070     return m_ubxNavPVT.fixType;
00071 }
00072 
00073 // 23  -    -   Number of satellites used in Nav Solution
00074 uint8_t NEOM9N::GetNumSV()
00075 {
00076     return m_ubxNavPVT.numSV;
00077 }
00078 
00079 //  0  -   ms   GPS time of week of the navigation epoch
00080 uint8_t NEOM9N::GetGPSTimeOfWeek()
00081 {
00082     return m_ubxNavPVT.iTOW;
00083 }
00084 
00085 // Heading of motion (2-D) in rad wrapped to (-pi, pi)
00086 float NEOM9N::GetHeadMot()
00087 {
00088     float headMot = static_cast<float>( m_ubxNavPVT.headMot )  * 1e-5f * M_PI / 180.0f;
00089     return atan2f( sinf(headMot), cosf(headMot) );
00090 }
00091 
00092 // 72 Heading accuracy estimate in rad (both motion and vehicle)
00093 float NEOM9N::GetHeadAcc()
00094 {
00095     return static_cast<float>( m_ubxNavPVT.headAcc )  * 1e-5f * M_PI / 180.0f;
00096 }
00097 
00098 // 40  Horizontal accuracy estimate in m
00099 float NEOM9N::GethAcc()
00100 {
00101     return static_cast<float>( m_ubxNavPVT.hAcc )  * 1e-3f;
00102 }
00103 
00104 // 44  Vertical accuracy estimate in m
00105 float NEOM9N::GetvAcc()
00106 {
00107     return static_cast<float>( m_ubxNavPVT.vAcc )  * 1e-3f;
00108 }
00109 
00110 // 68 Speed accuracy estimate in m/s
00111 float NEOM9N::GetsAcc()
00112 {
00113     return static_cast<float>( m_ubxNavPVT.sAcc )  * 1e-3f;
00114 }
00115 
00116 void NEOM9N::update()
00117 {
00118     static char buffer[256];                // buffer to readout from the SerialBuffer
00119     static char check_buffer[4];            // buffer to check if a seuenz is starting
00120     const static int msg_buffer_len = 100;  // message length 94 + 2 (UBX_PAYLOAD_INDEX), do not know why +2, do not care
00121     static char msg_buffer[msg_buffer_len]; // buffer to decode message
00122     // ? | ? | PAYLOAD 92 | CK_A | CK_B
00123     static uint8_t msg_buffer_cntr = 0;
00124     static bool is_msg_sequenz = false;
00125 
00126     while(true) {
00127 
00128         ThisThread::flags_wait_any(threadFlag);
00129 
00130         if (m_bufferedSerial.readable()) {
00131 
00132             uint32_t read_len = m_bufferedSerial.read(buffer, sizeof(buffer));
00133             for (uint32_t i = 0; i < read_len; i++) {
00134                 if (is_msg_sequenz) {
00135                     msg_buffer[msg_buffer_cntr++] = buffer[i];
00136                     if (msg_buffer_cntr == msg_buffer_len) { 
00137                         // 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
00138                         uint8_t CK_A = 0x08; // 0x01 + 0x07
00139                         uint8_t CK_B = 0x09; // 0x01 + 0x01 + 0x07;
00140                         for (int i = 0; i < msg_buffer_len - 2 - 4; i++) {
00141                             CK_A += msg_buffer[i];
00142                             CK_B += CK_A;
00143                         }
00144                         // proceed if checksum is valid
00145                         if (CK_A == msg_buffer[msg_buffer_len - 2 - 4] && CK_B == msg_buffer[msg_buffer_len - 1 - 4]) {
00146                             ubxNavPVT_t ubxNavPVT = decodeUbxNavPVTmsg(msg_buffer);
00147                             // update fixType and numSV always
00148                             m_ubxNavPVT.fixType = ubxNavPVT.fixType;
00149                             m_ubxNavPVT.numSV = ubxNavPVT.numSV;
00150                             // update if fixtype 3 and numSV sufficent
00151                             if(ubxNavPVT.fixType == 3 && ubxNavPVT.numSV >= M_MIN_SATS) {
00152                                 m_ubxNavPVT = ubxNavPVT;
00153                                 static bool is_reset_local_internal = false;
00154                                 if (!is_reset_local_internal) {
00155                                     reset_local();
00156                                     is_reset_local_internal = true;
00157                                 }
00158                                 m_pos_ecef = transformWGS84ToECEF(m_ubxNavPVT);
00159                                 m_pos_enu = m_R_ecefToLocal_0 * ( m_pos_ecef - m_pos_ecef_0 );
00160                             }
00161                             is_msg_sequenz = false;
00162                             // HACK: update float sens_GPS[13];     // pos_enu(3), vel_enu(3), headMot, numSV, hAcc, vAcc, sAcc, headAcc, timestamp
00163                             /*
00164                             data.sens_GPS[0] = m_pos_enu(0);
00165                             data.sens_GPS[1] = m_pos_enu(1);
00166                             data.sens_GPS[2] = m_pos_enu(2);
00167                             Eigen::Vector3f vel_enu = GetVelENU();
00168                             data.sens_GPS[3] = vel_enu(0);
00169                             data.sens_GPS[4] = vel_enu(1);
00170                             data.sens_GPS[5] = vel_enu(2);
00171                             data.sens_GPS[6] = GetHeadMot();
00172                             data.sens_GPS[7] = (float)GetNumSV();
00173                             data.sens_GPS[8] = GethAcc();
00174                             data.sens_GPS[9] = GetvAcc();
00175                             data.sens_GPS[10] = GetsAcc();
00176                             data.sens_GPS[11] = GetHeadAcc();
00177                             data.sens_GPS[12] = global_timer.read();
00178                             */
00179 #if PRINT_FOR_DEBUG
00180                             printf("%d, %d, %d, %d\r\n", ubxNavPVT.numSV, ubxNavPVT.lon, ubxNavPVT.lat, ubxNavPVT.height);
00181 #endif                  
00182                         }
00183                     }
00184                 }
00185                 // shift past readings and check if a message starts
00186                 check_buffer[3] = check_buffer[2]; // UBX_PVT_HEADER_0: 0xB5
00187                 check_buffer[2] = check_buffer[1]; // UBX_PVT_HEADER_1: 0x62
00188                 check_buffer[1] = check_buffer[0]; // UBX_PVT_CLASS   : 0x01
00189                 check_buffer[0] = buffer[i];       // UBX_PVT_ID      : 0x07
00190                 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) {
00191 #if PRINT_FOR_DEBUG
00192                     int run_time = std::chrono::duration_cast<std::chrono::microseconds>(m_run_timer.elapsed_time()).count();
00193                     m_run_timer.reset();
00194                     printf("Time since last received package: %d, MSG len: %d\r\n", run_time, msg_buffer_cntr);
00195 #endif
00196                     is_msg_sequenz = true;
00197                     msg_buffer_cntr = 0;
00198                 }
00199             }
00200         }
00201     }
00202 }
00203 
00204 Eigen::Vector3d NEOM9N::transformWGS84ToECEF(const ubxNavPVT_t& ubxNavPVT)
00205 {
00206     // persistent within this function
00207     const static double a = 6378137.0;         // WGS | 84 Earth semimajor axis
00208     const static double e = 8.1819191 * 1e-2;  // eccentricity
00209 
00210     double lon = static_cast<double>( ubxNavPVT.lon ) * 1e-7 * M_PI / 180.0;
00211     double lat = static_cast<double>( ubxNavPVT.lat ) * 1e-7 * M_PI / 180.0;
00212     double h   = static_cast<double>( ubxNavPVT.height ) * 1e-3;
00213 
00214     double sin_lat = sin(lat);
00215     double cos_lat = cos(lat);
00216     double N = a / sqrt(1 - e * e * sin_lat * sin_lat);
00217 
00218     Eigen::Vector3d pos_ecef;
00219     pos_ecef << (h + N) * cos_lat * cos(lon),
00220                 (h + N) * cos_lat * sin(lon),
00221                 (h + (1.0 - e*e)*N) * sin_lat;
00222     return pos_ecef;
00223 }
00224 
00225 Eigen::Matrix3d NEOM9N::getR_ECEFToLocal(const ubxNavPVT_t& ubxNavPVT)
00226 {
00227     double lon = static_cast<double>(ubxNavPVT.lon) * 1e-7 * M_PI / 180.0;
00228     double lat = static_cast<double>(ubxNavPVT.lat) * 1e-7 * M_PI / 180.0;
00229 
00230     double sin_lat = sin(lat);
00231     double cos_lat = cos(lat);
00232     double sin_lon = sin(lon);
00233     double cos_lon = cos(lon);
00234 
00235     Eigen::Matrix3d R_ECEFToLocal;
00236     R_ECEFToLocal <<         -sin_lon,          cos_lon,       0,
00237                      -cos_lon*sin_lat, -sin_lat*sin_lon, cos_lat,
00238                       cos_lat*cos_lon,  cos_lat*sin_lon, sin_lat;
00239     return R_ECEFToLocal;
00240 }
00241 
00242 NEOM9N::ubxNavPVT_t NEOM9N::decodeUbxNavPVTmsg(const char *buf)
00243 {
00244     static ubxNavPVT_t ubxNavPVT;
00245     static uint8_t index;
00246 
00247     /* example from u-center: len = 6*16+4 = 100 = 4 + 94 + 2
00248     09:41:53  0000  B5 62 01 07 5C 00 C8 72 AE 16 E6 07 06 09 09 29  µb..\.Èr®.æ....)
00249               0010  35 F3 FF FF FF FF 00 84 D7 17 00 00 A4 00 00 00  5óÿÿÿÿ..×...¤...
00250               0020  00 00 00 00 00 00 00 00 00 00 98 BD FF FF FF FF  ...........½ÿÿÿÿ
00251               0030  FF FF 00 34 F8 DF 00 00 00 00 00 00 00 00 00 00  ÿÿ.4øß..........
00252               0040  00 00 00 00 00 00 00 00 00 00 58 3E 0F 00 80 A8  ..........X>...¨
00253               0050  12 01 0F 27 00 00 EE 13 4F 2F 00 00 00 00 00 00  ...'..î.O/......
00254               0060  00 00 B9 53
00255     */
00256 
00257     // uint32_t iTOW;    //  0  -    -   GPS time of week of the navigation epoch
00258     index = UBX_PAYLOAD_INDEX + 0;
00259     ubxNavPVT.iTOW = buf[index++];
00260     ubxNavPVT.iTOW |= (buf[index++] << 8);
00261     ubxNavPVT.iTOW |= (buf[index++] << 16);
00262     ubxNavPVT.iTOW |= (buf[index++] << 24);
00263     // uint16_t year;    //  4  -    -   Year (UTC)
00264     index = UBX_PAYLOAD_INDEX + 4;
00265     ubxNavPVT.year = buf[index++];
00266     ubxNavPVT.year |= (buf[index++] << 8);
00267     // uint8_t month;    //  6  -    -   Month, range 1..12 (UTC)
00268     index = UBX_PAYLOAD_INDEX + 6;
00269     ubxNavPVT.month = buf[index++];
00270     // uint8_t day;      //  7  -    -   Day of month, range 1..31 (UTC)
00271     index = UBX_PAYLOAD_INDEX + 7;
00272     ubxNavPVT.day = buf[index++];
00273     // uint8_t hour;     //  8  -    -   Hour of day, range 0..23 (UTC)
00274     index = UBX_PAYLOAD_INDEX + 8;
00275     ubxNavPVT.hour = buf[index++];
00276     // uint8_t min;      //  9  -    -   Minute of hour, range 0..59 (UTC)
00277     index = UBX_PAYLOAD_INDEX + 9;
00278     ubxNavPVT.min = buf[index++];
00279     // uint8_t sec;      // 10  -    -   Seconds of minute, range 0..60 (UTC)
00280     index = UBX_PAYLOAD_INDEX + 10;
00281     ubxNavPVT.sec = buf[index++];
00282     // uint8_t fixType;  // 20  -    -   GNSSfix Type
00283     index = UBX_PAYLOAD_INDEX + 20;
00284     ubxNavPVT.fixType = buf[index];
00285     // uint8_t numSV;    // 23  -    -   Number of satellites used in Nav Solution
00286     index = UBX_PAYLOAD_INDEX + 23;
00287     ubxNavPVT.numSV = buf[index];
00288     // int32_t lon;      // 24 1e-7 deg  Longitude
00289     index = UBX_PAYLOAD_INDEX + 24;
00290     ubxNavPVT.lon = buf[index++];
00291     ubxNavPVT.lon |= (buf[index++] << 8);
00292     ubxNavPVT.lon |= (buf[index++] << 16);
00293     ubxNavPVT.lon |= (buf[index++] << 24);
00294     // int32_t lat;      // 28 1e-7 deg  Latitude
00295     index = UBX_PAYLOAD_INDEX + 28;
00296     ubxNavPVT.lat = buf[index++];
00297     ubxNavPVT.lat |= (buf[index++] << 8);
00298     ubxNavPVT.lat |= (buf[index++] << 16);
00299     ubxNavPVT.lat |= (buf[index++] << 24);
00300     // int32_t height;   // 32  -   mm   Height above ellipsoid
00301     index = UBX_PAYLOAD_INDEX + 32;
00302     ubxNavPVT.height = buf[index++];
00303     ubxNavPVT.height |= (buf[index++] << 8);
00304     ubxNavPVT.height |= (buf[index++] << 16);
00305     ubxNavPVT.height |= (buf[index++] << 24);
00306     // uint32_t hAcc;    // 40  -   mm   Horizontal accuracy estimate
00307     index = UBX_PAYLOAD_INDEX + 40;
00308     ubxNavPVT.hAcc = buf[index++];
00309     ubxNavPVT.hAcc |= (buf[index++] << 8);
00310     ubxNavPVT.hAcc |= (buf[index++] << 16);
00311     ubxNavPVT.hAcc |= (buf[index++] << 24);
00312     // uint32_t vAcc;    // 44  -   mm   Vertical accuracy estimate
00313     index = UBX_PAYLOAD_INDEX + 44;
00314     ubxNavPVT.vAcc = buf[index++];
00315     ubxNavPVT.vAcc |= (buf[index++] << 8);
00316     ubxNavPVT.vAcc |= (buf[index++] << 16);
00317     ubxNavPVT.vAcc |= (buf[index++] << 24);
00318     // int32_t velN;     // 48  -   mm/s NED north velocity
00319     index = UBX_PAYLOAD_INDEX + 48;
00320     ubxNavPVT.velN = buf[index++];
00321     ubxNavPVT.velN |= (buf[index++] << 8);
00322     ubxNavPVT.velN |= (buf[index++] << 16);
00323     ubxNavPVT.velN |= (buf[index++] << 24);
00324     // int32_t velE;     // 52  -   mm/s NED east velocity
00325     index = UBX_PAYLOAD_INDEX + 52;
00326     ubxNavPVT.velE = buf[index++];
00327     ubxNavPVT.velE |= (buf[index++] << 8);
00328     ubxNavPVT.velE |= (buf[index++] << 16);
00329     ubxNavPVT.velE |= (buf[index++] << 24);
00330     // int32_t velD;     // 56  -   mm/s NED down velocity
00331     index = UBX_PAYLOAD_INDEX + 56;
00332     ubxNavPVT.velD = buf[index++];
00333     ubxNavPVT.velD |= (buf[index++] << 8);
00334     ubxNavPVT.velD |= (buf[index++] << 16);
00335     ubxNavPVT.velD |= (buf[index++] << 24);
00336     // int32_t gSpeed;   // 60  -   mm/s Ground Speed (2-D)
00337     index = UBX_PAYLOAD_INDEX + 60;
00338     ubxNavPVT.gSpeed = buf[index++];
00339     ubxNavPVT.gSpeed |= (buf[index++] << 8);
00340     ubxNavPVT.gSpeed |= (buf[index++] << 16);
00341     ubxNavPVT.gSpeed |= (buf[index++] << 24);
00342     // int32_t headMot;  // 64 1e-5 deg  Heading of motion (2-D)
00343     index = UBX_PAYLOAD_INDEX + 64;
00344     ubxNavPVT.headMot = buf[index++];
00345     ubxNavPVT.headMot |= (buf[index++] << 8);
00346     ubxNavPVT.headMot |= (buf[index++] << 16);
00347     ubxNavPVT.headMot |= (buf[index++] << 24);
00348     // uint32_t sAcc;    // 68 -   mm/s Speed accuracy estimate
00349     index = UBX_PAYLOAD_INDEX + 68;
00350     ubxNavPVT.sAcc = buf[index++];
00351     ubxNavPVT.sAcc |= (buf[index++] << 8);
00352     ubxNavPVT.sAcc |= (buf[index++] << 16);
00353     ubxNavPVT.sAcc |= (buf[index++] << 24);
00354     // uint32_t headAcc; // 72 1e-5 deg  Heading accuracy estimate (both motion and vehicle)
00355     index = UBX_PAYLOAD_INDEX + 72;
00356     ubxNavPVT.headAcc = buf[index++];
00357     ubxNavPVT.headAcc |= (buf[index++] << 8);
00358     ubxNavPVT.headAcc |= (buf[index++] << 16);
00359     ubxNavPVT.headAcc |= (buf[index++] << 24);
00360     /*
00361     // int16_t magDec;   // 88 1e-2 deg  Magnetic declination.
00362     index = UBX_PAYLOAD_INDEX + 88;
00363     ubxNavPVT.magDec = buf[index++];
00364     ubxNavPVT.magDec |= (buf[index++] << 8);
00365     // uint16_t magAcc;  // 90 1e-2 deg  Magnetic declination accuracy
00366     index = UBX_PAYLOAD_INDEX + 90;
00367     ubxNavPVT.magAcc = buf[index++];
00368     ubxNavPVT.magAcc |= (buf[index++] << 8);
00369     */
00370     return ubxNavPVT;
00371 }
00372 
00373 void NEOM9N::sendThreadFlag()
00374 {
00375     thread.flags_set(threadFlag);
00376 }