File content as of revision 9:d5fcdcb3c89d:

#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;
}