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@92:52a91656458a, 2019-05-13 (annotated)
- Committer:
- joel_ssc
- Date:
- Mon May 13 19:25:26 2019 +0000
- Revision:
- 92:52a91656458a
- Parent:
- 75:92e79d23d29a
version for first flight test, timeouts not yet set correctly
Who changed what in which revision?
User | Revision | Line number | New 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 | 75:92e79d23d29a | 198 | return euler[0]; |
tnhnrl | 66:0f20870117b7 | 199 | } |
tnhnrl | 66:0f20870117b7 | 200 | |
tnhnrl | 66:0f20870117b7 | 201 | float IMU::getPitch() { |
tnhnrl | 75:92e79d23d29a | 202 | return euler[1]; |
tnhnrl | 66:0f20870117b7 | 203 | } |
tnhnrl | 66:0f20870117b7 | 204 | |
tnhnrl | 66:0f20870117b7 | 205 | float IMU::getHeading() { |
tnhnrl | 75:92e79d23d29a | 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 | } |