Sensors.cpp

00001 #include "Sensors.h"
00002 
00003 using namespace std;
00004 
00005 void init_sensors(GPS_I2C &myGPS_I2C, LSM9DS1 &myIMU)
00006 {
00007     /*    if (myGPS_I2C.begin() == false) printf("Module failed to respond. Please check wiring. \r\n");
00008         else                            printf("GPS module found!\r\n");
00009 
00010         if (myIMU.begin() == false)     printf("Failed to communicate with LSM9DS1.\r\n");
00011         else                            printf("LSM9DS1 found.\r\n");*/
00012 
00013     myGPS_I2C.begin();
00014 
00015     myIMU.begin();
00016     myIMU.calibrate();
00017 }
00018 
00019 void read_imu(LSM9DS1 &myIMU)
00020 {
00021     if (myIMU.gyroAvailable()) {
00022         myIMU.readGyro();
00023         //printf("gyro: %d %d %d\n\r" , myIMU.gx, myIMU.gy, myIMU.gz);
00024     }
00025 
00026     if (myIMU.magAvailable()) {
00027         myIMU.readMag();
00028         //printf("mag: %d %d %d\n\r", myIMU.mx, myIMU.my, myIMU.mz);
00029     }
00030 
00031     if (myIMU.tempAvailable()) {
00032         myIMU.readTemp();
00033         //printf("temp: %d \n\r"    , myIMU.temperature);
00034     }
00035 
00036     if (myIMU.accelAvailable()) {
00037         myIMU.readAccel();
00038         //printf("accel: %d %d %d\n\n\r", myIMU.ax, myIMU.ay, myIMU.az);
00039     }
00040 }
00041 
00042 
00043 void is_available(Is_sensor& is_sensor, LSM9DS1 &myIMU)
00044 {
00045     is_sensor.gyro  = myIMU.gyroAvailable();
00046     is_sensor.mag   = myIMU.magAvailable();
00047     is_sensor.temp  = myIMU.tempAvailable();
00048     is_sensor.accel = myIMU.accelAvailable();
00049     printf("is_gyro : %d \r\n, is_mag : %d \r\n, is_temp : %d \r\n, is_accel : %d \r\n", is_sensor.gyro, is_sensor.mag, is_sensor.temp, is_sensor.accel);
00050 }
00051 
00052 
00053 void read_gps(GPS_I2C &myGPS_I2C, Gps_info &info)
00054 {
00055     char    sentences[2048];
00056     char    rmc_sentence[2048];
00057     int     char_number = 0;
00058 
00059     while (myGPS_I2C.available()) { //available() returns the number of new bytes available from the GPS module
00060         char incoming = myGPS_I2C.read(); //Read the latest byte from Qwiic GPS
00061         if(incoming == '$') {
00062             sentences[char_number] = '\n';
00063             char_number++;
00064             sentences[char_number] = '\r';
00065 
00066         }
00067         sentences[char_number] = incoming;
00068         char_number++;
00069     }
00070 
00071     search_word(sentences, rmc_sentence);
00072 
00073     //GNRMC Parsing
00074     //Gps_info rmc;
00075     RMC_parsing(rmc_sentence, info);
00076     /*    cout << "time = "           << rmc.time << "\r\n";
00077         cout << "status = "         << rmc.status << "\r\n";
00078         cout << "latitude = "       << rmc.latitude << "\r\n";
00079         cout << "N/S indicator = "  << rmc.NS_indicator << "\r\n";
00080         cout << "longitude = "      << rmc.longitude << "\r\n";
00081         cout << "E/W indicator = "  << rmc.EW_indicator << "\r\n";
00082         cout << "Speed = "          << rmc.speed << "\r\n";
00083         cout << "Course = "         << rmc.course << "\r\n";
00084         cout << "Date = "           << rmc.date << "\r\n";
00085         cout << "Mode = "           << rmc.mode << "\r\n";*/
00086 }
00087 
00088 
00089 void  search_word(char sentences[MAX_PACKET_SIZE] , char one_sentence[MAX_PACKET_SIZE])
00090 {
00091     char *s = strstr(sentences, "$GNRMC");
00092 
00093     if (s != NULL) {                   // if successful then s now points at "$GNRMC"
00094         //printf("Found string at index = %d\r\n", s - sentences);
00095         strcpy(one_sentence, s);
00096         /*        printf("RMC Sentence : \r\n");
00097                 printf("%s", one_sentence);
00098                 printf("\r\n");*/
00099     }                                  // index of "sentence_to_find" in buff can be found by pointer subtraction
00100     else {
00101         printf("String not found\r\n");  // `strstr` returns NULL if search string not found
00102     }
00103 }
00104 
00105 void  RMC_parsing(char *rmc_sentence, Gps_info &rmc)
00106 {
00107     size_t rmc_field=0;
00108     rmc.time.clear();
00109     rmc.latitude.clear();
00110     rmc.longitude.clear();
00111     rmc.speed.clear();
00112     rmc.course.clear();
00113     rmc.date.clear();
00114     
00115     for(size_t i=0; i < MAX_PACKET_SIZE; i++) {
00116         while(rmc_sentence[i] == ',') {
00117             rmc_field++;
00118             i++;
00119         }
00120 
00121         switch (rmc_field) {
00122             case 1:
00123                 rmc.time.push_back(rmc_sentence[i]);
00124                 break;
00125 
00126             case 2:
00127                 rmc.status = rmc_sentence[i];
00128                 break;
00129 
00130             case 3:
00131                 rmc.latitude.push_back(rmc_sentence[i]);
00132                 break;
00133 
00134             case 4:
00135                 rmc.NS_indicator = rmc_sentence[i];
00136                 break;
00137 
00138             case 5:
00139                 rmc.longitude.push_back(rmc_sentence[i]);
00140                 break;
00141 
00142             case 6:
00143                 rmc.EW_indicator = rmc_sentence[i];
00144                 break;
00145 
00146             case 7:
00147                 rmc.speed.push_back(rmc_sentence[i]);
00148                 break;
00149 
00150             case 8:
00151                 rmc.course.push_back(rmc_sentence[i]);
00152                 break;
00153 
00154             case 9:
00155                 rmc.date.push_back(rmc_sentence[i]);
00156                 break;
00157 
00158             case 10:
00159                 rmc.mode = rmc_sentence[i];
00160                 break;
00161 
00162             default:
00163                 break;
00164         }
00165     }
00166 }