IMU.cpp

00001 #include "IMU.h"
00002 
00003 IMU::IMU(PinName Tx, PinName Rx): 
00004     _rs232(Tx,Rx)
00005 {
00006 }
00007 
00008 void IMU::initialize() {
00009     //set up the spi bus and frequency
00010     _rs232.baud(115200);
00011 
00012     // initialize the processing state machine
00013     state = SYNC0; 
00014     
00015     // initialize to zeros
00016     euler[0] = 0.0;
00017     euler[1] = 0.0;
00018     euler[2] = 0.0;
00019     
00020     // initialize to zeros
00021     latLonAlt[0] = 0.0;
00022     latLonAlt[1] = 0.0;
00023     latLonAlt[2] = 0.0;
00024     
00025     // initialize to no GNSS fix
00026     is2dFixValid = false;
00027     is3dFixValid = false;
00028     numSV = 0;
00029 }
00030 
00031 // this stops an interval timer trigger of the IMU update function
00032 void IMU::start() {
00033     interval.attach(this, &IMU::update, .05);  //this should be a 1 Hz sample rate
00034 }
00035 
00036 // this stops the interval timer trigger of the IMU update function
00037 void IMU::stop() {
00038     interval.detach();
00039 }
00040 
00041 void IMU::runIMU() {
00042     update();
00043 }
00044 
00045 // updated the imu update function with a state machine that doesn't hang if no data is present
00046 void IMU::update() {
00047     
00048 //    // DEBUGGING an example packet
00049 //    Serial pc(USBTX, USBRX);
00050 //    //char data[20] = {0x75,0x65,0x80,0x0E,0x0E,0x04,0x3E,0x7A,0x63,0xA0,0xBB,0x8E,0x3B,0x29,0x7F,0xE5,0xBF,0x7F,0x84,0xEE}; // 3d accel
00051 //    char data[20] = {0x75,0x65,0x80,0x0E,0x0E,0x0C,0xBA,0xE3,0xED,0x9B,0x3C,0x7D,0x6D,0xDF,0xBF,0x85,0x5C,0xF5,0x41,0xBB}; // euler cf
00052 //    for (int j=0; j<20; j++) {
00053         //byte = data[j];
00054         
00055     while (_rs232.readable()) {
00056         // read a single byte
00057         byte = _rs232.getc();
00058         
00059         // state machine to process byte-by-byte
00060         switch (state) {
00061         case SYNC0 : 
00062             if (byte == 0x75) {
00063                 packet[0] = byte; // save into the packet
00064                 state = SYNC1;
00065             }
00066             break;
00067             
00068         case SYNC1 :
00069             if (byte == 0x65) {
00070                 packet[1] = byte; // save into the packet
00071                 state = SET;
00072             }
00073             else {
00074                 state = SYNC0;
00075             }
00076             break;
00077             
00078         case SET :
00079             descriptor = byte; // save descriptor set
00080             packet[2] = byte; // save into the packet
00081             state = LEN;
00082             break;
00083             
00084         case LEN :
00085             len = byte; // save payload length
00086             packet[3] = byte; // save into the packet
00087             state = PAY;
00088             i = 0; // reset payload field length counter
00089             break;
00090             
00091         case PAY :
00092             if (i < len) { // keep adding until get all the payload length
00093                 packet[4+i] = byte; // add byte to the packet, skipping over the header 4-bytes
00094                 i++; // increment payload counter
00095             }
00096             else {
00097                 state = CRC0;
00098             }
00099             if (i >= len) { // not an elseif, since we want to escape when i==len
00100                 state = CRC0;
00101             }
00102             break;
00103             
00104         case CRC0 :
00105             crc0 = byte; // save the msb of the checksum
00106             state = CRC1;
00107             break;
00108             
00109         case CRC1 :
00110             crc1 = byte; // save the lsb of the checksum
00111             checksum = ((unsigned int)crc0 << 8) + (unsigned int)crc1; // make checksum into a uint16
00112             if (checksum == calcChecksum(packet, len+4)) { // passed checksum, wahoo!
00113                 processPayload(descriptor, packet[4], &packet[4]); // process the payload part of the packet, starting at byte 4
00114             }
00115             state = SYNC0; // reset to SYNC0 state
00116             break;
00117 
00118         default :
00119             state = SYNC0;
00120         }
00121     }
00122     return;
00123 }
00124 
00125 void IMU::processPayload(char descriptor, char length, unsigned char * payload) {
00126     if (descriptor == IMU_DATA_SET) { // find an IMU data descriptor set
00127         if (length > 2) { // make sure there are at least two bytes to see the field descriptor
00128             if (payload[1] == EULER_CF_DESCRIPTOR) { // find an euler CF field descriptor
00129                 processEulerCfPacket(length, payload);
00130             }
00131         }
00132     }
00133     else if (descriptor == GNSS_DATA_SET) { // find a GNSS data descriptor set
00134         if (length > 2) { // make sure there are at least two bytes to see the field descriptor
00135             if (payload[1] == LLH_POSITION_DESCRIPTOR) { // find a lat-lon-alt field descriptor
00136                 processLatLonAltPacket(length, payload);
00137             }
00138             else if (payload[1] == GNSS_FIX_INFO_DESCRIPTOR) { // find a gnss fix field descriptor
00139                 processGnssFixInformation(length, payload);
00140             }
00141         }
00142     }
00143 }
00144 
00145 void IMU::processEulerCfPacket(char length, unsigned char * payload) {
00146     if (length >= EULER_CF_LENGTH) { // make sure correct field length
00147         if (payload[0] == EULER_CF_LENGTH) { // make sure field length is as expected
00148             euler[0] = floatFromChar(&payload[ROLL_OFFSET+2])*180/_PI;  // roll Euler angle convert in degrees
00149             euler[1] = floatFromChar(&payload[PITCH_OFFSET+2])*180/_PI; // pitch Euler angle convert in degrees
00150             euler[2] = floatFromChar(&payload[YAW_OFFSET+2])*180/_PI;   // yaw Euler angle convert in degrees
00151         }
00152     }
00153 }
00154 
00155 void IMU::processLatLonAltPacket(char length, unsigned char * payload) {
00156     if (length >= LLH_POSITION_LENGTH) { // make sure correct field length
00157         if (payload[0] == LLH_POSITION_LENGTH) { // make sure field length is as expected
00158             latLonAlt[0] = doubleFromChar(&payload[LATITUDE_OFFSET+2]);   // latitude in decimal degrees
00159             latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET+2]);  // longitude in decimal degrees
00160             latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET+2]); // altitude above mean sea level in meters
00161         }
00162     }
00163 }
00164 
00165 void IMU::processGnssFixInformation(char length, unsigned char * payload) {
00166     if (length >= GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
00167         if (payload[0] == GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
00168             is2dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x01; // bitand on LSB to see 2d fix flag
00169             is3dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x02; // bitand on LSB to see 3d fix flag
00170             numSV = char(payload[NUM_SV_OFFSET+2]); // number of GNSS satellite vehicles
00171         }
00172     }
00173 }
00174 
00175 float IMU::floatFromChar(unsigned char * value) {
00176     unsigned char temp[4];
00177     temp[0] = value[3];
00178     temp[1] = value[2];
00179     temp[2] = value[1];
00180     temp[3] = value[0];
00181     return *(float *) temp;
00182 }
00183 
00184 double IMU::doubleFromChar(unsigned char * value) {
00185     unsigned char temp[8];
00186     temp[0] = value[7];
00187     temp[1] = value[6];
00188     temp[2] = value[5];
00189     temp[3] = value[4];
00190     temp[4] = value[3];
00191     temp[5] = value[2];
00192     temp[6] = value[1];
00193     temp[7] = value[0];
00194     return *(double *) temp;
00195 }
00196 
00197 float IMU::getRoll() {
00198     return euler[0];
00199 }
00200 
00201 float IMU::getPitch() {
00202     return euler[1];
00203 }
00204 
00205 float IMU::getHeading() {
00206     return euler[2];    //
00207 }
00208 
00209 bool IMU::getIsValid2dFix() {
00210     return is2dFixValid;   
00211 }
00212 
00213 bool IMU::getIsValid3dFix() {
00214     return is3dFixValid;
00215 }
00216 
00217 char IMU::getNumSV() {
00218     return numSV;
00219 }
00220 
00221 double IMU::getLatitude() {
00222     return latLonAlt[0];
00223 }
00224 
00225 double IMU::getLongitude() {
00226     return latLonAlt[1];
00227 }
00228 
00229 double IMU::getAltitudeMSL() {
00230     return latLonAlt[2];
00231 }
00232 
00233 unsigned int IMU::calcChecksum(unsigned char * mip_packet, char checksum_range) {
00234     unsigned char checksum_byte1 = 0;
00235     unsigned char checksum_byte2 = 0;
00236     unsigned int myChecksum = 0;
00237     
00238     for (int i=0; i<checksum_range; i++) {
00239         checksum_byte1 += mip_packet[i];
00240         checksum_byte2 += checksum_byte1;
00241     }
00242     myChecksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
00243     return myChecksum;
00244 }