FSG
/
IMU_code_
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
IMU_code_7_14
by 
Troy Holley
Diff: IMU_code.cpp
- Revision:
- 0:22fc81041056
- Child:
- 1:0672f84101e4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_code.cpp Fri Jun 09 17:31:10 2017 +0000
@@ -0,0 +1,178 @@
+#include "IMU_code.h" //include your header file
+
+#include "MODSERIAL.h"
+Serial IMU(p13,p14); //tx, rx for IMU
+//Serial PC1(USBTX, USBRX);
+
+//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);
+
+#include <string> //FOR STRINGS
+using namespace std;
+
+//constructor (blank)
+IMU_code::IMU_code()
+{
+}
+
+//functions
+string IMU_code::IMU_run()
+{
+ //PC1.baud(9600);
+ 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];
+ int sa_pos;
+ descripts[3]=0x0c;
+ //If adding/removing stuff, change NUMDESCRIPTS above
+ IMU.baud(9600);
+ current=0;
+ last=0;
+ //PC1.printf("I'm alive\n");
+
+ std::string imu_string = "";
+
+ while(1)
+ {
+ //PC1.printf("while loop\n");
+ if(IMU.readable()>0)
+ {
+ last=current;
+ current=IMU.getc();
+ if(mode==0&¤t==0x65&&last==0x75)
+ {
+ //PC1.printf("First packet recieved.\n");
+ 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
+ //PC1.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;
+ }
+ }
+ }
+ }
+ //return "TEST STRING"; //unreachable
+}
+
+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;
+}
\ No newline at end of file