Diff: Sensors.cpp

Revision:

0:ea630d259217

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors.cpp	Fri Dec 27 16:01:26 2019 +0000
@@ -0,0 +1,166 @@
+#include "Sensors.h"
+
+using namespace std;
+
+void init_sensors(GPS_I2C &myGPS_I2C, LSM9DS1 &myIMU)
+{
+    /*    if (myGPS_I2C.begin() == false) printf("Module failed to respond. Please check wiring. \r\n");
+        else                            printf("GPS module found!\r\n");
+
+        if (myIMU.begin() == false)     printf("Failed to communicate with LSM9DS1.\r\n");
+        else                            printf("LSM9DS1 found.\r\n");*/
+
+    myGPS_I2C.begin();
+
+    myIMU.begin();
+    myIMU.calibrate();
+}
+
+void read_imu(LSM9DS1 &myIMU)
+{
+    if (myIMU.gyroAvailable()) {
+        myIMU.readGyro();
+        //printf("gyro: %d %d %d\n\r" , myIMU.gx, myIMU.gy, myIMU.gz);
+    }
+
+    if (myIMU.magAvailable()) {
+        myIMU.readMag();
+        //printf("mag: %d %d %d\n\r", myIMU.mx, myIMU.my, myIMU.mz);
+    }
+
+    if (myIMU.tempAvailable()) {
+        myIMU.readTemp();
+        //printf("temp: %d \n\r"    , myIMU.temperature);
+    }
+
+    if (myIMU.accelAvailable()) {
+        myIMU.readAccel();
+        //printf("accel: %d %d %d\n\n\r", myIMU.ax, myIMU.ay, myIMU.az);
+    }
+}
+
+
+void is_available(Is_sensor& is_sensor, LSM9DS1 &myIMU)
+{
+    is_sensor.gyro  = myIMU.gyroAvailable();
+    is_sensor.mag   = myIMU.magAvailable();
+    is_sensor.temp  = myIMU.tempAvailable();
+    is_sensor.accel = myIMU.accelAvailable();
+    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);
+}
+
+
+void read_gps(GPS_I2C &myGPS_I2C, Gps_info &info)
+{
+    char    sentences[2048];
+    char    rmc_sentence[2048];
+    int     char_number = 0;
+
+    while (myGPS_I2C.available()) { //available() returns the number of new bytes available from the GPS module
+        char incoming = myGPS_I2C.read(); //Read the latest byte from Qwiic GPS
+        if(incoming == '$') {
+            sentences[char_number] = '\n';
+            char_number++;
+            sentences[char_number] = '\r';
+
+        }
+        sentences[char_number] = incoming;
+        char_number++;
+    }
+
+    search_word(sentences, rmc_sentence);
+
+    //GNRMC Parsing
+    //Gps_info rmc;
+    RMC_parsing(rmc_sentence, info);
+    /*    cout << "time = "           << rmc.time << "\r\n";
+        cout << "status = "         << rmc.status << "\r\n";
+        cout << "latitude = "       << rmc.latitude << "\r\n";
+        cout << "N/S indicator = "  << rmc.NS_indicator << "\r\n";
+        cout << "longitude = "      << rmc.longitude << "\r\n";
+        cout << "E/W indicator = "  << rmc.EW_indicator << "\r\n";
+        cout << "Speed = "          << rmc.speed << "\r\n";
+        cout << "Course = "         << rmc.course << "\r\n";
+        cout << "Date = "           << rmc.date << "\r\n";
+        cout << "Mode = "           << rmc.mode << "\r\n";*/
+}
+
+
+void  search_word(char sentences[MAX_PACKET_SIZE] , char one_sentence[MAX_PACKET_SIZE])
+{
+    char *s = strstr(sentences, "$GNRMC");
+
+    if (s != NULL) {                   // if successful then s now points at "$GNRMC"
+        //printf("Found string at index = %d\r\n", s - sentences);
+        strcpy(one_sentence, s);
+        /*        printf("RMC Sentence : \r\n");
+                printf("%s", one_sentence);
+                printf("\r\n");*/
+    }                                  // index of "sentence_to_find" in buff can be found by pointer subtraction
+    else {
+        printf("String not found\r\n");  // `strstr` returns NULL if search string not found
+    }
+}
+
+void  RMC_parsing(char *rmc_sentence, Gps_info &rmc)
+{
+    size_t rmc_field=0;
+    rmc.time.clear();
+    rmc.latitude.clear();
+    rmc.longitude.clear();
+    rmc.speed.clear();
+    rmc.course.clear();
+    rmc.date.clear();
+    
+    for(size_t i=0; i < MAX_PACKET_SIZE; i++) {
+        while(rmc_sentence[i] == ',') {
+            rmc_field++;
+            i++;
+        }
+
+        switch (rmc_field) {
+            case 1:
+                rmc.time.push_back(rmc_sentence[i]);
+                break;
+
+            case 2:
+                rmc.status = rmc_sentence[i];
+                break;
+
+            case 3:
+                rmc.latitude.push_back(rmc_sentence[i]);
+                break;
+
+            case 4:
+                rmc.NS_indicator = rmc_sentence[i];
+                break;
+
+            case 5:
+                rmc.longitude.push_back(rmc_sentence[i]);
+                break;
+
+            case 6:
+                rmc.EW_indicator = rmc_sentence[i];
+                break;
+
+            case 7:
+                rmc.speed.push_back(rmc_sentence[i]);
+                break;
+
+            case 8:
+                rmc.course.push_back(rmc_sentence[i]);
+                break;
+
+            case 9:
+                rmc.date.push_back(rmc_sentence[i]);
+                break;
+
+            case 10:
+                rmc.mode = rmc_sentence[i];
+                break;
+
+            default:
+                break;
+        }
+    }
+}
\ No newline at end of file