Diff: IMU/IMU.cpp

Revision:

10:085ab7328054

Parent:

9:d5fcdcb3c89d

Child:

11:3b241ecb75ed

diff -r d5fcdcb3c89d -r 085ab7328054 IMU/IMU.cpp
--- a/IMU/IMU.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/IMU/IMU.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,73 +1,157 @@
 #include "IMU.h"
 
-IMU::IMU(): _rs232(p13,p14) //maybe this should configurable on construction
+IMU::IMU(PinName Tx, PinName Rx): 
+    _rs232(Tx,Rx)
 {
-
 }
 
-void IMU::initialize()
-{
+void IMU::initialize() {
     //set up the spi bus and frequency
     _rs232.baud(115200);
+
+    // initialize the processing state machine
+    state = SYNC0; 
     
-    // Define IMU packet type 
-    unsigned char SYNC1 = 0x75;             // First sync byte will always be 'u' (0x75)
-    unsigned char SYNC2 = 0x65;             // Second sync byte will always be 'e' (0x65)
-    unsigned char descripter_set = 0x80;    // Descriptor set byte for AHRS (0x80)
-    int payload_length = 0x0E;              // Payload length byte for CF Euler Angles (0x0E)
-    int field_length = 0x0E;                // Field length byte for CF Euler Angles (0x0E)
-    unsigned char data_descriptor = 0x0C;   // Data descriptor byte for CF Euler Angles (0x0C)
-    unsigned char data[30];                 // Data sent CF euler angles rpy [radians]      
-    int data_offset = 6;                    // Binary offset
-    int roll_offset = 2;                    // Binary offset
-    int pitch_offset = 6;                   // Binary offset
-    int yaw_offset = 10;                    // Binary offset
-    float euler[3];    
+    // initialize to zeros
+    euler[0] = 0.0;
+    euler[1] = 0.0;
+    euler[2] = 0.0;
     
-    int i = 0;// set packet_length based on field_length (convert from hex to int)
-    unsigned char current = 0x00;
-    unsigned char last = 0x00;
-
+    // 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 starts an interupt driven trigger of the external ADC
-void IMU::start()
-{
+// 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 it
-void IMU::stop()
-{
+// 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;
 
-void IMU::update()
-{
-    last = current;
-    current = _rs232.getc();        
-    if (last == SYNC1 && current == SYNC2){  // Detect beginning of packet
-        data[0] = last;
-        data[0] = current;
-        data[0] = _rs232.getc();
-        data[0] = _rs232.getc();
-        i = 0;
-        while(i < field_length){            // Get data
-            data[i] = _rs232.getc();
-            i += 1;
+        default :
+            state = SYNC0; 
         }
-        data[i + data_offset + 1] = _rs232.getc();     // Check sum 1
-        data[i + data_offset + 2] = _rs232.getc();     // Check sum 2    
-    }       
-    euler[0] = float_from_char(&data[roll_offset])*180/_PI;       // roll Euler angle convert to float
-    euler[1] = float_from_char(&data[pitch_offset])*180/_PI;      // pitch Euler angle convert to float
-    euler[2] = float_from_char(&data[yaw_offset])*180/_PI;        // yaw Euler angle convert to float      
-    return ;
+    }
+    return;
 }
 
-float IMU::float_from_char(unsigned char * value)
-{
+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];
@@ -76,3 +160,63 @@
     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;
+}
\ No newline at end of file