node_device.cpp

#include <SoftwareSerial.h>
#include <Wire.h>

/* sum of ASCII codepoints in 'GGA', 'GLL', etc. */
#define GGA 207
#define GLL 223
#define GSA 219
#define GSV 240
#define RMC 226
#define VTG 241

/* Configuration constants. */
uint8_t imuAddress = 0x68; // 0b0110_1000;

const int rxPin = 9;
const int txPin = 12; // unused
const int recBtn = 8;

const int actLed = 7;
const int fixLed = 6;
const int recLed = 5;

const float accelerometer_scale = (2.0*9.81);
const float gyro_scale = 250.0;


template <typename T>
struct Vector3 {
    T x;
    T y;
    T z;

    Vector3() : x(), y(), z() {};
    Vector3(T x, T y, T z) : x(x), y(y), z(z) {};

    T magn();
    T dot(const Vector3<T> &rhs);
    
    template<typename Other>
    Vector3<T> operator+ (const Vector3<Other> &rhs);
    
    template<typename Other>
    Vector3<T> operator- (const Vector3<Other> &rhs);
    
    template<typename Other>
    Vector3<T> operator* (const Other rhs);
    
    template<typename Other>
    Vector3<T> operator/ (const Other rhs);
    
    template<typename Other>
    Vector3<T>& operator+= (const Vector3<Other> &rhs);
    
    template<typename Other>
    Vector3<T>& operator-= (const Vector3<Other> &rhs);
    
    template<typename Other>
    Vector3<T>& operator*= (const Other rhs);
    
    template<typename Other>
    Vector3<T>& operator/= (const Other rhs);
    
    void dumpToSerial();
};

template<typename T>
T Vector3<T>::magn() {
    return sqrt((x*x) + (y*y) + (z*z));
}

template<typename T>
T Vector3<T>::dot(const Vector3<T> &rhs) {
    return (
        (this->x * rhs.x) +
        (this->y * rhs.y) +
        (this->z * rhs.z)
    );
}

template <typename T>
template <typename Other>
Vector3<T> Vector3<T>::operator+(const Vector3<Other> &rhs) {
    return Vector3<T>(
        this->x + rhs.x,
        this->y + rhs.y,
        this->z + rhs.z
    );
}

template <typename T>
template <typename Other>
Vector3<T> Vector3<T>::operator-(const Vector3<Other> &rhs) {
    return Vector3<T>(
        this->x - rhs.x,
        this->y - rhs.y,
        this->z - rhs.z
    );
}

template <typename T>
template <typename Other>
Vector3<T> Vector3<T>::operator*(const Other rhs) {
    return Vector3<T>(
        this->x * rhs,
        this->y * rhs,
        this->z * rhs
    );
}

template <typename T>
template <typename Other>
Vector3<T> Vector3<T>::operator/(const Other rhs) {
    return Vector3<T>(
        this->x / rhs,
        this->y / rhs,
        this->z / rhs
    );
}

template <typename T>
template <typename Other>
Vector3<T>& Vector3<T>::operator+=(const Vector3<Other> &rhs) {
    this->x += rhs.x;
    this->y += rhs.y;
    this->z += rhs.z;
    
    return *this;
}

template <typename T>
template <typename Other>
Vector3<T>& Vector3<T>::operator-=(const Vector3<Other> &rhs) {
    this->x -= rhs.x;
    this->y -= rhs.y;
    this->z -= rhs.z;
    
    return *this;
}

template <typename T>
template <typename Other>
Vector3<T>& Vector3<T>::operator*=(const Other rhs) {
    this->x *= rhs.x;
    this->y *= rhs.y;
    this->z *= rhs.z;
    
    return *this;
}

template <typename T>
template <typename Other>
Vector3<T>& Vector3<T>::operator/=(const Other rhs) {
    this->x /= rhs.x;
    this->y /= rhs.y;
    this->z /= rhs.z;
    
    return *this;
}

