Diff: IMU/IMU.cpp

Revision:

9:d5fcdcb3c89d

Child:

10:085ab7328054

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/IMU.cpp	Fri Oct 20 11:41:22 2017 +0000
@@ -0,0 +1,78 @@
+#include "IMU.h"
+
+IMU::IMU(): _rs232(p13,p14) //maybe this should configurable on construction
+{
+
+}
+
+void IMU::initialize()
+{
+    //set up the spi bus and frequency
+    _rs232.baud(115200);
+    
+    // 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];    
+    
+    int i = 0;// set packet_length based on field_length (convert from hex to int)
+    unsigned char current = 0x00;
+    unsigned char last = 0x00;
+
+}
+
+//This starts an interupt driven trigger of the external ADC
+void IMU::start()
+{
+    interval.attach(this, &IMU::update, 1);  //this should be a 1 Hz sample rate
+}
+
+//This stops it
+void IMU::stop()
+{
+    interval.detach();
+}
+
+
+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;
+        }
+        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 ;
+}
+
+float IMU::float_from_char(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;
+}
+