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@10:085ab7328054, 2017-10-23 (annotated)

Committer:
danstrider
Date:
Mon Oct 23 12:50:53 2017 +0000
Revision:
10:085ab7328054
Parent:
9:d5fcdcb3c89d
Child:
11:3b241ecb75ed
checked out on the hardware

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() {
mkelly10 9:d5fcdcb3c89d 33 interval.attach(this, &IMU::update, 1); //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 10:085ab7328054 43 while (_rs232.readable()) {
danstrider 10:085ab7328054 44 // read a single byte
danstrider 10:085ab7328054 45 byte = _rs232.getc();
danstrider 10:085ab7328054 46
danstrider 10:085ab7328054 47 // state machine to process byte-by-byte
danstrider 10:085ab7328054 48 switch (state) {
danstrider 10:085ab7328054 49 case SYNC0 :
danstrider 10:085ab7328054 50 if (byte == 0x75) {
danstrider 10:085ab7328054 51 state = SYNC1;
danstrider 10:085ab7328054 52 }
danstrider 10:085ab7328054 53 break;
danstrider 10:085ab7328054 54
danstrider 10:085ab7328054 55 case SYNC1 :
danstrider 10:085ab7328054 56 if (byte == 0x65) {
danstrider 10:085ab7328054 57 state = SET;
danstrider 10:085ab7328054 58 }
danstrider 10:085ab7328054 59 state = SYNC0;
danstrider 10:085ab7328054 60 break;
danstrider 10:085ab7328054 61
danstrider 10:085ab7328054 62 case SET :
danstrider 10:085ab7328054 63 descriptor = byte; // save descriptor set
danstrider 10:085ab7328054 64 state = LEN;
danstrider 10:085ab7328054 65 break;
danstrider 10:085ab7328054 66
danstrider 10:085ab7328054 67 case LEN :
danstrider 10:085ab7328054 68 len = byte; // save payload length
danstrider 10:085ab7328054 69 state = PAY;
danstrider 10:085ab7328054 70 i = 0; // reset payload length counter
danstrider 10:085ab7328054 71 break;
danstrider 10:085ab7328054 72
danstrider 10:085ab7328054 73 case PAY :
danstrider 10:085ab7328054 74 if (i < len) {
danstrider 10:085ab7328054 75 payload[i] = byte; // add byte to the payload array
danstrider 10:085ab7328054 76 i++; // increment payload counter
danstrider 10:085ab7328054 77 }
danstrider 10:085ab7328054 78 else {
danstrider 10:085ab7328054 79 state = CRC0;
danstrider 10:085ab7328054 80 }
danstrider 10:085ab7328054 81 break;
danstrider 10:085ab7328054 82
danstrider 10:085ab7328054 83 case CRC0 :
danstrider 10:085ab7328054 84 crc0 = byte;
danstrider 10:085ab7328054 85 state = CRC1;
danstrider 10:085ab7328054 86 break;
danstrider 10:085ab7328054 87
danstrider 10:085ab7328054 88 case CRC1 :
danstrider 10:085ab7328054 89 crc1 = byte;
danstrider 10:085ab7328054 90 checksum = crc0<<8 + crc1; // make checksum
danstrider 10:085ab7328054 91 if (checksum == calcChecksum(payload, len)) {
danstrider 10:085ab7328054 92 processPacket(descriptor, len, payload); // process packet
danstrider 10:085ab7328054 93 }
danstrider 10:085ab7328054 94 state = SYNC0; // reset to SYNC0 state
danstrider 10:085ab7328054 95 break;
mkelly10 9:d5fcdcb3c89d 96
danstrider 10:085ab7328054 97 default :
danstrider 10:085ab7328054 98 state = SYNC0;
mkelly10 9:d5fcdcb3c89d 99 }
danstrider 10:085ab7328054 100 }
danstrider 10:085ab7328054 101 return;
mkelly10 9:d5fcdcb3c89d 102 }
mkelly10 9:d5fcdcb3c89d 103
danstrider 10:085ab7328054 104 void IMU::processPacket(char descriptor, char length, unsigned char * payload) {
danstrider 10:085ab7328054 105 if (descriptor == IMU_DATA_SET) { // find an IMU data descriptor set
danstrider 10:085ab7328054 106 if (length > 2) { // make sure there are at least two bytes to see the field descriptor
danstrider 10:085ab7328054 107 if (payload[1] == EULER_CF_DESCRIPTOR) { // find an euler CF field descriptor
danstrider 10:085ab7328054 108 processEulerCfPacket(length, payload);
danstrider 10:085ab7328054 109 }
danstrider 10:085ab7328054 110 }
danstrider 10:085ab7328054 111 }
danstrider 10:085ab7328054 112 else if (descriptor == GNSS_DATA_SET) { // find a GNSS data descriptor set
danstrider 10:085ab7328054 113 if (length > 2) { // make sure there are at least two bytes to see the field descriptor
danstrider 10:085ab7328054 114 if (payload[1] == LLH_POSITION_DESCRIPTOR) { // find a lat-lon-alt field descriptor
danstrider 10:085ab7328054 115 processLatLonAltPacket(length, payload);
danstrider 10:085ab7328054 116 }
danstrider 10:085ab7328054 117 else if (payload[1] == GNSS_FIX_INFO_DESCRIPTOR) { // find a gnss fix field descriptor
danstrider 10:085ab7328054 118 processGnssFixInformation(length, payload);
danstrider 10:085ab7328054 119 }
danstrider 10:085ab7328054 120 }
danstrider 10:085ab7328054 121 }
danstrider 10:085ab7328054 122 }
danstrider 10:085ab7328054 123
danstrider 10:085ab7328054 124 void IMU::processEulerCfPacket(char length, unsigned char * payload) {
danstrider 10:085ab7328054 125 if (length >= EULER_CF_LENGTH) { // make sure correct field length
danstrider 10:085ab7328054 126 if (payload[0] == EULER_CF_LENGTH) { // make sure field length is as expected
danstrider 10:085ab7328054 127 euler[0] = floatFromChar(&payload[ROLL_OFFSET])*180/_PI; // roll Euler angle convert in degrees
danstrider 10:085ab7328054 128 euler[1] = floatFromChar(&payload[PITCH_OFFSET])*180/_PI; // pitch Euler angle convert in degrees
danstrider 10:085ab7328054 129 euler[2] = floatFromChar(&payload[YAW_OFFSET])*180/_PI; // yaw Euler angle convert in degrees
danstrider 10:085ab7328054 130 }
danstrider 10:085ab7328054 131 }
danstrider 10:085ab7328054 132 }
danstrider 10:085ab7328054 133
danstrider 10:085ab7328054 134 void IMU::processLatLonAltPacket(char length, unsigned char * payload) {
danstrider 10:085ab7328054 135 if (length >= LLH_POSITION_LENGTH) { // make sure correct field length
danstrider 10:085ab7328054 136 if (payload[0] == LLH_POSITION_LENGTH) { // make sure field length is as expected
danstrider 10:085ab7328054 137 latLonAlt[0] = doubleFromChar(&payload[LATITUDE_OFFSET]); // latitude in decimal degrees
danstrider 10:085ab7328054 138 latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET]); // longitude in decimal degrees
danstrider 10:085ab7328054 139 latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET]); // altitude above mean sea level in meters
danstrider 10:085ab7328054 140 }
danstrider 10:085ab7328054 141 }
danstrider 10:085ab7328054 142 }
danstrider 10:085ab7328054 143
danstrider 10:085ab7328054 144 void IMU::processGnssFixInformation(char length, unsigned char * data) {
danstrider 10:085ab7328054 145 if (length >= GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
danstrider 10:085ab7328054 146 if (payload[0] == GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
danstrider 10:085ab7328054 147 is2dFixValid = char(payload[FIX_TYPE_OFFSET]) & 0x01; // bitand on LSB to see 2d fix flag
danstrider 10:085ab7328054 148 is3dFixValid = char(payload[FIX_TYPE_OFFSET]) & 0x02; // bitand on LSB to see 3d fix flag
danstrider 10:085ab7328054 149 numSV = char(payload[NUM_SV_OFFSET]); // number of GNSS satellite vehicles
danstrider 10:085ab7328054 150 }
danstrider 10:085ab7328054 151 }
danstrider 10:085ab7328054 152 }
danstrider 10:085ab7328054 153
danstrider 10:085ab7328054 154 float IMU::floatFromChar(unsigned char * value) {
mkelly10 9:d5fcdcb3c89d 155 unsigned char temp[4];
mkelly10 9:d5fcdcb3c89d 156 temp[0] = value[3];
mkelly10 9:d5fcdcb3c89d 157 temp[1] = value[2];
mkelly10 9:d5fcdcb3c89d 158 temp[2] = value[1];
mkelly10 9:d5fcdcb3c89d 159 temp[3] = value[0];
mkelly10 9:d5fcdcb3c89d 160 return *(float *) temp;
mkelly10 9:d5fcdcb3c89d 161 }
mkelly10 9:d5fcdcb3c89d 162
danstrider 10:085ab7328054 163 double IMU::doubleFromChar(unsigned char * value) {
danstrider 10:085ab7328054 164 unsigned char temp[8];
danstrider 10:085ab7328054 165 temp[0] = value[7];
danstrider 10:085ab7328054 166 temp[1] = value[6];
danstrider 10:085ab7328054 167 temp[2] = value[5];
danstrider 10:085ab7328054 168 temp[3] = value[4];
danstrider 10:085ab7328054 169 temp[4] = value[3];
danstrider 10:085ab7328054 170 temp[5] = value[2];
danstrider 10:085ab7328054 171 temp[6] = value[1];
danstrider 10:085ab7328054 172 temp[7] = value[0];
danstrider 10:085ab7328054 173 return *(double *) temp;
danstrider 10:085ab7328054 174 }
danstrider 10:085ab7328054 175
danstrider 10:085ab7328054 176 float IMU::getRoll() {
danstrider 10:085ab7328054 177 return euler[0];
danstrider 10:085ab7328054 178 }
danstrider 10:085ab7328054 179
danstrider 10:085ab7328054 180 float IMU::getPitch() {
danstrider 10:085ab7328054 181 return euler[1];
danstrider 10:085ab7328054 182 }
danstrider 10:085ab7328054 183
danstrider 10:085ab7328054 184 float IMU::getHeading() {
danstrider 10:085ab7328054 185 return euler[2];
danstrider 10:085ab7328054 186 }
danstrider 10:085ab7328054 187
danstrider 10:085ab7328054 188 bool IMU::getIsValid2dFix() {
danstrider 10:085ab7328054 189 return is2dFixValid;
danstrider 10:085ab7328054 190 }
danstrider 10:085ab7328054 191
danstrider 10:085ab7328054 192 bool IMU::getIsValid3dFix() {
danstrider 10:085ab7328054 193 return is3dFixValid;
danstrider 10:085ab7328054 194 }
danstrider 10:085ab7328054 195
danstrider 10:085ab7328054 196 char IMU::getNumSV() {
danstrider 10:085ab7328054 197 return numSV;
danstrider 10:085ab7328054 198 }
danstrider 10:085ab7328054 199
danstrider 10:085ab7328054 200 double IMU::getLatitude() {
danstrider 10:085ab7328054 201 return latLonAlt[0];
danstrider 10:085ab7328054 202 }
danstrider 10:085ab7328054 203
danstrider 10:085ab7328054 204 double IMU::getLongitude() {
danstrider 10:085ab7328054 205 return latLonAlt[1];
danstrider 10:085ab7328054 206 }
danstrider 10:085ab7328054 207
danstrider 10:085ab7328054 208 double IMU::getAltitudeMSL() {
danstrider 10:085ab7328054 209 return latLonAlt[2];
danstrider 10:085ab7328054 210 }
danstrider 10:085ab7328054 211
danstrider 10:085ab7328054 212 unsigned int IMU::calcChecksum(unsigned char * mip_packet, char checksum_range) {
danstrider 10:085ab7328054 213 unsigned char checksum_byte1 = 0;
danstrider 10:085ab7328054 214 unsigned char checksum_byte2 = 0;
danstrider 10:085ab7328054 215
danstrider 10:085ab7328054 216 for (int i=0; i<checksum_range; i++) {
danstrider 10:085ab7328054 217 checksum_byte1 += mip_packet[i];
danstrider 10:085ab7328054 218 checksum_byte2 += checksum_byte1;
danstrider 10:085ab7328054 219 }
danstrider 10:085ab7328054 220 checksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
danstrider 10:085ab7328054 221 return checksum;
danstrider 10:085ab7328054 222 }