File content as of revision 10:085ab7328054:

#include "IMU.h"

IMU::IMU(PinName Tx, PinName Rx): 
    _rs232(Tx,Rx)
{
}

void IMU::initialize() {
    //set up the spi bus and frequency
    _rs232.baud(115200);

    // initialize the processing state machine
    state = SYNC0; 
    
    // initialize to zeros
    euler[0] = 0.0;
    euler[1] = 0.0;
    euler[2] = 0.0;
    
    // initialize to zeros
    latLonAlt[0] = 0.0;
    latLonAlt[1] = 0.0;
    latLonAlt[2] = 0.0;
    
    // initialize to no GNSS fix
    is2dFixValid = false;
    is3dFixValid = false;
    numSV = 0;
}

// this stops an interval timer trigger of the IMU update function
void IMU::start() {
    interval.attach(this, &IMU::update, 1);  //this should be a 1 Hz sample rate
}

// this stops the interval timer trigger of the IMU update function
void IMU::stop() {
    interval.detach();
}

// updated the imu update function with a state machine that doesn't hang if no data is present
void IMU::update() {
    while (_rs232.readable()) {
        // read a single byte
        byte = _rs232.getc();
        
        // state machine to process byte-by-byte
        switch (state) {
        case SYNC0 : 
            if (byte == 0x75) {
                state = SYNC1;
            }
            break;
            
        case SYNC1 :
            if (byte == 0x65) {
                state = SET;
            }
            state = SYNC0;
            break;
            
        case SET :
            descriptor = byte; // save descriptor set
            state = LEN;
            break;
            
        case LEN :
            len = byte; // save payload length
            state = PAY;
            i = 0; // reset payload length counter
            break;
            
        case PAY :
            if (i < len) {
                payload[i] = byte; // add byte to the payload array
                i++; // increment payload counter
            }
            else {
                state = CRC0;
            }
            break;
            
        case CRC0 :
            crc0 = byte;
            state = CRC1;
            break;
            
        case CRC1 :
            crc1 = byte;
            checksum = crc0<<8 + crc1; // make checksum
            if (checksum == calcChecksum(payload, len)) {
                processPacket(descriptor, len, payload); // process packet
            }
            state = SYNC0; // reset to SYNC0 state
            break;

        default :
            state = SYNC0; 
        }
    }
    return;
}

void IMU::processPacket(char descriptor, char length, unsigned char * payload) {
    if (descriptor == IMU_DATA_SET) { // find an IMU data descriptor set
        if (length > 2) { // make sure there are at least two bytes to see the field descriptor
            if (payload[1] == EULER_CF_DESCRIPTOR) { // find an euler CF field descriptor
                processEulerCfPacket(length, payload);
            }
        }
    }
    else if (descriptor == GNSS_DATA_SET) { // find a GNSS data descriptor set
        if (length > 2) { // make sure there are at least two bytes to see the field descriptor
            if (payload[1] == LLH_POSITION_DESCRIPTOR) { // find a lat-lon-alt field descriptor
                processLatLonAltPacket(length, payload);
            }
            else if (payload[1] == GNSS_FIX_INFO_DESCRIPTOR) { // find a gnss fix field descriptor
                processGnssFixInformation(length, payload);
            }
        }
    }
}

void IMU::processEulerCfPacket(char length, unsigned char * payload) {
    if (length >= EULER_CF_LENGTH) { // make sure correct field length
        if (payload[0] == EULER_CF_LENGTH) { // make sure field length is as expected
            euler[0] = floatFromChar(&payload[ROLL_OFFSET])*180/_PI;  // roll Euler angle convert in degrees
            euler[1] = floatFromChar(&payload[PITCH_OFFSET])*180/_PI; // pitch Euler angle convert in degrees
            euler[2] = floatFromChar(&payload[YAW_OFFSET])*180/_PI;   // yaw Euler angle convert in degrees
        }
    }
}

void IMU::processLatLonAltPacket(char length, unsigned char * payload) {
    if (length >= LLH_POSITION_LENGTH) { // make sure correct field length
        if (payload[0] == LLH_POSITION_LENGTH) { // make sure field length is as expected
            latLonAlt[0] = doubleFromChar(&payload[LATITUDE_OFFSET]);   // latitude in decimal degrees
            latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET]);  // longitude in decimal degrees
            latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET]); // altitude above mean sea level in meters
        }
    }
}

void IMU::processGnssFixInformation(char length, unsigned char * data) {
    if (length >= GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
        if (payload[0] == GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
            is2dFixValid = char(payload[FIX_TYPE_OFFSET]) & 0x01; // bitand on LSB to see 2d fix flag
            is3dFixValid = char(payload[FIX_TYPE_OFFSET]) & 0x02; // bitand on LSB to see 3d fix flag
            numSV = char(payload[NUM_SV_OFFSET]); // number of GNSS satellite vehicles
        }
    }
}

float IMU::floatFromChar(unsigned char * value) {
    unsigned char temp[4];
    temp[0] = value[3];
    temp[1] = value[2];
    temp[2] = value[1];
    temp[3] = value[0];
    return *(float *) temp;
}

double IMU::doubleFromChar(unsigned char * value) {
    unsigned char temp[8];
    temp[0] = value[7];
    temp[1] = value[6];
    temp[2] = value[5];
    temp[3] = value[4];
    temp[4] = value[3];
    temp[5] = value[2];
    temp[6] = value[1];
    temp[7] = value[0];
    return *(double *) temp;
}

float IMU::getRoll() {
    return euler[0];
}

float IMU::getPitch() {
    return euler[1];
}

float IMU::getHeading() {
    return euler[2];
}

bool IMU::getIsValid2dFix() {
    return is2dFixValid;   
}

bool IMU::getIsValid3dFix() {
    return is3dFixValid;
}

char IMU::getNumSV() {
    return numSV;
}

double IMU::getLatitude() {
    return latLonAlt[0];
}

double IMU::getLongitude() {
    return latLonAlt[1];
}

double IMU::getAltitudeMSL() {
    return latLonAlt[2];
}

unsigned int IMU::calcChecksum(unsigned char * mip_packet, char checksum_range) {
    unsigned char checksum_byte1 = 0;
    unsigned char checksum_byte2 = 0;
    
    for (int i=0; i<checksum_range; i++) {
        checksum_byte1 += mip_packet[i];
        checksum_byte2 += checksum_byte1;
    }
    checksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
    return checksum;
}