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@11:3b241ecb75ed, 2017-10-27 (annotated)

Committer:
danstrider
Date:
Fri Oct 27 00:37:32 2017 +0000
Revision:
11:3b241ecb75ed
Parent:
10:085ab7328054 
This version has been in the pool, working with all the hardware.  Had occasional string pot problems and got stuck in RISE, letting the battery stall out against the endcap.

Who changed what in which revision?

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