Diff: IMU/IMU.cpp

Revision:

11:3b241ecb75ed

Parent:

10:085ab7328054

diff -r 085ab7328054 -r 3b241ecb75ed IMU/IMU.cpp
--- a/IMU/IMU.cpp	Mon Oct 23 12:50:53 2017 +0000
+++ b/IMU/IMU.cpp	Fri Oct 27 00:37:32 2017 +0000
@@ -30,7 +30,7 @@
 
 // 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
+    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
@@ -40,6 +40,14 @@
 
 // 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();
@@ -48,60 +56,69 @@
         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;
             }
-            state = SYNC0;
+            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 length counter
+            i = 0; // reset payload field length counter
             break;
             
         case PAY :
-            if (i < len) {
-                payload[i] = byte; // add byte to the payload array
+            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;
+            crc0 = byte; // save the msb of the checksum
             state = CRC1;
             break;
             
         case CRC1 :
-            crc1 = byte;
-            checksum = crc0<<8 + crc1; // make checksum
-            if (checksum == calcChecksum(payload, len)) {
-                processPacket(descriptor, len, payload); // process packet
+            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; 
+            state = SYNC0;
         }
     }
     return;
 }
 
-void IMU::processPacket(char descriptor, char length, unsigned char * payload) {
+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
@@ -124,9 +141,9 @@
 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
+            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
         }
     }
 }
@@ -134,19 +151,19 @@
 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
+            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 * data) {
+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]) & 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
+            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
         }
     }
 }
@@ -212,11 +229,12 @@
 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;
     }
-    checksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
-    return checksum;
+    myChecksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
+    return myChecksum;
 }
\ No newline at end of file