gnss_operations.cpp

00001 #include "gnss_operations.h"
00002 
00003 #ifndef UBLOX_WEARABLE_FRAMEWORK
00004 #define SEND_LOGGING_MESSAGE printf
00005 #else
00006 #include "MessageView.h"
00007 #endif
00008 
00009 #define FIRST_BYTE 0x000000FF
00010 #define SECOND_BYTE 0x0000FF00
00011 #define THIRD_BYTE 0x00FF0000
00012 #define FOURTH_BYTE 0xFF000000
00013 #define RETRY 5
00014 
00015 #define EXTRACT_BYTE(INDEX, BYTE, VALUE) ((VALUE & BYTE) >> (INDEX*8))
00016 
00017 /**
00018  *
00019  * Enable UBX-NAV-PVT using UBX-CFG-MSG
00020  * @param return    SUCCESS: 1
00021  *                  FAILURE: 0
00022  */
00023 int GnssOperations::enable_ubx_nav_pvt()
00024 {
00025     int conf = RETRY;
00026     unsigned char enable_ubx_nav_pvt[]= {0x01, 0x07, 0x01};
00027     conf = RETRY;
00028     int length =0;
00029 
00030     while(conf)
00031     {
00032 
00033         length = GnssSerial::sendUbx(0x06, 0x01, enable_ubx_nav_pvt, sizeof(enable_ubx_nav_pvt));
00034         if(length >= (int)(sizeof(enable_ubx_nav_pvt) + UBX_FRAME_SIZE))
00035         {
00036             SEND_LOGGING_MESSAGE("UBX-NAV-PVT was enabled\r\n");
00037             wait(0.5);
00038             break;
00039         }
00040         else
00041         {
00042             SEND_LOGGING_MESSAGE("enabling UBX-NAV-PVT...\r\n");
00043             conf = conf - 1;
00044         }
00045     }
00046 
00047     return (conf == 0) ? 0 : 1;
00048 }
00049 
00050 int GnssOperations::enable_ubx_nav_status() {
00051     int conf = RETRY;
00052     unsigned char enable_ubx_nav_status[]= {0x01, 0x03, 0x01};
00053     conf = RETRY;
00054     int length =0;
00055 
00056     while(conf)
00057     {
00058 
00059         length = GnssSerial::sendUbx(0x06, 0x01, enable_ubx_nav_status, sizeof(enable_ubx_nav_status));
00060         if(length >= (int)(sizeof(enable_ubx_nav_status) + UBX_FRAME_SIZE))
00061         {
00062             SEND_LOGGING_MESSAGE("UBX-NAV-STATUS was enabled\r\n");
00063             wait(0.5);
00064             break;
00065         }
00066         else
00067         {
00068             SEND_LOGGING_MESSAGE("enabling UBX-NAV-STATUS...\r\n");
00069             conf = conf - 1;
00070         }
00071     }
00072 
00073     return (conf == 0) ? 0 : 1;
00074 
00075 }
00076 
00077 int GnssOperations::enable_ubx_nav_sat() {
00078     int conf = RETRY;
00079     unsigned char enable_ubx_nav_sat[]= {0x01, 0x35, 0x01};
00080     conf = RETRY;
00081     int length =0;
00082 
00083     while(conf)
00084     {
00085 
00086         length = GnssSerial::sendUbx(0x06, 0x01, enable_ubx_nav_sat, sizeof(enable_ubx_nav_sat));
00087         if(length >= (int)(sizeof(enable_ubx_nav_sat) + UBX_FRAME_SIZE))
00088         {
00089             SEND_LOGGING_MESSAGE("UBX-NAV-STATUS was enabled\r\n");
00090             wait(0.5);
00091             break;
00092         }
00093         else
00094         {
00095             SEND_LOGGING_MESSAGE("enabling UBX-NAV-STATUS...\r\n");
00096             conf = conf - 1;
00097         }
00098     }
00099 
00100     return (conf == 0) ? 0 : 1;
00101 
00102 }
00103 
00104 int GnssOperations::enable_ubx_nav_sol() {
00105     int conf = RETRY;
00106     unsigned char enable_ubx_nav_status[]= {0x01, 0x06, 0x0A};
00107     conf = RETRY;
00108     int length =0;
00109 
00110     while(conf)
00111     {
00112 
00113         length = GnssSerial::sendUbx(0x06, 0x01, enable_ubx_nav_status, sizeof(enable_ubx_nav_status));
00114         if(length >= (int)(sizeof(enable_ubx_nav_status) + UBX_FRAME_SIZE))
00115         {
00116             SEND_LOGGING_MESSAGE("UBX-NAV-STATUS was enabled\r\n");
00117             wait(0.5);
00118             break;
00119         }
00120         else
00121         {
00122             SEND_LOGGING_MESSAGE("enabling UBX-NAV-STATUS...\r\n");
00123             conf = conf - 1;
00124         }
00125     }
00126 
00127     return (conf == 0) ? 0 : 1;
00128 
00129 }
00130 
00131 
00132 /**
00133  *
00134  * Disable UBX-NAV-PVT
00135  * @param return    SUCCESS: 1
00136  *                  FAILURE: 0
00137  */
00138 int GnssOperations::disable_ubx_nav_pvt()
00139 {
00140     int conf = RETRY;
00141     unsigned char enable_ubx_nav_pvt[]= {0x01, 0x07, 0x00};
00142     conf = RETRY;
00143     int length =0;
00144 
00145     while(conf)
00146     {
00147 
00148         length = GnssSerial::sendUbx(0x06, 0x01, enable_ubx_nav_pvt, sizeof(enable_ubx_nav_pvt));
00149         if(length >= (int)(sizeof(enable_ubx_nav_pvt) + UBX_FRAME_SIZE))
00150         {
00151             SEND_LOGGING_MESSAGE("UBX-NAV-PVT was disabled\r\n");
00152             wait(0.5);
00153             break;
00154         }
00155         else
00156         {
00157             SEND_LOGGING_MESSAGE("disabling UBX-NAV-PVT...\r\n");
00158             conf = conf - 1;
00159         }
00160     }
00161 
00162     return (conf == 0) ? 0 : 1;
00163 }
00164 
00165 int GnssOperations::enable_ubx_nav5(unsigned int acc)
00166 {
00167     int conf = RETRY;
00168     conf = RETRY;
00169     int length =0;
00170     //convert unsigned int acc to hex
00171     //ask if positioning mask or time accuracy mask
00172     unsigned char   ubx_cfg_nav5[]= {0xFF, 0xFF, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00,
00173                                      0x0A, 0x00, 0xFA, 0x00,0xFA, 0x00, (unsigned char)EXTRACT_BYTE(0, FIRST_BYTE, acc), (unsigned char)EXTRACT_BYTE(1, SECOND_BYTE, acc),
00174                                      0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,0x00, 0x00, 0x00, 0x00
00175                                    };
00176 
00177     while(conf)
00178     {
00179         length = GnssSerial::sendUbx(0x06, 0x24, ubx_cfg_nav5, sizeof(ubx_cfg_nav5));
00180         if(length >= (int)(sizeof(ubx_cfg_nav5) + UBX_FRAME_SIZE))
00181         {
00182             SEND_LOGGING_MESSAGE("ubx_cfg_nav5 was enabled\r\n");
00183             wait(0.5);
00184             break;
00185         }
00186         else
00187         {
00188             SEND_LOGGING_MESSAGE("enabling ubx_cfg_nav5...\r\n");
00189             conf = conf - 1;
00190         }
00191     }
00192 
00193     return (conf == 0) ? 0 : 1;
00194 }
00195 
00196 int GnssOperations::enable_ubx_navx5()
00197 {
00198     int conf = RETRY;
00199     conf = RETRY;
00200     int length =0;
00201     //convert unsigned int acc to hex
00202     //ask if positioning mask or time accuracy mask
00203     unsigned char   ubx_cfg_navx5[]= {0x28, 0x00, 0x02, 0x00, 0xFF, 0xFF, 0xFF, 0x02, 0x00, 0x00, 0x03, 0x02, 0x03, 0x20, 0x06, 0x00, 0x01, 0x01, 0x00, 0x00, 0x90,
00204                                       0x07, 0x00, 0x01, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x02, 0x64, 0x64, 0x00, 0x00, 0x01, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF9, 0xF7
00205                                      };
00206 
00207     while(conf)
00208     {
00209         length = GnssSerial::sendUbx(0x06, 0x23, ubx_cfg_navx5, sizeof(ubx_cfg_navx5));
00210         if(length >= (int)(sizeof(ubx_cfg_navx5) + UBX_FRAME_SIZE))
00211         {
00212             SEND_LOGGING_MESSAGE("ubx_cfg_navx5 was enabled\r\n");
00213             wait(0.5);
00214             break;
00215         }
00216         else
00217         {
00218             SEND_LOGGING_MESSAGE("enabling ubx_cfg_navx5...\r\n");
00219             conf = conf - 1;
00220         }
00221     }
00222 
00223     return (conf == 0) ? 0 : 1;
00224 }
00225 
00226 /**
00227  * Enabling UBX-ODOMETER using UBX-CFG-ODO
00228  * @param return    SUCCESS: 1
00229  *                  FAILURE: 0
00230  *
00231  */
00232 int GnssOperations::enable_ubx_odo()
00233 {
00234     int conf = RETRY;
00235     unsigned char ubx_cfg_odo[]= {0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x19, 0x46, 0x19, 0x66, 0x0A, 0x32, 0x00,
00236                                   0x00, 0x99, 0x4C, 0x00, 0x00
00237                                  };
00238     conf = RETRY;
00239     int length =0;
00240 
00241     while(conf)
00242     {
00243         length = GnssSerial::sendUbx(0x06, 0x1E, ubx_cfg_odo, sizeof(ubx_cfg_odo));
00244         if(length >= (int)(sizeof(ubx_cfg_odo) + UBX_FRAME_SIZE))
00245         {
00246             SEND_LOGGING_MESSAGE("UBX-ODO was enabled\r\n");
00247             wait(0.5);
00248             break;
00249         }
00250         else
00251         {
00252             SEND_LOGGING_MESSAGE("enabling UBX-ODO...\r\n");
00253             conf = conf - 1;
00254         }
00255     }
00256 
00257     return (conf == 0) ? 0 : 1;
00258 }
00259 
00260 int GnssOperations::disable_ubx_odo()
00261 {
00262     int conf = RETRY;
00263     unsigned char ubx_cfg_odo[]= {0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x19, 0x46, 0x19, 0x66, 0x0A, 0x32, 0x00,
00264                                   0x00, 0x99, 0x4C, 0x00, 0x00
00265                                  };
00266     conf = RETRY;
00267     int length =0;
00268 
00269     while(conf)
00270     {
00271         length = GnssSerial::sendUbx(0x06, 0x1E, ubx_cfg_odo, sizeof(ubx_cfg_odo));
00272         if(length >= (int)(sizeof(ubx_cfg_odo) + UBX_FRAME_SIZE))
00273         {
00274             SEND_LOGGING_MESSAGE("UBX-ODO was disabled\r\n");
00275             wait(0.5);
00276             break;
00277         }
00278         else
00279         {
00280             SEND_LOGGING_MESSAGE("disabling UBX-ODO...\r\n");
00281             conf = conf - 1;
00282         }
00283     }
00284 
00285     return (conf == 0) ? 0 : 1;
00286 }
00287 /**
00288  * Enabling UBX-NAV-ODO messages using UBX-CFG-MSG
00289  * @param return    SUCCESS: 1
00290  *                  FAILURE: 0
00291  *
00292  */
00293 int GnssOperations::enable_ubx_nav_odo()
00294 {
00295     int conf = RETRY;
00296     unsigned char ubx_nav_odo[]= {0x01, 0x09, 0x01};
00297     conf = RETRY;
00298     int length =0;
00299 
00300     while(conf)
00301     {
00302         length = GnssSerial::sendUbx(0x06, 0x01, ubx_nav_odo, sizeof(ubx_nav_odo));
00303         if(length >= (int)(sizeof(ubx_nav_odo) + UBX_FRAME_SIZE))
00304         {
00305             SEND_LOGGING_MESSAGE("UBX-NAV-ODO was enabled\r\n");
00306             wait(0.5);
00307             break;
00308         }
00309         else
00310         {
00311             SEND_LOGGING_MESSAGE("enabling UBX-NAV-ODO...\r\n");
00312             conf = conf - 1;
00313         }
00314     }
00315 
00316     return (conf == 0) ? 0 : 1;
00317 }
00318 
00319 /**
00320  * Disabling UBX-NAV-ODO messages using UBX-CFG-MSG
00321  * @param return    SUCCESS: 1
00322  *                  FAILURE: 0
00323  *
00324  */
00325 int GnssOperations::disable_ubx_nav_odo()
00326 {
00327     int conf = RETRY;
00328     unsigned char ubx_nav_odo[]= {0x01, 0x09, 0x00};
00329     conf = RETRY;
00330     int length =0;
00331 
00332     while(conf)
00333     {
00334         length = GnssSerial::sendUbx(0x06, 0x01, ubx_nav_odo, sizeof(ubx_nav_odo));
00335         if(length >= (int)(sizeof(ubx_nav_odo) + UBX_FRAME_SIZE))
00336         {
00337             SEND_LOGGING_MESSAGE("UBX-NAV-ODO was disabled\r\n");
00338             wait(0.5);
00339             break;
00340         }
00341         else
00342         {
00343             SEND_LOGGING_MESSAGE("disabling UBX-NAV-ODO...\r\n");
00344             conf = conf - 1;
00345         }
00346     }
00347 
00348     return (conf == 0) ? 0 : 1;
00349 }
00350 
00351 int GnssOperations::enable_ubx_batch_feature()
00352 {
00353     int conf = RETRY;
00354     unsigned char enable_ubx_log_batch[]= {0x00, 0x0D, 0x0A, 0x00, 0x07, 0x00, 0x00, 0x01};
00355     conf = RETRY;
00356     int length =0;
00357 
00358     //Disable NAV-ODO and NAV-PVT
00359     disable_ubx_nav_odo();
00360     disable_ubx_nav_pvt();
00361 
00362     while(conf)
00363     {
00364         length = GnssSerial::sendUbx(0x06, 0x93, enable_ubx_log_batch, sizeof(enable_ubx_log_batch));
00365         if(length >= (int)(sizeof(enable_ubx_log_batch) + UBX_FRAME_SIZE))
00366         {
00367             SEND_LOGGING_MESSAGE("UBX_LOG_BATCH was enabled\r\n");
00368             wait(0.5);
00369             break;
00370         }
00371         else
00372         {
00373             SEND_LOGGING_MESSAGE("enable ubx_batch_log...\r\n");
00374             conf = conf - 1;
00375         }
00376     }
00377     return (conf == 0) ? 0 : 1;
00378 }
00379 
00380 int GnssOperations::disable_ubx_batch_feature()
00381 {
00382     int conf = RETRY;
00383     unsigned char enable_ubx_log_batch[]= {0x00, 0x0C, 0x0A, 0x00, 0x07, 0x00, 0x00, 0x01};
00384     conf = RETRY;
00385     int length =0;
00386 
00387     //Enable NAV-ODO and NAV-PVT
00388     enable_ubx_nav_odo();
00389     enable_ubx_nav_pvt();
00390 
00391     while(conf)
00392     {
00393         length = GnssSerial::sendUbx(0x06, 0x93, enable_ubx_log_batch, sizeof(enable_ubx_log_batch));
00394         if(length >= (int)(sizeof(enable_ubx_log_batch) + UBX_FRAME_SIZE))
00395         {
00396             SEND_LOGGING_MESSAGE("UBX_LOG_BATCH was enabled\r\n");
00397             wait(0.5);
00398             break;
00399         }
00400         else
00401         {
00402             SEND_LOGGING_MESSAGE("enable ubx_batch_log...\r\n");
00403             conf = conf - 1;
00404         }
00405     }
00406     return (conf == 0) ? 0 : 1;
00407 }
00408 
00409 /**
00410  *
00411  * Configuring UBX-LOG-BATCH with UBX-CFG-BATCH
00412  *
00413  * @param obj struct containing the data to be send in payload
00414  * @param return    SUCCESS: 1
00415  *                  FAIL:    0
00416  *
00417  */
00418 int GnssOperations::cfg_batch_feature(tUBX_CFG_BATCH *obj)
00419 {
00420     int length =0;
00421     const unsigned char cfg_batch_feature[] = {0x00, 0x01, (unsigned char)EXTRACT_BYTE(0, FIRST_BYTE, obj->bufSize),
00422                                                (unsigned char) EXTRACT_BYTE(1, SECOND_BYTE, obj->bufSize), (unsigned char) EXTRACT_BYTE(0, FIRST_BYTE, obj->notifThrs),
00423                                                (unsigned char) EXTRACT_BYTE(1, SECOND_BYTE, obj->notifThrs), obj->pioId, 0x00
00424                                               };
00425 
00426     length = GnssSerial::sendUbx(0x06, 0x93, cfg_batch_feature, sizeof(cfg_batch_feature));
00427 
00428     return (length >= (int)(sizeof(cfg_batch_feature) + UBX_FRAME_SIZE)) ? 1 : 0;
00429 }
00430 
00431 /*
00432  *  Power mode configuration for GNSS receiver
00433  *
00434  *  Pending: Need to send extended power management configuration messages (UBX-CFG-PM2)
00435  *
00436  *
00437  */
00438 int GnssOperations::cfg_power_mode(Powermodes power_mode, bool minimumAcqTimeZero)
00439 {
00440     int length = 0;
00441     const int minimumAcqTime_index = 22;
00442     unsigned char semi_continuous_pms[] = {0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
00443     unsigned char semi_continuous_pm2[] = {0x02, 0x06, 0x00, 0x00, 0x02, 0x00, 0x43, 0x01, 0x10, 0x27, 0x00, 0x00, 0x10,
00444                                            0x27, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2C, 0x01, 0x2C, 0x01, 0x00, 0x00, 0xCF, 0x40, 0x00,
00445                                            0x00, 0x87, 0x5A, 0xA4, 0x46, 0xFE, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
00446                                           };
00447     unsigned char semi_continuous_rate[] = {0xE8, 0x03, 0x01, 0x00, 0x01, 0x00};
00448 
00449     unsigned char aggresive_continuous_pms[] = {0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
00450     unsigned char aggresive_continuous_pm2[] = {0x02, 0x06, 0x00, 0x00, 0x02, 0x00, 0x43, 0x01, 0xE8, 0x03, 0x00, 0x00,
00451                                                 0x10, 0x27, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2C, 0x01, 0x2C, 0x01, 0x00, 0x00, 0xCF, 0x40,
00452                                                 0x00, 0x00, 0x87, 0x5A, 0xA4, 0x46, 0xFE, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
00453                                                };
00454     unsigned char aggressive_continuous_rate[] = {0xE8, 0x03, 0x01, 0x00, 0x01, 0x00};
00455 
00456     unsigned char conservative_continuous_pms[] = {0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
00457     unsigned char conservative_continuous_pm2[] = {0x02, 0x06, 0x00, 0x00, 0x00, 0x00, 0x43, 0x01, 0xE8, 0x03, 0x00, 0x00,
00458                                                    0x10, 0x27, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2C, 0x01, 0x2C, 0x01, 0x00, 0x00, 0xCF, 0x41,
00459                                                    0x00, 0x00, 0x88, 0x6A, 0xA4, 0x46, 0xFE, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
00460                                                   };
00461     unsigned char conservative_continuous_rate[] = {0xE8, 0x03, 0x01, 0x00, 0x01, 0x00};
00462 
00463     unsigned char full_power_pms[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
00464     unsigned char full_power_rate[] = {0xE8, 0x03, 0x01, 0x00, 0x01, 0x00};
00465 
00466     unsigned char full_power_block_level_pms[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
00467     unsigned char full_power_block_level_rate[] = {0xE8, 0x03, 0x01, 0x00, 0x01, 0x00};
00468 
00469     unsigned char full_power_building_level_pms[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
00470     unsigned char full_power_building_level_rate[] = {0xE8, 0x03, 0x01, 0x00, 0x01, 0x00};
00471 
00472     switch (power_mode)
00473     {
00474     case SEMI_CONTINOUS:
00475         SEND_LOGGING_MESSAGE("Configuring SEMI_CONTINOUS");
00476         length = GnssSerial::sendUbx(0x06, 0x86, semi_continuous_pms, sizeof(semi_continuous_pms));
00477         wait(0.5);
00478 
00479         if(minimumAcqTimeZero) {
00480             semi_continuous_pm2[minimumAcqTime_index] = 0x00;
00481             semi_continuous_pm2[minimumAcqTime_index + 1] = 0x00;
00482         }
00483 
00484         length = GnssSerial::sendUbx(0x06, 0x3B, semi_continuous_pm2, sizeof(semi_continuous_pm2));
00485         wait(0.5);
00486         length = GnssSerial::sendUbx(0x06, 0x08, semi_continuous_rate, sizeof(semi_continuous_rate));
00487         wait(0.5);
00488         break;
00489 
00490     case AGGRESSIVE_CONTINUOS:
00491         SEND_LOGGING_MESSAGE("Configuring AGGRESSIVE_CONTINUOS");
00492         length = GnssSerial::sendUbx(0x06, 0x86, aggresive_continuous_pms, sizeof(aggresive_continuous_pms));
00493         wait(0.5);
00494 
00495         if(minimumAcqTimeZero) {
00496             semi_continuous_pm2[minimumAcqTime_index] = 0x00;
00497             semi_continuous_pm2[minimumAcqTime_index + 1] = 0x00;
00498         }
00499 
00500         length = GnssSerial::sendUbx(0x06, 0x3B, aggresive_continuous_pm2, sizeof(aggresive_continuous_pm2));
00501         wait(0.5);
00502         length = GnssSerial::sendUbx(0x06, 0x08, aggressive_continuous_rate, sizeof(aggressive_continuous_rate));
00503         wait(0.5);
00504         break;
00505 
00506     case CONSERVATIVE_CONTINOUS:
00507         SEND_LOGGING_MESSAGE("Configuring CONSERVATIVE_CONTINOUS");
00508         length = GnssSerial::sendUbx(0x06, 0x86, conservative_continuous_pms, sizeof(conservative_continuous_pms));
00509         wait(0.5);
00510 
00511         if(minimumAcqTimeZero) {
00512             semi_continuous_pm2[minimumAcqTime_index] = 0x00;
00513             semi_continuous_pm2[minimumAcqTime_index + 1] = 0x00;
00514         }
00515 
00516         length = GnssSerial::sendUbx(0x06, 0x3B, conservative_continuous_pm2, sizeof(conservative_continuous_pm2));
00517         wait(0.5);
00518         length = GnssSerial::sendUbx(0x06, 0x08, conservative_continuous_rate, sizeof(conservative_continuous_rate));
00519         wait(0.5);
00520         break;
00521 
00522     case FULL_POWER:
00523         SEND_LOGGING_MESSAGE("Configuring FULL_POWER");
00524         length = GnssSerial::sendUbx(0x06, 0x86, full_power_pms, sizeof(full_power_pms));
00525         wait(0.5);
00526         length = GnssSerial::sendUbx(0x06, 0x08, full_power_rate, sizeof(full_power_rate));
00527         wait(0.5);
00528         break;
00529     case FULL_POWER_BLOCK_LEVEL:
00530         SEND_LOGGING_MESSAGE("Configuring FULL_POWER_BLOCK_LEVEL");
00531         length = GnssSerial::sendUbx(0x06, 0x86, full_power_block_level_pms, sizeof(full_power_block_level_pms));
00532         wait(0.5);
00533         length = GnssSerial::sendUbx(0x06, 0x08, full_power_block_level_rate, sizeof(full_power_block_level_rate));
00534         wait(0.5);
00535         break;
00536     case FULL_POWER_BUILDING_LEVEL:
00537         SEND_LOGGING_MESSAGE("Configuring FULL_POWER_BUILDING_LEVEL");
00538         length = GnssSerial::sendUbx(0x06, 0x86, full_power_building_level_pms, sizeof(full_power_building_level_pms));
00539         wait(0.5);
00540         length = GnssSerial::sendUbx(0x06, 0x08, full_power_building_level_rate, sizeof(full_power_building_level_rate));
00541         wait(0.5);
00542         break;
00543     case AVAILABLE_OPERATION:
00544     default : {
00545         SEND_LOGGING_MESSAGE("Invalid power mode");
00546     }
00547     break;
00548     }
00549 
00550     return (length >= (int)(sizeof(semi_continuous_pms) + UBX_FRAME_SIZE)) ? 1 : 0;
00551 }
00552 
00553 bool GnssOperations::verify_gnss_mode() {
00554 
00555     unsigned char CFG_PMS[] = {0xB5, 0x62, 0x06, 0x86, 0x00, 0x00, 0x8c, 0xAA};
00556     unsigned char CFG_PM2[] = {0xB5, 0x62, 0x06, 0x3B, 0x00, 0x00, 0x41, 0xC9};
00557     unsigned char CFG_RATE[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x0E, 0x30};
00558     unsigned char CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00, 0x2A, 0x84};
00559     unsigned char CFG_NAVX5[] = {0xB5, 0x62, 0x06, 0x23, 0x00, 0x00, 0x29, 0x81};
00560 
00561     this->_send(CFG_PMS, sizeof(CFG_PMS));
00562     wait(0.5);
00563 
00564     this->_send(CFG_PM2, sizeof(CFG_PM2));
00565     wait(0.5);
00566 
00567     this->_send(CFG_RATE, sizeof(CFG_RATE));
00568     wait(0.5);
00569 
00570     this->_send(CFG_NAV5, sizeof(CFG_NAV5));
00571     wait(0.5);
00572 
00573     this->_send(CFG_NAVX5, sizeof(CFG_NAVX5));
00574     wait(0.5);
00575 
00576     return true;
00577 }
00578 
00579 /**
00580  *  GNSS start modes (Hot/Warm/Cold start)
00581  *
00582  *  @param return   SUCCESS: 1
00583  *                  FAILURE:    0
00584  *
00585  */
00586 int GnssOperations::start_mode(int start_mode)
00587 {
00588     int length = 0;
00589     unsigned char hot_start[] = {0x00, 0x00, 0x02, 0x00};
00590     unsigned char warm_start[] = {0x01, 0x00, 0x02, 0x00};
00591     unsigned char cold_start[] = {0xFF, 0xFF, 0x02, 0x00};
00592 
00593     switch (start_mode)
00594     {
00595     case HOT:
00596         length = GnssSerial::sendUbx(0x06, 0x04, hot_start, sizeof(hot_start));
00597         break;
00598 
00599     case WARM:
00600         length = GnssSerial::sendUbx(0x06, 0x04, warm_start, sizeof(warm_start));
00601         break;
00602 
00603     case COLD:
00604         length = GnssSerial::sendUbx(0x06, 0x04, cold_start, sizeof(cold_start));
00605         break;
00606     }
00607 
00608     return (length >= (int)(sizeof(hot_start) + UBX_FRAME_SIZE)) ? 1 : 0;
00609 }
00610 
00611 void GnssOperations::send_to_gnss(char rChar)
00612 {
00613     GnssSerial::putc(rChar);
00614 }
00615 
00616 void GnssOperations::power_on_gnss()
00617 {
00618     GnssSerial::_powerOn();
00619 }