template <typename T>
void Vector3<T>::dumpToSerial() {
    Serial.print("X: ");
    Serial.print(this->x);
    Serial.print(" | Y: ");
    Serial.print(this->y);
    Serial.print(" | Z: ");
    Serial.print(this->z);
}

typedef struct __attribute__ ((packed)) imuData {
    int16_t acc_x;
    int16_t acc_y; 
    int16_t acc_z;  
    int16_t temp;
    int16_t gyro_x;
    int16_t gyro_y;
    int16_t gyro_z;
} imuData;

typedef struct __attribute__ ((packed)) readingFrame {
    uint8_t size;
    uint8_t reserved[3];

    float   calib_x;
    float   calib_y;
    float   calib_z;
    
    float   acc_x;
    float   acc_y;
    float   acc_z;
    
    float   gyro_x;
    float   gyro_y;
    float   gyro_z;
    
    float   utc_seconds;
    float   latitude;
    float   longitude;
    float   altitude;
    
    /*
     * bit 0    = GPS validity flag
     * bit 1    = latitude N/S  (1 = N)
     * bit 2    = longitude E/W (1 = E)
     */
    uint8_t flags;
    
    uint8_t checksum; // XOR of all previous bytes
} readingFrame;

readingFrame curData;

/* GPS reception status */
char cur_nmea_msg[82];
int cur_recv_idx = -1;              // -1 = no message being received 
unsigned long last_msg_time = -1; 

int utc_hours = 0;
int utc_minutes = 0;
float utc_seconds = 0.0;

SoftwareSerial gpsSerial(rxPin, txPin);

/* IMU state */
imuData imuBuffer;
Vector3<float> raw_acc;
Vector3<float> raw_avel;

Vector3<float> cur_acc;
Vector3<float> cur_avel;

Vector3<float> calibration_acc;
Vector3<float> calibration_avel;
size_t calibration_count = 0;


char* nmea_value(char *cur) {
    // skip past initial commas and spaces to the next data element
    while (*cur == ',' || *cur == ' ') {
        cur++;
    }

    return cur;
}

char* next_field(char *cur) { 
    // Find the next comma or CR
    while (*cur != ',' && *cur != 0x0D) {
        cur++;
    }

    // Find the start of the next data value
    while (*cur != ' ' && *cur != ',') {
        cur++;
    }

    return cur+1;
}

float read_decimal_number(char* cur) {
    int whole = 0;
    int frac = 0;
    int n_frac_digits = 0;
    float out = 0;

    while (*cur != '.' && isDigit(*cur)) {
        whole = (whole * 10) + (*cur - '0');
        cur++;
    }

    if (*cur == 0x0D || *cur == 0x0A) {
        return (float)whole;
    }

    cur++;

    while (isDigit(*cur)) {
        frac = (frac * 10) + (*cur - '0');
        cur++;
    }

    out = (float)whole;

    if (frac > 0) {
        out += (float)frac / (float)n_frac_digits;
    }

    return out;
}

int read_digits(char *cur, int n) {
    int out = 0;

    while(n-- > 0 && isDigit(*cur)) {
        out = (out * 10) + (*cur++ - '0');
    }

    return out;
}

void print_gps_data() {
    Serial.print("UTC time: ");
    Serial.print(utc_hours);
    Serial.print(":");
    Serial.print(utc_minutes);
    Serial.print(":");
    if (utc_seconds < 10) {
        Serial.print("0");
    }
    Serial.print(utc_seconds);
    Serial.print("\n");

    Serial.print("Latitude: ");
    Serial.print(curData.latitude);
    Serial.println((curData.flags & (1<<1)) > 0 ? " N" : " S");

    Serial.print("Longitude: ");
    Serial.print(curData.longitude);
    Serial.println((curData.flags & (1<<2)) > 0 ? " E" : " W");

    Serial.print("Altitude: ");
    Serial.print(curData.altitude);
    Serial.println("m MSL");

    Serial.print("Data Valid: ");
    Serial.println((curData.flags & 1) > 0 ? "Yes" : "No");
}

