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

IMU_code.cpp@0:22fc81041056, 2017-06-09 (annotated)

Committer:
mdavis30
Date:
Fri Jun 09 17:31:10 2017 +0000
Revision:
0:22fc81041056
Child:
1:0672f84101e4
working and doesn't move when reset

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mdavis30 0:22fc81041056 1 #include "IMU_code.h" //include your header file
mdavis30 0:22fc81041056 2
mdavis30 0:22fc81041056 3 #include "MODSERIAL.h"
mdavis30 0:22fc81041056 4 Serial IMU(p13,p14); //tx, rx for IMU
mdavis30 0:22fc81041056 5 //Serial PC1(USBTX, USBRX);
mdavis30 0:22fc81041056 6
mdavis30 0:22fc81041056 7 //FUNCTION DECLARATION for what is used below in IMU
mdavis30 0:22fc81041056 8 double double_from_le_bytes(unsigned char * p_to_double);
mdavis30 0:22fc81041056 9 float float_from_le_bytes(unsigned char * p_to_float);
mdavis30 0:22fc81041056 10
mdavis30 0:22fc81041056 11 #include <string> //FOR STRINGS
mdavis30 0:22fc81041056 12 using namespace std;
mdavis30 0:22fc81041056 13
mdavis30 0:22fc81041056 14 //constructor (blank)
mdavis30 0:22fc81041056 15 IMU_code::IMU_code()
mdavis30 0:22fc81041056 16 {
mdavis30 0:22fc81041056 17 }
mdavis30 0:22fc81041056 18
mdavis30 0:22fc81041056 19 //functions
mdavis30 0:22fc81041056 20 string IMU_code::IMU_run()
mdavis30 0:22fc81041056 21 {
mdavis30 0:22fc81041056 22 //PC1.baud(9600);
mdavis30 0:22fc81041056 23 unsigned char current, last, cb1=0, cb2=0, data[MAXPACKETSIZE];
mdavis30 0:22fc81041056 24 int packetlength=MAXPACKETSIZE; //Redefined for each packet payod length below
mdavis30 0:22fc81041056 25 int mode=0,count=0;
mdavis30 0:22fc81041056 26 //Field length for IMU data (always 0x0E)
mdavis30 0:22fc81041056 27 unsigned char fieldlength=0x0E;
mdavis30 0:22fc81041056 28 unsigned char descripts[NUMDESCRIPTS];
mdavis30 0:22fc81041056 29 //GPS Correlation Timestamp (0x0e,0x12)
mdavis30 0:22fc81041056 30 double timestamp;
mdavis30 0:22fc81041056 31 descripts[0]=0x12;
mdavis30 0:22fc81041056 32 //Delta Theta Vector (0x0e,0x07)
mdavis30 0:22fc81041056 33 float delta_theta[3];
mdavis30 0:22fc81041056 34 int dt_pos;
mdavis30 0:22fc81041056 35 descripts[1]=0x07;
mdavis30 0:22fc81041056 36 //Delta Velocity Vector (0x0e,0x08)
mdavis30 0:22fc81041056 37 float delta_vel[3];
mdavis30 0:22fc81041056 38 int dv_pos;
mdavis30 0:22fc81041056 39 descripts[2]=0x08;
mdavis30 0:22fc81041056 40 //CF Stablized Accel Vector (up) (0x0e,0x11)
mdavis30 0:22fc81041056 41 float stab_accel[3];
mdavis30 0:22fc81041056 42 int sa_pos;
mdavis30 0:22fc81041056 43 descripts[3]=0x0c;
mdavis30 0:22fc81041056 44 //If adding/removing stuff, change NUMDESCRIPTS above
mdavis30 0:22fc81041056 45 IMU.baud(9600);
mdavis30 0:22fc81041056 46 current=0;
mdavis30 0:22fc81041056 47 last=0;
mdavis30 0:22fc81041056 48 //PC1.printf("I'm alive\n");
mdavis30 0:22fc81041056 49
mdavis30 0:22fc81041056 50 std::string imu_string = "";
mdavis30 0:22fc81041056 51
mdavis30 0:22fc81041056 52 while(1)
mdavis30 0:22fc81041056 53 {
mdavis30 0:22fc81041056 54 //PC1.printf("while loop\n");
mdavis30 0:22fc81041056 55 if(IMU.readable()>0)
mdavis30 0:22fc81041056 56 {
mdavis30 0:22fc81041056 57 last=current;
mdavis30 0:22fc81041056 58 current=IMU.getc();
mdavis30 0:22fc81041056 59 if(mode==0&&current==0x65&&last==0x75)
mdavis30 0:22fc81041056 60 {
mdavis30 0:22fc81041056 61 //PC1.printf("First packet recieved.\n");
mdavis30 0:22fc81041056 62 mode=1;
mdavis30 0:22fc81041056 63 data[0]=last;
mdavis30 0:22fc81041056 64 data[1]=current;
mdavis30 0:22fc81041056 65 cb1=last;
mdavis30 0:22fc81041056 66 cb2=last;
mdavis30 0:22fc81041056 67 cb1+=current;
mdavis30 0:22fc81041056 68 cb2+=cb1;
mdavis30 0:22fc81041056 69 count=2;
mdavis30 0:22fc81041056 70 }
mdavis30 0:22fc81041056 71 else
mdavis30 0:22fc81041056 72 {
mdavis30 0:22fc81041056 73 //Packet length is header (4) + payload length (varies) + Checksum (2)
mdavis30 0:22fc81041056 74 if(count==3) //payload length is byte 3
mdavis30 0:22fc81041056 75 packetlength=current+6;
mdavis30 0:22fc81041056 76 //Read in packet length worth of bytes (includes Checksum)
mdavis30 0:22fc81041056 77 if(count<packetlength)
mdavis30 0:22fc81041056 78 {
mdavis30 0:22fc81041056 79 //PC.printf("%02x ",current);
mdavis30 0:22fc81041056 80 if(count<packetlength-2)
mdavis30 0:22fc81041056 81 {
mdavis30 0:22fc81041056 82 cb1+=current;
mdavis30 0:22fc81041056 83 cb2+=cb1;
mdavis30 0:22fc81041056 84 }
mdavis30 0:22fc81041056 85 data[count]=current;
mdavis30 0:22fc81041056 86 count++;
mdavis30 0:22fc81041056 87 }
mdavis30 0:22fc81041056 88 //Process packet! But only if checksums match
mdavis30 0:22fc81041056 89 if(count==packetlength&&data[packetlength-2]==cb1&&data[packetlength-1]==cb2)
mdavis30 0:22fc81041056 90 {
mdavis30 0:22fc81041056 91 //PC.printf("Are you processing the packet?");
mdavis30 0:22fc81041056 92 for(int i=0;i<NUMDESCRIPTS;i++)
mdavis30 0:22fc81041056 93 {
mdavis30 0:22fc81041056 94 count=4;
mdavis30 0:22fc81041056 95 //Search data vector for each subtopic
mdavis30 0:22fc81041056 96 while(count<packetlength-1)
mdavis30 0:22fc81041056 97 {
mdavis30 0:22fc81041056 98 if(data[count]==fieldlength&&data[count+1]==descripts[i])
mdavis30 0:22fc81041056 99 {
mdavis30 0:22fc81041056 100 if(i==0)
mdavis30 0:22fc81041056 101 timestamp=double_from_le_bytes(&data[count+2]);
mdavis30 0:22fc81041056 102 else if(i==1)
mdavis30 0:22fc81041056 103 {
mdavis30 0:22fc81041056 104 delta_theta[0]=float_from_le_bytes(&data[count+2+0])*SAMPLE_RATE;
mdavis30 0:22fc81041056 105 delta_theta[1]=float_from_le_bytes(&data[count+2+4])*SAMPLE_RATE;
mdavis30 0:22fc81041056 106 delta_theta[2]=float_from_le_bytes(&data[count+2+8])*SAMPLE_RATE;
mdavis30 0:22fc81041056 107 dt_pos=count+2;
mdavis30 0:22fc81041056 108 }
mdavis30 0:22fc81041056 109 else if(i==2)
mdavis30 0:22fc81041056 110 {
mdavis30 0:22fc81041056 111 delta_vel[0]=float_from_le_bytes(&data[count+2+0])*SAMPLE_RATE*GRAVITY;
mdavis30 0:22fc81041056 112 delta_vel[1]=float_from_le_bytes(&data[count+2+4])*SAMPLE_RATE*GRAVITY;
mdavis30 0:22fc81041056 113 delta_vel[2]=float_from_le_bytes(&data[count+2+8])*SAMPLE_RATE*GRAVITY;
mdavis30 0:22fc81041056 114 dv_pos=count+2;
mdavis30 0:22fc81041056 115 }
mdavis30 0:22fc81041056 116 else if(i==3)
mdavis30 0:22fc81041056 117 {
mdavis30 0:22fc81041056 118 stab_accel[0]=float_from_le_bytes(&data[count+2+0])*57.3;//*GRAVITY;
mdavis30 0:22fc81041056 119 stab_accel[1]=float_from_le_bytes(&data[count+2+4])*57.3;//GRAVITY;
mdavis30 0:22fc81041056 120 stab_accel[2]=float_from_le_bytes(&data[count+2+8])*57.3;//*GRAVITY;
mdavis30 0:22fc81041056 121 sa_pos=count+2;
mdavis30 0:22fc81041056 122 }
mdavis30 0:22fc81041056 123 count=packetlength;
mdavis30 0:22fc81041056 124 }
mdavis30 0:22fc81041056 125 count++;
mdavis30 0:22fc81041056 126 }
mdavis30 0:22fc81041056 127 }
mdavis30 0:22fc81041056 128 timestamp=double_from_le_bytes(&data[6]);
mdavis30 0:22fc81041056 129 //Relevant data to send back to ground station
mdavis30 0:22fc81041056 130 //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]);
mdavis30 0:22fc81041056 131 //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]);
mdavis30 0:22fc81041056 132 char char_buffer [60];
mdavis30 0:22fc81041056 133 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]);
mdavis30 0:22fc81041056 134 //PC.printf("cb: %s\n\n", char_buffer); //TROY: My way to double-check this
mdavis30 0:22fc81041056 135 //Sanity check
mdavis30 0:22fc81041056 136 //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],
mdavis30 0:22fc81041056 137 //delta_vel[0],delta_vel[1],delta_vel[2],stab_accel[0],stab_accel[1],stab_accel[2]);
mdavis30 0:22fc81041056 138 count=MAXPACKETSIZE+1;
mdavis30 0:22fc81041056 139
mdavis30 0:22fc81041056 140 imu_string = char_buffer;
mdavis30 0:22fc81041056 141 return imu_string;
mdavis30 0:22fc81041056 142 }
mdavis30 0:22fc81041056 143 if(current==0x65&&last==0x75)
mdavis30 0:22fc81041056 144 {
mdavis30 0:22fc81041056 145 //return 0;
mdavis30 0:22fc81041056 146 cb1=1;
mdavis30 0:22fc81041056 147 cb2=0;
mdavis30 0:22fc81041056 148 mode=0;
mdavis30 0:22fc81041056 149 }
mdavis30 0:22fc81041056 150 }
mdavis30 0:22fc81041056 151 }
mdavis30 0:22fc81041056 152 }
mdavis30 0:22fc81041056 153 //return "TEST STRING"; //unreachable
mdavis30 0:22fc81041056 154 }
mdavis30 0:22fc81041056 155
mdavis30 0:22fc81041056 156 double double_from_le_bytes(unsigned char * p_to_double)
mdavis30 0:22fc81041056 157 {
mdavis30 0:22fc81041056 158 unsigned char temp[8];
mdavis30 0:22fc81041056 159 temp[0] = p_to_double[7];
mdavis30 0:22fc81041056 160 temp[1] = p_to_double[6];
mdavis30 0:22fc81041056 161 temp[2] = p_to_double[5];
mdavis30 0:22fc81041056 162 temp[3] = p_to_double[4];
mdavis30 0:22fc81041056 163 temp[4] = p_to_double[3];
mdavis30 0:22fc81041056 164 temp[5] = p_to_double[2];
mdavis30 0:22fc81041056 165 temp[6] = p_to_double[1];
mdavis30 0:22fc81041056 166 temp[7] = p_to_double[0];
mdavis30 0:22fc81041056 167 return *(double *) temp;
mdavis30 0:22fc81041056 168 }
mdavis30 0:22fc81041056 169
mdavis30 0:22fc81041056 170 float float_from_le_bytes(unsigned char * p_to_float)
mdavis30 0:22fc81041056 171 {
mdavis30 0:22fc81041056 172 unsigned char temp[4];
mdavis30 0:22fc81041056 173 temp[0] = p_to_float[3];
mdavis30 0:22fc81041056 174 temp[1] = p_to_float[2];
mdavis30 0:22fc81041056 175 temp[2] = p_to_float[1];
mdavis30 0:22fc81041056 176 temp[3] = p_to_float[0];
mdavis30 0:22fc81041056 177 return *(float *) temp;
mdavis30 0:22fc81041056 178 }