most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
IMU/IMU.cpp
- Committer:
- joel_ssc
- Date:
- 2019-05-13
- Revision:
- 92:52a91656458a
- Parent:
- 75:92e79d23d29a
File content as of revision 92:52a91656458a:
#include "IMU.h" IMU::IMU(PinName Tx, PinName Rx): _rs232(Tx,Rx) { } void IMU::initialize() { //set up the spi bus and frequency _rs232.baud(115200); // initialize the processing state machine state = SYNC0; // initialize to zeros euler[0] = 0.0; euler[1] = 0.0; euler[2] = 0.0; // initialize to zeros latLonAlt[0] = 0.0; latLonAlt[1] = 0.0; latLonAlt[2] = 0.0; // initialize to no GNSS fix is2dFixValid = false; is3dFixValid = false; numSV = 0; } // this stops an interval timer trigger of the IMU update function void IMU::start() { interval.attach(this, &IMU::update, .05); //this should be a 1 Hz sample rate } // this stops the interval timer trigger of the IMU update function void IMU::stop() { interval.detach(); } void IMU::runIMU() { update(); } // updated the imu update function with a state machine that doesn't hang if no data is present void IMU::update() { // // DEBUGGING an example packet // Serial pc(USBTX, USBRX); // //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 // 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 // for (int j=0; j<20; j++) { //byte = data[j]; while (_rs232.readable()) { // read a single byte byte = _rs232.getc(); // state machine to process byte-by-byte switch (state) { case SYNC0 : if (byte == 0x75) { packet[0] = byte; // save into the packet state = SYNC1; } break; case SYNC1 : if (byte == 0x65) { packet[1] = byte; // save into the packet state = SET; } else { state = SYNC0; } break; case SET : descriptor = byte; // save descriptor set packet[2] = byte; // save into the packet state = LEN; break; case LEN : len = byte; // save payload length packet[3] = byte; // save into the packet state = PAY; i = 0; // reset payload field length counter break; case PAY : if (i < len) { // keep adding until get all the payload length packet[4+i] = byte; // add byte to the packet, skipping over the header 4-bytes i++; // increment payload counter } else { state = CRC0; } if (i >= len) { // not an elseif, since we want to escape when i==len state = CRC0; } break; case CRC0 : crc0 = byte; // save the msb of the checksum state = CRC1; break; case CRC1 : crc1 = byte; // save the lsb of the checksum checksum = ((unsigned int)crc0 << 8) + (unsigned int)crc1; // make checksum into a uint16 if (checksum == calcChecksum(packet, len+4)) { // passed checksum, wahoo! processPayload(descriptor, packet[4], &packet[4]); // process the payload part of the packet, starting at byte 4 } state = SYNC0; // reset to SYNC0 state break; default : state = SYNC0; } } return; } void IMU::processPayload(char descriptor, char length, unsigned char * payload) { if (descriptor == IMU_DATA_SET) { // find an IMU data descriptor set if (length > 2) { // make sure there are at least two bytes to see the field descriptor if (payload[1] == EULER_CF_DESCRIPTOR) { // find an euler CF field descriptor processEulerCfPacket(length, payload); } } } else if (descriptor == GNSS_DATA_SET) { // find a GNSS data descriptor set if (length > 2) { // make sure there are at least two bytes to see the field descriptor if (payload[1] == LLH_POSITION_DESCRIPTOR) { // find a lat-lon-alt field descriptor processLatLonAltPacket(length, payload); } else if (payload[1] == GNSS_FIX_INFO_DESCRIPTOR) { // find a gnss fix field descriptor processGnssFixInformation(length, payload); } } } } void IMU::processEulerCfPacket(char length, unsigned char * payload) { if (length >= EULER_CF_LENGTH) { // make sure correct field length if (payload[0] == EULER_CF_LENGTH) { // make sure field length is as expected euler[0] = floatFromChar(&payload[ROLL_OFFSET+2])*180/_PI; // roll Euler angle convert in degrees euler[1] = floatFromChar(&payload[PITCH_OFFSET+2])*180/_PI; // pitch Euler angle convert in degrees euler[2] = floatFromChar(&payload[YAW_OFFSET+2])*180/_PI; // yaw Euler angle convert in degrees } } } void IMU::processLatLonAltPacket(char length, unsigned char * payload) { if (length >= LLH_POSITION_LENGTH) { // make sure correct field length if (payload[0] == LLH_POSITION_LENGTH) { // make sure field length is as expected latLonAlt[0] = doubleFromChar(&payload[LATITUDE_OFFSET+2]); // latitude in decimal degrees latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET+2]); // longitude in decimal degrees latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET+2]); // altitude above mean sea level in meters } } } void IMU::processGnssFixInformation(char length, unsigned char * payload) { if (length >= GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected if (payload[0] == GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected is2dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x01; // bitand on LSB to see 2d fix flag is3dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x02; // bitand on LSB to see 3d fix flag numSV = char(payload[NUM_SV_OFFSET+2]); // number of GNSS satellite vehicles } } } float IMU::floatFromChar(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; } double IMU::doubleFromChar(unsigned char * value) { unsigned char temp[8]; temp[0] = value[7]; temp[1] = value[6]; temp[2] = value[5]; temp[3] = value[4]; temp[4] = value[3]; temp[5] = value[2]; temp[6] = value[1]; temp[7] = value[0]; return *(double *) temp; } float IMU::getRoll() { return euler[0]; } float IMU::getPitch() { return euler[1]; } float IMU::getHeading() { return euler[2]; // } bool IMU::getIsValid2dFix() { return is2dFixValid; } bool IMU::getIsValid3dFix() { return is3dFixValid; } char IMU::getNumSV() { return numSV; } double IMU::getLatitude() { return latLonAlt[0]; } double IMU::getLongitude() { return latLonAlt[1]; } double IMU::getAltitudeMSL() { return latLonAlt[2]; } unsigned int IMU::calcChecksum(unsigned char * mip_packet, char checksum_range) { unsigned char checksum_byte1 = 0; unsigned char checksum_byte2 = 0; unsigned int myChecksum = 0; for (int i=0; i<checksum_range; i++) { checksum_byte1 += mip_packet[i]; checksum_byte2 += checksum_byte1; } myChecksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2; return myChecksum; }