void handle_gga_msg() {
    /*
     * message data in order:
     * 1. UTC time (hhmmss.sss)
     * 2. Latitude (ddmm.mmmm)
     * 3. North / South indicator (N/S)
     * 4. Longitude (dddmm.mmmm)
     * 5. East / West indicator (E/W)
     * 6. Position fix indicator
     * 7. # of satellites used
     * 8. Horizontal Dilution of Precision
     * 9. MSL altitude (m)
     */

    // read in UTC time:
    char* cur_value = next_field(cur_nmea_msg);
    utc_hours = read_digits(cur_value, 2);
    utc_minutes = read_digits(cur_value+2, 2);
    utc_seconds = read_decimal_number(cur_value+4);

    curData.flags = 0;
    curData.utc_seconds = (3600 * utc_hours) + (60 * utc_minutes) + utc_seconds;
    
    // Read latitude
    cur_value = next_field(cur_value);  
    curData.latitude = read_decimal_number(cur_value) / 100.0f;

    // Read N/S indicator
    cur_value = next_field(cur_value);
    curData.flags |= (*cur_value == 'N') ? (1<<1) : 0;

    // Read longitude
    cur_value = next_field(cur_value);
    curData.longitude = read_decimal_number(cur_value) / 1000.0f;

    // Read E/W indicator
    cur_value = next_field(cur_value);
    curData.flags |= (*cur_value == 'E') ? (1<<2) : 0;

    // read position fix validity indicator
    cur_value = next_field(cur_value);
    int fix_status = read_digits(cur_value, 1);
    curData.flags |= (fix_status > 0) ? 1 : 0;

    // skip number of satellites used
    cur_value = next_field(cur_value);

    // skip HDOP
    cur_value = next_field(cur_value);

    // read MSL altitude
    cur_value = next_field(cur_value);
    curData.altitude = read_decimal_number(cur_value);

    //Serial.println("\nReceived GGA message.");
    //print_gps_data();
}

void handle_gll_msg() {
    /*
     * message data in order:
     * 1. UTC time (hhmmss.sss)
     * 2. Latitude (ddmm.mmmm)
     * 3. North / South indicator (N/S)
     * 4. Longitude (dddmm.mmmm)
     * 5. East / West indicator (E/W)
     * 6. Position fix indicator
     * 7. # of satellites used
     * 8. Horizontal Dilution of Precision
     * 9. MSL altitude (m)
     */

    curData.flags = 0;
     
    // Read latitude
    char* cur_value = next_field(cur_nmea_msg);
    curData.latitude = read_decimal_number(cur_value) / 100.0f;

    // Read N/S indicator
    cur_value = next_field(cur_value);
    curData.flags |= (*cur_value == 'N') ? (1<<1) : 0;

    // Read longitude
    cur_value = next_field(cur_value);
    curData.longitude = read_decimal_number(cur_value) / 1000.0f;

    // Read E/W indicator
    cur_value = next_field(cur_value);
    curData.flags |= (*cur_value == 'E') ? (1<<2) : 0;

    // read UTC time
    cur_value = next_field(cur_value);
    utc_hours = read_digits(cur_value, 2);
    utc_minutes = read_digits(cur_value+2, 2);
    utc_seconds = read_decimal_number(cur_value+4);
    curData.utc_seconds = (utc_hours * 3600) + (utc_minutes * 60) + utc_seconds;

    // read fix status
    cur_value = next_field(cur_value);
    curData.flags |= (*cur_value == 'A') ? 1 : 0;

    //Serial.println("\nReceived GLL message.");
    //print_gps_data();
}

void handle_nmea_msg() {
    int ident = cur_nmea_msg[3] + cur_nmea_msg[4] + cur_nmea_msg[5];

    if (ident == GGA) {
        last_msg_time = millis();
        return handle_gga_msg();
    } else if (ident == GLL) {
        last_msg_time = millis();
        return handle_gll_msg();
    }
}

