Diff: IMU/IMU.cpp

Revision:

65:2ac186553959

Parent:

64:a8939bc127ab

Child:

66:0f20870117b7

--- a/IMU/IMU.cpp	Mon Jun 18 21:04:09 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,240 +0,0 @@
-#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, .05);  //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() {
-    
-//    // DEBUGGING an example packet
-//    Serial pc(USBTX, USBRX);
-//    //char data[20] = {0x75,0x65,0x80,0x0E,0x0E,0x04,0x3E,0x7A,0x63,0xA0,0xBB,0x8E,0x3B,0x29,0x7F,0xE5,0xBF,0x7F,0x84,0xEE}; // 3d accel
-//    char data[20] = {0x75,0x65,0x80,0x0E,0x0E,0x0C,0xBA,0xE3,0xED,0x9B,0x3C,0x7D,0x6D,0xDF,0xBF,0x85,0x5C,0xF5,0x41,0xBB}; // euler cf
-//    for (int j=0; j<20; j++) {
-        //byte = data[j];
-        
-    while (_rs232.readable()) {
-        // read a single byte
-        byte = _rs232.getc();
-        
-        // state machine to process byte-by-byte
-        switch (state) {
-        case SYNC0 : 
-            if (byte == 0x75) {
-                packet[0] = byte; // save into the packet
-                state = SYNC1;
-            }
-            break;
-            
-        case SYNC1 :
-            if (byte == 0x65) {
-                packet[1] = byte; // save into the packet
-                state = SET;
-            }
-            else {
-                state = SYNC0;
-            }
-            break;
-            
-        case SET :
-            descriptor = byte; // save descriptor set
-            packet[2] = byte; // save into the packet
-            state = LEN;
-            break;
-            
-        case LEN :
-            len = byte; // save payload length
-            packet[3] = byte; // save into the packet
-            state = PAY;
-            i = 0; // reset payload field length counter
-            break;
-            
-        case PAY :
-            if (i < len) { // keep adding until get all the payload length
-                packet[4+i] = byte; // add byte to the packet, skipping over the header 4-bytes
-                i++; // increment payload counter
-            }
-            else {
-                state = CRC0;
-            }
-            if (i >= len) { // not an elseif, since we want to escape when i==len
-                state = CRC0;
-            }
-            break;
-            
-        case CRC0 :
-            crc0 = byte; // save the msb of the checksum
-            state = CRC1;
-            break;
-            
-        case CRC1 :
-            crc1 = byte; // save the lsb of the checksum
-            checksum = ((unsigned int)crc0 << 8) + (unsigned int)crc1; // make checksum into a uint16
-            if (checksum == calcChecksum(packet, len+4)) { // passed checksum, wahoo!
-                processPayload(descriptor, packet[4], &packet[4]); // process the payload part of the packet, starting at byte 4
-            }
-            state = SYNC0; // reset to SYNC0 state
-            break;
-
-        default :
-            state = SYNC0;
-        }
-    }
-    return;
-}
-
-void IMU::processPayload(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+2])*180/_PI;  // roll Euler angle convert in degrees
-            euler[1] = floatFromChar(&payload[PITCH_OFFSET+2])*180/_PI; // pitch Euler angle convert in degrees
-            euler[2] = floatFromChar(&payload[YAW_OFFSET+2])*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+2]);   // latitude in decimal degrees
-            latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET+2]);  // longitude in decimal degrees
-            latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET+2]); // altitude above mean sea level in meters
-        }
-    }
-}
-
-void IMU::processGnssFixInformation(char length, unsigned char * payload) {
-    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+2]) & 0x01; // bitand on LSB to see 2d fix flag
-            is3dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x02; // bitand on LSB to see 3d fix flag
-            numSV = char(payload[NUM_SV_OFFSET+2]); // 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;
-    unsigned int myChecksum = 0;
-    
-    for (int i=0; i<checksum_range; i++) {
-        checksum_byte1 += mip_packet[i];
-        checksum_byte2 += checksum_byte1;
-    }
-    myChecksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
-    return myChecksum;
-}
\ No newline at end of file