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
Diff: IMU/IMU.cpp
- Revision:
- 10:085ab7328054
- Parent:
- 9:d5fcdcb3c89d
- Child:
- 11:3b241ecb75ed
--- a/IMU/IMU.cpp Fri Oct 20 11:41:22 2017 +0000 +++ b/IMU/IMU.cpp Mon Oct 23 12:50:53 2017 +0000 @@ -1,73 +1,157 @@ #include "IMU.h" -IMU::IMU(): _rs232(p13,p14) //maybe this should configurable on construction +IMU::IMU(PinName Tx, PinName Rx): + _rs232(Tx,Rx) { - } -void IMU::initialize() -{ +void IMU::initialize() { //set up the spi bus and frequency _rs232.baud(115200); + + // initialize the processing state machine + state = SYNC0; - // Define IMU packet type - unsigned char SYNC1 = 0x75; // First sync byte will always be 'u' (0x75) - unsigned char SYNC2 = 0x65; // Second sync byte will always be 'e' (0x65) - unsigned char descripter_set = 0x80; // Descriptor set byte for AHRS (0x80) - int payload_length = 0x0E; // Payload length byte for CF Euler Angles (0x0E) - int field_length = 0x0E; // Field length byte for CF Euler Angles (0x0E) - unsigned char data_descriptor = 0x0C; // Data descriptor byte for CF Euler Angles (0x0C) - unsigned char data[30]; // Data sent CF euler angles rpy [radians] - int data_offset = 6; // Binary offset - int roll_offset = 2; // Binary offset - int pitch_offset = 6; // Binary offset - int yaw_offset = 10; // Binary offset - float euler[3]; + // initialize to zeros + euler[0] = 0.0; + euler[1] = 0.0; + euler[2] = 0.0; - int i = 0;// set packet_length based on field_length (convert from hex to int) - unsigned char current = 0x00; - unsigned char last = 0x00; - + // 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 starts an interupt driven trigger of the external ADC -void IMU::start() -{ +// this stops an interval timer trigger of the IMU update function +void IMU::start() { interval.attach(this, &IMU::update, 1); //this should be a 1 Hz sample rate } -//This stops it -void IMU::stop() -{ +// this stops the interval timer trigger of the IMU update function +void IMU::stop() { interval.detach(); } +// updated the imu update function with a state machine that doesn't hang if no data is present +void IMU::update() { + while (_rs232.readable()) { + // read a single byte + byte = _rs232.getc(); + + // state machine to process byte-by-byte + switch (state) { + case SYNC0 : + if (byte == 0x75) { + state = SYNC1; + } + break; + + case SYNC1 : + if (byte == 0x65) { + state = SET; + } + state = SYNC0; + break; + + case SET : + descriptor = byte; // save descriptor set + state = LEN; + break; + + case LEN : + len = byte; // save payload length + state = PAY; + i = 0; // reset payload length counter + break; + + case PAY : + if (i < len) { + payload[i] = byte; // add byte to the payload array + i++; // increment payload counter + } + else { + state = CRC0; + } + break; + + case CRC0 : + crc0 = byte; + state = CRC1; + break; + + case CRC1 : + crc1 = byte; + checksum = crc0<<8 + crc1; // make checksum + if (checksum == calcChecksum(payload, len)) { + processPacket(descriptor, len, payload); // process packet + } + state = SYNC0; // reset to SYNC0 state + break; -void IMU::update() -{ - last = current; - current = _rs232.getc(); - if (last == SYNC1 && current == SYNC2){ // Detect beginning of packet - data[0] = last; - data[0] = current; - data[0] = _rs232.getc(); - data[0] = _rs232.getc(); - i = 0; - while(i < field_length){ // Get data - data[i] = _rs232.getc(); - i += 1; + default : + state = SYNC0; } - data[i + data_offset + 1] = _rs232.getc(); // Check sum 1 - data[i + data_offset + 2] = _rs232.getc(); // Check sum 2 - } - euler[0] = float_from_char(&data[roll_offset])*180/_PI; // roll Euler angle convert to float - euler[1] = float_from_char(&data[pitch_offset])*180/_PI; // pitch Euler angle convert to float - euler[2] = float_from_char(&data[yaw_offset])*180/_PI; // yaw Euler angle convert to float - return ; + } + return; } -float IMU::float_from_char(unsigned char * value) -{ +void IMU::processPacket(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])*180/_PI; // roll Euler angle convert in degrees + euler[1] = floatFromChar(&payload[PITCH_OFFSET])*180/_PI; // pitch Euler angle convert in degrees + euler[2] = floatFromChar(&payload[YAW_OFFSET])*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]); // latitude in decimal degrees + latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET]); // longitude in decimal degrees + latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET]); // altitude above mean sea level in meters + } + } +} + +void IMU::processGnssFixInformation(char length, unsigned char * data) { + 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]) & 0x01; // bitand on LSB to see 2d fix flag + is3dFixValid = char(payload[FIX_TYPE_OFFSET]) & 0x02; // bitand on LSB to see 3d fix flag + numSV = char(payload[NUM_SV_OFFSET]); // number of GNSS satellite vehicles + } + } +} + +float IMU::floatFromChar(unsigned char * value) { unsigned char temp[4]; temp[0] = value[3]; temp[1] = value[2]; @@ -76,3 +160,63 @@ 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; + + for (int i=0; i<checksum_range; i++) { + checksum_byte1 += mip_packet[i]; + checksum_byte2 += checksum_byte1; + } + checksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2; + return checksum; +} \ No newline at end of file