Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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@66:0f20870117b7, 2018-06-19 (annotated)

Committer:
tnhnrl
Date:
Tue Jun 19 20:14:23 2018 +0000
Revision:
66:0f20870117b7
Child:
74:d281aaef9766
XBee testing file transmission good.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 66:0f20870117b7 1 #include "IMU.h"
tnhnrl 66:0f20870117b7 2
tnhnrl 66:0f20870117b7 3 IMU::IMU(PinName Tx, PinName Rx):
tnhnrl 66:0f20870117b7 4 _rs232(Tx,Rx)
tnhnrl 66:0f20870117b7 5 {
tnhnrl 66:0f20870117b7 6 }
tnhnrl 66:0f20870117b7 7
tnhnrl 66:0f20870117b7 8 void IMU::initialize() {
tnhnrl 66:0f20870117b7 9 //set up the spi bus and frequency
tnhnrl 66:0f20870117b7 10 _rs232.baud(115200);
tnhnrl 66:0f20870117b7 11
tnhnrl 66:0f20870117b7 12 // initialize the processing state machine
tnhnrl 66:0f20870117b7 13 state = SYNC0;
tnhnrl 66:0f20870117b7 14
tnhnrl 66:0f20870117b7 15 // initialize to zeros
tnhnrl 66:0f20870117b7 16 euler[0] = 0.0;
tnhnrl 66:0f20870117b7 17 euler[1] = 0.0;
tnhnrl 66:0f20870117b7 18 euler[2] = 0.0;
tnhnrl 66:0f20870117b7 19
tnhnrl 66:0f20870117b7 20 // initialize to zeros
tnhnrl 66:0f20870117b7 21 latLonAlt[0] = 0.0;
tnhnrl 66:0f20870117b7 22 latLonAlt[1] = 0.0;
tnhnrl 66:0f20870117b7 23 latLonAlt[2] = 0.0;
tnhnrl 66:0f20870117b7 24
tnhnrl 66:0f20870117b7 25 // initialize to no GNSS fix
tnhnrl 66:0f20870117b7 26 is2dFixValid = false;
tnhnrl 66:0f20870117b7 27 is3dFixValid = false;
tnhnrl 66:0f20870117b7 28 numSV = 0;
tnhnrl 66:0f20870117b7 29 }
tnhnrl 66:0f20870117b7 30
tnhnrl 66:0f20870117b7 31 // this stops an interval timer trigger of the IMU update function
tnhnrl 66:0f20870117b7 32 void IMU::start() {
tnhnrl 66:0f20870117b7 33 interval.attach(this, &IMU::update, .05); //this should be a 1 Hz sample rate
tnhnrl 66:0f20870117b7 34 }
tnhnrl 66:0f20870117b7 35
tnhnrl 66:0f20870117b7 36 // this stops the interval timer trigger of the IMU update function
tnhnrl 66:0f20870117b7 37 void IMU::stop() {
tnhnrl 66:0f20870117b7 38 interval.detach();
tnhnrl 66:0f20870117b7 39 }
tnhnrl 66:0f20870117b7 40
tnhnrl 66:0f20870117b7 41 void IMU::runIMU() {
tnhnrl 66:0f20870117b7 42 update();
tnhnrl 66:0f20870117b7 43 }
tnhnrl 66:0f20870117b7 44
tnhnrl 66:0f20870117b7 45 // updated the imu update function with a state machine that doesn't hang if no data is present
tnhnrl 66:0f20870117b7 46 void IMU::update() {
tnhnrl 66:0f20870117b7 47
tnhnrl 66:0f20870117b7 48 // // DEBUGGING an example packet
tnhnrl 66:0f20870117b7 49 // Serial pc(USBTX, USBRX);
tnhnrl 66:0f20870117b7 50 // //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
tnhnrl 66:0f20870117b7 51 // 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
tnhnrl 66:0f20870117b7 52 // for (int j=0; j<20; j++) {
tnhnrl 66:0f20870117b7 53 //byte = data[j];
tnhnrl 66:0f20870117b7 54
tnhnrl 66:0f20870117b7 55 while (_rs232.readable()) {
tnhnrl 66:0f20870117b7 56 // read a single byte
tnhnrl 66:0f20870117b7 57 byte = _rs232.getc();
tnhnrl 66:0f20870117b7 58
tnhnrl 66:0f20870117b7 59 // state machine to process byte-by-byte
tnhnrl 66:0f20870117b7 60 switch (state) {
tnhnrl 66:0f20870117b7 61 case SYNC0 :
tnhnrl 66:0f20870117b7 62 if (byte == 0x75) {
tnhnrl 66:0f20870117b7 63 packet[0] = byte; // save into the packet
tnhnrl 66:0f20870117b7 64 state = SYNC1;
tnhnrl 66:0f20870117b7 65 }
tnhnrl 66:0f20870117b7 66 break;
tnhnrl 66:0f20870117b7 67
tnhnrl 66:0f20870117b7 68 case SYNC1 :
tnhnrl 66:0f20870117b7 69 if (byte == 0x65) {
tnhnrl 66:0f20870117b7 70 packet[1] = byte; // save into the packet
tnhnrl 66:0f20870117b7 71 state = SET;
tnhnrl 66:0f20870117b7 72 }
tnhnrl 66:0f20870117b7 73 else {
tnhnrl 66:0f20870117b7 74 state = SYNC0;
tnhnrl 66:0f20870117b7 75 }
tnhnrl 66:0f20870117b7 76 break;
tnhnrl 66:0f20870117b7 77
tnhnrl 66:0f20870117b7 78 case SET :
tnhnrl 66:0f20870117b7 79 descriptor = byte; // save descriptor set
tnhnrl 66:0f20870117b7 80 packet[2] = byte; // save into the packet
tnhnrl 66:0f20870117b7 81 state = LEN;
tnhnrl 66:0f20870117b7 82 break;
tnhnrl 66:0f20870117b7 83
tnhnrl 66:0f20870117b7 84 case LEN :
tnhnrl 66:0f20870117b7 85 len = byte; // save payload length
tnhnrl 66:0f20870117b7 86 packet[3] = byte; // save into the packet
tnhnrl 66:0f20870117b7 87 state = PAY;
tnhnrl 66:0f20870117b7 88 i = 0; // reset payload field length counter
tnhnrl 66:0f20870117b7 89 break;
tnhnrl 66:0f20870117b7 90
tnhnrl 66:0f20870117b7 91 case PAY :
tnhnrl 66:0f20870117b7 92 if (i < len) { // keep adding until get all the payload length
tnhnrl 66:0f20870117b7 93 packet[4+i] = byte; // add byte to the packet, skipping over the header 4-bytes
tnhnrl 66:0f20870117b7 94 i++; // increment payload counter
tnhnrl 66:0f20870117b7 95 }
tnhnrl 66:0f20870117b7 96 else {
tnhnrl 66:0f20870117b7 97 state = CRC0;
tnhnrl 66:0f20870117b7 98 }
tnhnrl 66:0f20870117b7 99 if (i >= len) { // not an elseif, since we want to escape when i==len
tnhnrl 66:0f20870117b7 100 state = CRC0;
tnhnrl 66:0f20870117b7 101 }
tnhnrl 66:0f20870117b7 102 break;
tnhnrl 66:0f20870117b7 103
tnhnrl 66:0f20870117b7 104 case CRC0 :
tnhnrl 66:0f20870117b7 105 crc0 = byte; // save the msb of the checksum
tnhnrl 66:0f20870117b7 106 state = CRC1;
tnhnrl 66:0f20870117b7 107 break;
tnhnrl 66:0f20870117b7 108
tnhnrl 66:0f20870117b7 109 case CRC1 :
tnhnrl 66:0f20870117b7 110 crc1 = byte; // save the lsb of the checksum
tnhnrl 66:0f20870117b7 111 checksum = ((unsigned int)crc0 << 8) + (unsigned int)crc1; // make checksum into a uint16
tnhnrl 66:0f20870117b7 112 if (checksum == calcChecksum(packet, len+4)) { // passed checksum, wahoo!
tnhnrl 66:0f20870117b7 113 processPayload(descriptor, packet[4], &packet[4]); // process the payload part of the packet, starting at byte 4
tnhnrl 66:0f20870117b7 114 }
tnhnrl 66:0f20870117b7 115 state = SYNC0; // reset to SYNC0 state
tnhnrl 66:0f20870117b7 116 break;
tnhnrl 66:0f20870117b7 117
tnhnrl 66:0f20870117b7 118 default :
tnhnrl 66:0f20870117b7 119 state = SYNC0;
tnhnrl 66:0f20870117b7 120 }
tnhnrl 66:0f20870117b7 121 }
tnhnrl 66:0f20870117b7 122 return;
tnhnrl 66:0f20870117b7 123 }
tnhnrl 66:0f20870117b7 124
tnhnrl 66:0f20870117b7 125 void IMU::processPayload(char descriptor, char length, unsigned char * payload) {
tnhnrl 66:0f20870117b7 126 if (descriptor == IMU_DATA_SET) { // find an IMU data descriptor set
tnhnrl 66:0f20870117b7 127 if (length > 2) { // make sure there are at least two bytes to see the field descriptor
tnhnrl 66:0f20870117b7 128 if (payload[1] == EULER_CF_DESCRIPTOR) { // find an euler CF field descriptor
tnhnrl 66:0f20870117b7 129 processEulerCfPacket(length, payload);
tnhnrl 66:0f20870117b7 130 }
tnhnrl 66:0f20870117b7 131 }
tnhnrl 66:0f20870117b7 132 }
tnhnrl 66:0f20870117b7 133 else if (descriptor == GNSS_DATA_SET) { // find a GNSS data descriptor set
tnhnrl 66:0f20870117b7 134 if (length > 2) { // make sure there are at least two bytes to see the field descriptor
tnhnrl 66:0f20870117b7 135 if (payload[1] == LLH_POSITION_DESCRIPTOR) { // find a lat-lon-alt field descriptor
tnhnrl 66:0f20870117b7 136 processLatLonAltPacket(length, payload);
tnhnrl 66:0f20870117b7 137 }
tnhnrl 66:0f20870117b7 138 else if (payload[1] == GNSS_FIX_INFO_DESCRIPTOR) { // find a gnss fix field descriptor
tnhnrl 66:0f20870117b7 139 processGnssFixInformation(length, payload);
tnhnrl 66:0f20870117b7 140 }
tnhnrl 66:0f20870117b7 141 }
tnhnrl 66:0f20870117b7 142 }
tnhnrl 66:0f20870117b7 143 }
tnhnrl 66:0f20870117b7 144
tnhnrl 66:0f20870117b7 145 void IMU::processEulerCfPacket(char length, unsigned char * payload) {
tnhnrl 66:0f20870117b7 146 if (length >= EULER_CF_LENGTH) { // make sure correct field length
tnhnrl 66:0f20870117b7 147 if (payload[0] == EULER_CF_LENGTH) { // make sure field length is as expected
tnhnrl 66:0f20870117b7 148 euler[0] = floatFromChar(&payload[ROLL_OFFSET+2])*180/_PI; // roll Euler angle convert in degrees
tnhnrl 66:0f20870117b7 149 euler[1] = floatFromChar(&payload[PITCH_OFFSET+2])*180/_PI; // pitch Euler angle convert in degrees
tnhnrl 66:0f20870117b7 150 euler[2] = floatFromChar(&payload[YAW_OFFSET+2])*180/_PI; // yaw Euler angle convert in degrees
tnhnrl 66:0f20870117b7 151 }
tnhnrl 66:0f20870117b7 152 }
tnhnrl 66:0f20870117b7 153 }
tnhnrl 66:0f20870117b7 154
tnhnrl 66:0f20870117b7 155 void IMU::processLatLonAltPacket(char length, unsigned char * payload) {
tnhnrl 66:0f20870117b7 156 if (length >= LLH_POSITION_LENGTH) { // make sure correct field length
tnhnrl 66:0f20870117b7 157 if (payload[0] == LLH_POSITION_LENGTH) { // make sure field length is as expected
tnhnrl 66:0f20870117b7 158 latLonAlt[0] = doubleFromChar(&payload[LATITUDE_OFFSET+2]); // latitude in decimal degrees
tnhnrl 66:0f20870117b7 159 latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET+2]); // longitude in decimal degrees
tnhnrl 66:0f20870117b7 160 latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET+2]); // altitude above mean sea level in meters
tnhnrl 66:0f20870117b7 161 }
tnhnrl 66:0f20870117b7 162 }
tnhnrl 66:0f20870117b7 163 }
tnhnrl 66:0f20870117b7 164
tnhnrl 66:0f20870117b7 165 void IMU::processGnssFixInformation(char length, unsigned char * payload) {
tnhnrl 66:0f20870117b7 166 if (length >= GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
tnhnrl 66:0f20870117b7 167 if (payload[0] == GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
tnhnrl 66:0f20870117b7 168 is2dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x01; // bitand on LSB to see 2d fix flag
tnhnrl 66:0f20870117b7 169 is3dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x02; // bitand on LSB to see 3d fix flag
tnhnrl 66:0f20870117b7 170 numSV = char(payload[NUM_SV_OFFSET+2]); // number of GNSS satellite vehicles
tnhnrl 66:0f20870117b7 171 }
tnhnrl 66:0f20870117b7 172 }
tnhnrl 66:0f20870117b7 173 }
tnhnrl 66:0f20870117b7 174
tnhnrl 66:0f20870117b7 175 float IMU::floatFromChar(unsigned char * value) {
tnhnrl 66:0f20870117b7 176 unsigned char temp[4];
tnhnrl 66:0f20870117b7 177 temp[0] = value[3];
tnhnrl 66:0f20870117b7 178 temp[1] = value[2];
tnhnrl 66:0f20870117b7 179 temp[2] = value[1];
tnhnrl 66:0f20870117b7 180 temp[3] = value[0];
tnhnrl 66:0f20870117b7 181 return *(float *) temp;
tnhnrl 66:0f20870117b7 182 }
tnhnrl 66:0f20870117b7 183
tnhnrl 66:0f20870117b7 184 double IMU::doubleFromChar(unsigned char * value) {
tnhnrl 66:0f20870117b7 185 unsigned char temp[8];
tnhnrl 66:0f20870117b7 186 temp[0] = value[7];
tnhnrl 66:0f20870117b7 187 temp[1] = value[6];
tnhnrl 66:0f20870117b7 188 temp[2] = value[5];
tnhnrl 66:0f20870117b7 189 temp[3] = value[4];
tnhnrl 66:0f20870117b7 190 temp[4] = value[3];
tnhnrl 66:0f20870117b7 191 temp[5] = value[2];
tnhnrl 66:0f20870117b7 192 temp[6] = value[1];
tnhnrl 66:0f20870117b7 193 temp[7] = value[0];
tnhnrl 66:0f20870117b7 194 return *(double *) temp;
tnhnrl 66:0f20870117b7 195 }
tnhnrl 66:0f20870117b7 196
tnhnrl 66:0f20870117b7 197 float IMU::getRoll() {
tnhnrl 66:0f20870117b7 198 return euler[0];
tnhnrl 66:0f20870117b7 199 }
tnhnrl 66:0f20870117b7 200
tnhnrl 66:0f20870117b7 201 float IMU::getPitch() {
tnhnrl 66:0f20870117b7 202 return euler[1];
tnhnrl 66:0f20870117b7 203 }
tnhnrl 66:0f20870117b7 204
tnhnrl 66:0f20870117b7 205 float IMU::getHeading() {
tnhnrl 66:0f20870117b7 206 return euler[2];
tnhnrl 66:0f20870117b7 207 }
tnhnrl 66:0f20870117b7 208
tnhnrl 66:0f20870117b7 209 bool IMU::getIsValid2dFix() {
tnhnrl 66:0f20870117b7 210 return is2dFixValid;
tnhnrl 66:0f20870117b7 211 }
tnhnrl 66:0f20870117b7 212
tnhnrl 66:0f20870117b7 213 bool IMU::getIsValid3dFix() {
tnhnrl 66:0f20870117b7 214 return is3dFixValid;
tnhnrl 66:0f20870117b7 215 }
tnhnrl 66:0f20870117b7 216
tnhnrl 66:0f20870117b7 217 char IMU::getNumSV() {
tnhnrl 66:0f20870117b7 218 return numSV;
tnhnrl 66:0f20870117b7 219 }
tnhnrl 66:0f20870117b7 220
tnhnrl 66:0f20870117b7 221 double IMU::getLatitude() {
tnhnrl 66:0f20870117b7 222 return latLonAlt[0];
tnhnrl 66:0f20870117b7 223 }
tnhnrl 66:0f20870117b7 224
tnhnrl 66:0f20870117b7 225 double IMU::getLongitude() {
tnhnrl 66:0f20870117b7 226 return latLonAlt[1];
tnhnrl 66:0f20870117b7 227 }
tnhnrl 66:0f20870117b7 228
tnhnrl 66:0f20870117b7 229 double IMU::getAltitudeMSL() {
tnhnrl 66:0f20870117b7 230 return latLonAlt[2];
tnhnrl 66:0f20870117b7 231 }
tnhnrl 66:0f20870117b7 232
tnhnrl 66:0f20870117b7 233 unsigned int IMU::calcChecksum(unsigned char * mip_packet, char checksum_range) {
tnhnrl 66:0f20870117b7 234 unsigned char checksum_byte1 = 0;
tnhnrl 66:0f20870117b7 235 unsigned char checksum_byte2 = 0;
tnhnrl 66:0f20870117b7 236 unsigned int myChecksum = 0;
tnhnrl 66:0f20870117b7 237
tnhnrl 66:0f20870117b7 238 for (int i=0; i<checksum_range; i++) {
tnhnrl 66:0f20870117b7 239 checksum_byte1 += mip_packet[i];
tnhnrl 66:0f20870117b7 240 checksum_byte2 += checksum_byte1;
tnhnrl 66:0f20870117b7 241 }
tnhnrl 66:0f20870117b7 242 myChecksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
tnhnrl 66:0f20870117b7 243 return myChecksum;
tnhnrl 66:0f20870117b7 244 }