void imuRead(byte regAddr, size_t len, uint8_t* out_buf) {
    Wire.beginTransmission(imuAddress);
    Wire.write(regAddr);
    Wire.endTransmission(false);
    Wire.requestFrom(imuAddress, len);

    for (size_t i=0;i<len;i+=2) {
        // we receive values MSB-first, so we need to account for that
        out_buf[i+1] = Wire.read();
        out_buf[i]   = Wire.read();
    }
}

void imuWrite(byte regAddr, size_t len, uint8_t* in_buf) {
    Wire.beginTransmission(imuAddress);
    Wire.write(regAddr);
    Wire.write(in_buf, len);
    Wire.endTransmission(true);
}

void transmitReadingFrame(readingFrame* frame) {
    frame->size = sizeof(readingFrame);
    frame->checksum = 0;
    uint8_t* rawBytes = (uint8_t*)frame;
    
    for (size_t i=0;i<sizeof(readingFrame)-1;i++) {
        frame->checksum ^= rawBytes[i];
    }
    
    Serial.write("\xA5\x5A");
    Serial.write(rawBytes, sizeof(readingFrame));
}

void setup() {
    pinMode(rxPin, INPUT);
    pinMode(txPin, OUTPUT);
    pinMode(recBtn, INPUT);

    pinMode(actLed, OUTPUT);
    pinMode(fixLed, OUTPUT);
    pinMode(recLed, OUTPUT);

    gpsSerial.begin(9600);
    Serial.begin(9600);
    Wire.begin();
    
    uint8_t power_cmd[] = { 0x00, 0x00 };
    imuWrite(108, 2, power_cmd);
}

void loop() {
    digitalWrite(actLed, LOW);
    
    imuRead(59, 14, (uint8_t*)&imuBuffer);

    raw_acc.x = ((float)imuBuffer.acc_x / 32768.0) * accelerometer_scale;
    raw_acc.y = ((float)imuBuffer.acc_y / 32768.0) * accelerometer_scale;
    raw_acc.z = ((float)imuBuffer.acc_z / 32768.0) * accelerometer_scale;

    raw_avel.x = ((float)imuBuffer.gyro_x / 32768.0) * gyro_scale;
    raw_avel.y = ((float)imuBuffer.gyro_y / 32768.0) * gyro_scale;
    raw_avel.z = ((float)imuBuffer.gyro_z / 32768.0) * gyro_scale;

    if (millis() < 500) {
        calibration_count++;
        
        calibration_acc += (raw_acc - calibration_acc);
        calibration_avel += (raw_avel - calibration_avel) / (float)calibration_count;
        
        curData.calib_x = calibration_acc.x;
        curData.calib_y = calibration_acc.y;
        curData.calib_z = calibration_acc.z;
        
        digitalWrite(actLed, HIGH);
    } else {
        cur_acc = raw_acc - calibration_acc;
        cur_avel = raw_avel - calibration_avel;
        
        curData.acc_x = cur_acc.x;
        curData.acc_y = cur_acc.y;
        curData.acc_z = cur_acc.z;
        
        curData.gyro_x = cur_avel.x;
        curData.gyro_y = cur_avel.y;
        curData.gyro_z = cur_avel.z;
    }
    
    while (gpsSerial.available() > 0) {
        char inByte = gpsSerial.read();
        if (inByte == '$') {
            cur_recv_idx = 0;
            for (size_t i=0;i<82;i++) {
                cur_nmea_msg[i] = 0;
            }
        }

        if (cur_recv_idx >= 0) {
            if(inByte == 0x0D || inByte == 0x0A) {
                cur_nmea_msg[cur_recv_idx] = '\0';
                cur_recv_idx = -1;
                
                handle_nmea_msg();
                
                if (curData.flags & 1) {
                    digitalWrite(fixLed, HIGH);
                } else {
                    digitalWrite(fixLed, LOW);
                }
            } else {
                cur_nmea_msg[cur_recv_idx++] = inByte;
            }
        }
        
        digitalWrite(actLed, HIGH);
    }
    
    if ((millis() - last_msg_time) < 60) {
        digitalWrite(actLed, HIGH);
    }
    
    transmitReadingFrame(&curData);
    
    delay(50);
}