File content as of revision 1:0672f84101e4:

#include "IMU_code.h"       //include your header file

#include "MODSERIAL.h"
Serial IMU(p13,p14);         //tx, rx for IMU

//FUNCTION DECLARATION for what is used below in IMU
double double_from_le_bytes(unsigned char * p_to_double);
float float_from_le_bytes(unsigned char * p_to_float);

const double GRAVITY = 9.80665; //new placement 6/22/2017
float stab_accel[3];

#include <string>           //FOR STRINGS
using namespace std;

//constructor (blank)
IMU_code::IMU_code()
{
}

//functions
string IMU_code::IMU_run()
{   
    unsigned char current, last, cb1=0, cb2=0, data[MAXPACKETSIZE];
    int packetlength=MAXPACKETSIZE; //Redefined for each packet payod length below
    int mode=0,count=0;
    //Field length for IMU data (always 0x0E)
    unsigned char fieldlength=0x0E;
    unsigned char descripts[NUMDESCRIPTS];
    //GPS Correlation Timestamp (0x0e,0x12)
    double timestamp;
    descripts[0]=0x12;
    //Delta Theta Vector (0x0e,0x07)
    float delta_theta[3];
    int dt_pos;
    descripts[1]=0x07;
    //Delta Velocity Vector (0x0e,0x08)
    float delta_vel[3];
    int dv_pos;
    descripts[2]=0x08;
    //CF Stablized Accel Vector (up) (0x0e,0x11)
    //float stab_accel[3];  //TROY: I MADE THIS GLOBAL 6/22/17
    int sa_pos;
    descripts[3]=0x0c;
    //If adding/removing stuff, change NUMDESCRIPTS above
    IMU.baud(9600);
    current=0;
    last=0;
    //PC.printf("I'm alive\n");
    
    std::string imu_string = "";
    
    while(1) 
    {
        if(IMU.readable()>0)
        {
            //PC.printf("IMU readable");
            last=current;
            current=IMU.getc();
            if(mode==0&&current==0x65&&last==0x75)
            {
                mode=1;
                data[0]=last;
                data[1]=current;
                cb1=last;
                cb2=last;
                cb1+=current;
                cb2+=cb1;
                count=2;
            }
            else
            {
                //Packet length is header (4) + payload length (varies) + Checksum (2)
                if(count==3) //payload length is byte 3
                    packetlength=current+6;
                //Read in packet length worth of bytes (includes Checksum)
                if(count<packetlength)
                {
                    //PC.printf("%02x ",current);
                    if(count<packetlength-2)
                    {
                        cb1+=current;
                        cb2+=cb1;
                    }
                    data[count]=current;
                    count++;
                }
                //Process packet! But only if checksums match
                if(count==packetlength&&data[packetlength-2]==cb1&&data[packetlength-1]==cb2)
                {
                    //PC.printf("Are you processing the packet?");
                    for(int i=0;i<NUMDESCRIPTS;i++)
                    {
                        count=4;
                        //Search data vector for each subtopic
                        while(count<packetlength-1)
                        {
                            if(data[count]==fieldlength&&data[count+1]==descripts[i])
                            {
                                if(i==0)
                                    timestamp=double_from_le_bytes(&data[count+2]);
                                else if(i==1)
                                {
                                    delta_theta[0]=float_from_le_bytes(&data[count+2+0])*SAMPLE_RATE;
                                    delta_theta[1]=float_from_le_bytes(&data[count+2+4])*SAMPLE_RATE;
                                    delta_theta[2]=float_from_le_bytes(&data[count+2+8])*SAMPLE_RATE;
                                    dt_pos=count+2;
                                }
                                else if(i==2)
                                {
                                    delta_vel[0]=float_from_le_bytes(&data[count+2+0])*SAMPLE_RATE*GRAVITY;
                                    delta_vel[1]=float_from_le_bytes(&data[count+2+4])*SAMPLE_RATE*GRAVITY;
                                    delta_vel[2]=float_from_le_bytes(&data[count+2+8])*SAMPLE_RATE*GRAVITY;
                                    dv_pos=count+2;
                                }
                                else if(i==3)
                                {
                                    stab_accel[0]=float_from_le_bytes(&data[count+2+0])*57.3;//*GRAVITY;
                                    stab_accel[1]=float_from_le_bytes(&data[count+2+4])*57.3;//GRAVITY;
                                    stab_accel[2]=float_from_le_bytes(&data[count+2+8])*57.3;//*GRAVITY;
                                    sa_pos=count+2;
                                }
                                count=packetlength;  
                            }
                            count++;
                        }
                    }
                    timestamp=double_from_le_bytes(&data[6]);
                    //Relevant data to send back to ground station
                    //PC.printf("\nT: %f SA:(%d) Roll: %9.6f Pitch: %9.6f Yaw:%9.6f\n\n",timestamp,sa_pos,stab_accel[0],stab_accel[1],stab_accel[2]);
                    //PC.printf("\nT: %f SA:(%d) Roll: %9.6f Pitch: %9.6f Yaw:%9.6f\n",timestamp,sa_pos,stab_accel[0],stab_accel[1],stab_accel[2]);
                    char char_buffer [60];
                    sprintf(char_buffer,"T: %f SA:(%d) Roll: %9.6f Pitch: %9.6f Yaw: %9.6f",timestamp,sa_pos,stab_accel[0],stab_accel[1],stab_accel[2]); 
                    //PC.printf("cb: %s\n\n", char_buffer); //TROY: My way to double-check this
                    //Sanity check
                    //PC.printf("\nCheck1: %02x == %02x check2 %02x == %02x PL: %d T: %f DT: %f %f %f DV: %f %f %f SA: %f %f %f\n\n",data[packetlength-2],cb1, data[packetlength-1],cb2,packetlength,timestamp,delta_theta[0],delta_theta[1],delta_theta[2],
                    //delta_vel[0],delta_vel[1],delta_vel[2],stab_accel[0],stab_accel[1],stab_accel[2]);
                    count=MAXPACKETSIZE+1;
                    
                    imu_string = char_buffer;
                    return imu_string;
                }
                if(current==0x65&&last==0x75)
                {
                    //return 0;
                    cb1=1;
                    cb2=0;
                    mode=0;
                }                
            }
        }
    } 
}

float IMU_code::IMU_pitch()
{   
    return stab_accel[1];
}

double double_from_le_bytes(unsigned char * p_to_double)
{
    unsigned char temp[8];
    temp[0] = p_to_double[7];
    temp[1] = p_to_double[6];
    temp[2] = p_to_double[5];
    temp[3] = p_to_double[4];
    temp[4] = p_to_double[3];
    temp[5] = p_to_double[2];
    temp[6] = p_to_double[1];
    temp[7] = p_to_double[0];
    return *(double *) temp;
}

float float_from_le_bytes(unsigned char * p_to_float)
{
    unsigned char temp[4];
    temp[0] = p_to_float[3];
    temp[1] = p_to_float[2];
    temp[2] = p_to_float[1];
    temp[3] = p_to_float[0];
    return *(float *) temp;
}