Flying Sea Glider
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
Embed:
(wiki syntax)
Show/hide line numbers
Gui.cpp
00001 #include "Gui.hpp" 00002 #include "StaticDefs.hpp" 00003 00004 // 0x FE ED CMD PL CRC1 CRC2 00005 00006 // SIT_IDLE, CHECK_TUNING, FIND_NEUTRAL, DIVE, RISE, FLOAT_LEVEL, FLOAT_BROADCAST, EMERGENCY_CLIMB, MULTI_DIVE, MULTI_RISE, POSITION_DIVE, POSITION_RISE, KEYBOARD, TRANSMIT_LOG, RECEIVE_SEQUENCE, PITCH_TUNER_DEPTH, PITCH_TUNER_RUN, SEND_STATUS 00007 00008 Gui::Gui() { 00009 } 00010 00011 //calculate the crc with an integer array 00012 int Gui::guiCalcCrc1(int *input_array, int array_length) { 00013 int crc = 0; 00014 00015 int crc_table[] = {0, 49345, 49537, 320, 49921, 960, 640, 49729, 50689, 1728, 1920, 51009, 1280, 50625, 50305, 1088, 52225, 3264, 3456, 52545, 3840, 53185, 52865, 3648, 2560, 51905, 52097, 2880, 51457, 2496, 2176, 51265, 55297, 6336, 6528, 55617, 6912, 56257, 55937, 6720, 7680, 57025, 57217, 8000, 56577, 7616, 7296, 56385, 5120, 54465, 54657, 5440, 55041, 6080, 5760, 54849, 53761, 4800, 4992, 54081, 4352, 53697, 53377, 4160, 61441, 12480, 12672, 61761, 13056, 62401, 62081, 12864, 13824, 63169, 63361, 14144, 62721, 13760, 13440, 62529, 15360, 64705, 64897, 15680, 65281, 16320, 16000, 65089, 64001, 15040, 15232, 64321, 14592, 63937, 63617, 14400, 10240, 59585, 59777, 10560, 60161, 11200, 10880, 59969, 60929, 11968, 12160, 61249, 11520, 60865, 60545, 11328, 58369, 9408, 9600, 58689, 9984, 59329, 59009, 9792, 8704, 58049, 58241, 9024, 57601, 8640, 8320, 57409, 40961, 24768, 24960, 41281, 25344, 41921, 41601, 25152, 26112, 42689, 42881, 26432, 42241, 26048, 25728, 42049, 27648, 44225, 44417, 27968, 44801, 28608, 28288, 44609, 43521, 27328, 27520, 43841, 26880, 43457, 43137, 26688, 30720, 47297, 47489, 31040, 47873, 31680, 31360, 47681, 48641, 32448, 32640, 48961, 32000, 48577, 48257, 31808, 46081, 29888, 30080, 46401, 30464, 47041, 46721, 30272, 29184, 45761, 45953, 29504, 45313, 29120, 28800, 45121, 20480, 37057, 37249, 20800, 37633, 21440, 21120, 37441, 38401, 22208, 22400, 38721, 21760, 38337, 38017, 21568, 39937, 23744, 23936, 40257, 24320, 40897, 40577, 24128, 23040, 39617, 39809, 23360, 39169, 22976, 22656, 38977, 34817, 18624, 18816, 35137, 19200, 35777, 35457, 19008, 19968, 36545, 36737, 20288, 36097, 19904, 19584, 35905, 17408, 33985, 34177, 17728, 34561, 18368, 18048, 34369, 33281, 17088, 17280, 33601, 16640, 33217, 32897, 16448}; 00016 00017 for (int z = 0; z < array_length; z++) { 00018 crc = (crc_table[(input_array[z] ^ crc) & 0xff] ^ (crc >> 8)) & 0xFFFF; 00019 } 00020 00021 return crc / 256; //second-to-last byte 00022 } 00023 00024 int Gui::guiCalcCrc2(int *input_array, int array_length) { 00025 int crc = 0; 00026 int crc_table[] = {0, 49345, 49537, 320, 49921, 960, 640, 49729, 50689, 1728, 1920, 51009, 1280, 50625, 50305, 1088, 52225, 3264, 3456, 52545, 3840, 53185, 52865, 3648, 2560, 51905, 52097, 2880, 51457, 2496, 2176, 51265, 55297, 6336, 6528, 55617, 6912, 56257, 55937, 6720, 7680, 57025, 57217, 8000, 56577, 7616, 7296, 56385, 5120, 54465, 54657, 5440, 55041, 6080, 5760, 54849, 53761, 4800, 4992, 54081, 4352, 53697, 53377, 4160, 61441, 12480, 12672, 61761, 13056, 62401, 62081, 12864, 13824, 63169, 63361, 14144, 62721, 13760, 13440, 62529, 15360, 64705, 64897, 15680, 65281, 16320, 16000, 65089, 64001, 15040, 15232, 64321, 14592, 63937, 63617, 14400, 10240, 59585, 59777, 10560, 60161, 11200, 10880, 59969, 60929, 11968, 12160, 61249, 11520, 60865, 60545, 11328, 58369, 9408, 9600, 58689, 9984, 59329, 59009, 9792, 8704, 58049, 58241, 9024, 57601, 8640, 8320, 57409, 40961, 24768, 24960, 41281, 25344, 41921, 41601, 25152, 26112, 42689, 42881, 26432, 42241, 26048, 25728, 42049, 27648, 44225, 44417, 27968, 44801, 28608, 28288, 44609, 43521, 27328, 27520, 43841, 26880, 43457, 43137, 26688, 30720, 47297, 47489, 31040, 47873, 31680, 31360, 47681, 48641, 32448, 32640, 48961, 32000, 48577, 48257, 31808, 46081, 29888, 30080, 46401, 30464, 47041, 46721, 30272, 29184, 45761, 45953, 29504, 45313, 29120, 28800, 45121, 20480, 37057, 37249, 20800, 37633, 21440, 21120, 37441, 38401, 22208, 22400, 38721, 21760, 38337, 38017, 21568, 39937, 23744, 23936, 40257, 24320, 40897, 40577, 24128, 23040, 39617, 39809, 23360, 39169, 22976, 22656, 38977, 34817, 18624, 18816, 35137, 19200, 35777, 35457, 19008, 19968, 36545, 36737, 20288, 36097, 19904, 19584, 35905, 17408, 33985, 34177, 17728, 34561, 18368, 18048, 34369, 33281, 17088, 17280, 33601, 16640, 33217, 32897, 16448}; 00027 00028 for (int z = 0; z < array_length; z++) { 00029 crc = (crc_table[(input_array[z] ^ crc) & 0xff] ^ (crc >> 8)) & 0xFFFF; 00030 } 00031 return crc % 256; //second-to-last byte 00032 } 00033 00034 int Gui::calcCrcOneVector(vector <int> crc_packet) { 00035 std::vector<int>::iterator itr; 00036 00037 //can't initialize the table in the constructor in c++ 00038 int crc_table [256] = {0, 49345, 49537, 320, 49921, 960, 640, 49729, 50689, 1728, 1920, 51009, 1280, 50625, 50305, 1088, 52225, 3264, 3456, 52545, 3840, 53185, 52865, 3648, 2560, 51905, 52097, 2880, 51457, 2496, 2176, 51265, 55297, 6336, 6528, 55617, 6912, 56257, 55937, 6720, 7680, 57025, 57217, 8000, 56577, 7616, 7296, 56385, 5120, 54465, 54657, 5440, 55041, 6080, 5760, 54849, 53761, 4800, 4992, 54081, 4352, 53697, 53377, 4160, 61441, 12480, 12672, 61761, 13056, 62401, 62081, 12864, 13824, 63169, 63361, 14144, 62721, 13760, 13440, 62529, 15360, 64705, 64897, 15680, 65281, 16320, 16000, 65089, 64001, 15040, 15232, 64321, 14592, 63937, 63617, 14400, 10240, 59585, 59777, 10560, 60161, 11200, 10880, 59969, 60929, 11968, 12160, 61249, 11520, 60865, 60545, 11328, 58369, 9408, 9600, 58689, 9984, 59329, 59009, 9792, 8704, 58049, 58241, 9024, 57601, 8640, 8320, 57409, 40961, 24768, 24960, 41281, 25344, 41921, 41601, 25152, 26112, 42689, 42881, 26432, 42241, 26048, 25728, 42049, 27648, 44225, 44417, 27968, 44801, 28608, 28288, 44609, 43521, 27328, 27520, 43841, 26880, 43457, 43137, 26688, 30720, 47297, 47489, 31040, 47873, 31680, 31360, 47681, 48641, 32448, 32640, 48961, 32000, 48577, 48257, 31808, 46081, 29888, 30080, 46401, 30464, 47041, 46721, 30272, 29184, 45761, 45953, 29504, 45313, 29120, 28800, 45121, 20480, 37057, 37249, 20800, 37633, 21440, 21120, 37441, 38401, 22208, 22400, 38721, 21760, 38337, 38017, 21568, 39937, 23744, 23936, 40257, 24320, 40897, 40577, 24128, 23040, 39617, 39809, 23360, 39169, 22976, 22656, 38977, 34817, 18624, 18816, 35137, 19200, 35777, 35457, 19008, 19968, 36545, 36737, 20288, 36097, 19904, 19584, 35905, 17408, 33985, 34177, 17728, 34561, 18368, 18048, 34369, 33281, 17088, 17280, 33601, 16640, 33217, 32897, 16448}; 00039 00040 int crc = 0; 00041 for (itr = crc_packet.begin(); itr < crc_packet.end(); itr++) 00042 crc = (crc_table[(*itr ^ crc) & 0xff] ^ (crc >> 8)) & 0xFFFF; 00043 00044 return crc / 256; //second-to-last byte 00045 } 00046 00047 int Gui::calcCrcTwoVector(vector <int> crc_packet) { 00048 std::vector<int>::iterator itr; 00049 00050 int crc_table [256] = {0, 49345, 49537, 320, 49921, 960, 640, 49729, 50689, 1728, 1920, 51009, 1280, 50625, 50305, 1088, 52225, 3264, 3456, 52545, 3840, 53185, 52865, 3648, 2560, 51905, 52097, 2880, 51457, 2496, 2176, 51265, 55297, 6336, 6528, 55617, 6912, 56257, 55937, 6720, 7680, 57025, 57217, 8000, 56577, 7616, 7296, 56385, 5120, 54465, 54657, 5440, 55041, 6080, 5760, 54849, 53761, 4800, 4992, 54081, 4352, 53697, 53377, 4160, 61441, 12480, 12672, 61761, 13056, 62401, 62081, 12864, 13824, 63169, 63361, 14144, 62721, 13760, 13440, 62529, 15360, 64705, 64897, 15680, 65281, 16320, 16000, 65089, 64001, 15040, 15232, 64321, 14592, 63937, 63617, 14400, 10240, 59585, 59777, 10560, 60161, 11200, 10880, 59969, 60929, 11968, 12160, 61249, 11520, 60865, 60545, 11328, 58369, 9408, 9600, 58689, 9984, 59329, 59009, 9792, 8704, 58049, 58241, 9024, 57601, 8640, 8320, 57409, 40961, 24768, 24960, 41281, 25344, 41921, 41601, 25152, 26112, 42689, 42881, 26432, 42241, 26048, 25728, 42049, 27648, 44225, 44417, 27968, 44801, 28608, 28288, 44609, 43521, 27328, 27520, 43841, 26880, 43457, 43137, 26688, 30720, 47297, 47489, 31040, 47873, 31680, 31360, 47681, 48641, 32448, 32640, 48961, 32000, 48577, 48257, 31808, 46081, 29888, 30080, 46401, 30464, 47041, 46721, 30272, 29184, 45761, 45953, 29504, 45313, 29120, 28800, 45121, 20480, 37057, 37249, 20800, 37633, 21440, 21120, 37441, 38401, 22208, 22400, 38721, 21760, 38337, 38017, 21568, 39937, 23744, 23936, 40257, 24320, 40897, 40577, 24128, 23040, 39617, 39809, 23360, 39169, 22976, 22656, 38977, 34817, 18624, 18816, 35137, 19200, 35777, 35457, 19008, 19968, 36545, 36737, 20288, 36097, 19904, 19584, 35905, 17408, 33985, 34177, 17728, 34561, 18368, 18048, 34369, 33281, 17088, 17280, 33601, 16640, 33217, 32897, 16448}; 00051 00052 int crc = 0; 00053 for (itr = crc_packet.begin(); itr < crc_packet.end(); itr++) 00054 crc = (crc_table[(*itr ^ crc) & 0xff] ^ (crc >> 8)) & 0xFFFF; 00055 00056 return crc % 256; //last byte 00057 } 00058 00059 void Gui::transmitDataPacket(vector <int> full_packet) { 00060 std::vector<int>::iterator itr; 00061 00062 //transmit full packet 00063 for (itr = full_packet.begin(); itr < full_packet.end(); itr++) { 00064 xbee().putc(*itr); //send integers over serial port one byte at a time 00065 } 00066 } 00067 00068 // get command one byte at a time 00069 void Gui::getCommandFSM() { 00070 static int fsm_command = -1; 00071 static int command_state = HEADER_FE; //state in switch statement 00072 int incoming_byte = -1; //reset each time a character is read 00073 static int input_packet[4]; //changed from char in previous iteration 03/28/2018 00074 static int command_crc_one = 0; //hold crc values until they're reset with calculations 00075 static int command_crc_two = 0; 00076 static int command_packet_size = -1; 00077 00078 if (xbee().readable()) { 00079 incoming_byte = xbee().getc(); 00080 00081 switch(command_state) { 00082 case HEADER_FE: 00083 //continue processing 00084 if (incoming_byte == 254){ //FE 00085 command_state = HEADER_ED; 00086 input_packet[0] = 254; 00087 } 00088 //did not receive byte 1 00089 else { 00090 command_state = HEADER_FE; //go back to checking the first packet 00091 } 00092 break; 00093 00094 case HEADER_ED: 00095 if(incoming_byte == 237) { //ED 00096 command_state = COMMAND_PACKET; 00097 input_packet[1] = 237; 00098 } 00099 00100 //did not get the correct second byte 00101 else { 00102 command_state = HEADER_FE; //go back to checking the first packet 00103 } 00104 00105 break; 00106 00107 case COMMAND_PACKET: 00108 // EMERGENCY_CLIMB, MULTI_DIVE, MULTI_RISE, POSITION_DIVE, POSITION_RISE, KEYBOARD, TRANSMIT_LOG, RECEIVE_SEQUENCE, PITCH_TUNER_DEPTH, PITCH_TUNER_RUN, SEND_STATUS 00109 00110 //note these ENUM values come from the StateMachine 00111 00112 if (incoming_byte == 1) 00113 fsm_command = CHECK_TUNING; 00114 else if (incoming_byte == 2) 00115 fsm_command = FIND_NEUTRAL; 00116 else if (incoming_byte == 3) 00117 fsm_command = DIVE; 00118 else if (incoming_byte == 4) 00119 fsm_command = RISE; 00120 else if (incoming_byte == 5) 00121 fsm_command = FLOAT_LEVEL; 00122 else if (incoming_byte == 6) 00123 fsm_command = FLOAT_BROADCAST; 00124 else if (incoming_byte == 7) 00125 fsm_command = EMERGENCY_CLIMB; 00126 else if (incoming_byte == 8) 00127 fsm_command = MULTI_DIVE; 00128 else if (incoming_byte == 9) 00129 fsm_command = MULTI_RISE; 00130 else if (incoming_byte == 10) 00131 fsm_command = POSITION_DIVE; 00132 else if (incoming_byte == 11) 00133 fsm_command = POSITION_RISE; 00134 //SKIP 13 00135 else if (incoming_byte == 12) 00136 fsm_command = TX_MBED_LOG; 00137 else if (incoming_byte == 13) 00138 fsm_command = RX_SEQUENCE; 00139 else { 00140 command_state = HEADER_FE; //go back to checking the first packet 00141 } 00142 00143 //state 00144 command_state = FSM_PACKET_SIZE; 00145 00146 input_packet[2] = incoming_byte; 00147 00148 break; 00149 00150 case FSM_PACKET_SIZE: 00151 if (incoming_byte >= 0) { 00152 command_packet_size = incoming_byte; 00153 } 00154 00155 command_state = FSM_CRC_ONE; 00156 00157 input_packet[3] = incoming_byte; 00158 00159 break; 00160 00161 case FSM_CRC_ONE: 00162 command_crc_one = guiCalcCrc1(input_packet, 4); //calc CRC 1 from the input packet (size 4) 00163 00164 if (incoming_byte == command_crc_one) { 00165 command_state = FSM_CRC_TWO; 00166 } 00167 00168 else 00169 command_state = HEADER_FE; 00170 //or state remains the same? 00171 00172 break; 00173 00174 case FSM_CRC_TWO: 00175 command_crc_two = guiCalcCrc2(input_packet, 4); //calc CRC 2 from the input packet (size 4) 00176 00177 //check if CRC TWO is correct (then send full packet) 00178 if (incoming_byte == command_crc_two) { 00179 //set state of statemachine 00180 stateMachine().setState(fsm_command); 00181 xbee().printf("CRC 1 and CRC 2 IS GOOD! fsm_command is %d\n\r", fsm_command); 00182 } 00183 00184 command_state = HEADER_FE; 00185 00186 break; 00187 } /* switch statement complete */ 00188 } /* end of pc readable */ 00189 } 00190 00191 //remove class vector? 00192 00193 void Gui::updateGUI() { 00194 float roll_value = imu().getRoll(); 00195 float pitch_value = imu().getPitch(); 00196 float heading_value = imu().getHeading(); 00197 float depth_value = depthLoop().getPosition(); //filtered depth position 00198 float timer_value = stateMachine().getTimerValue(); 00199 00200 xbee().printf("roll %0.2f / pitch %0.2f / heading %0.2f / depth %0.2f / timer %0.2f\n\r", roll_value, pitch_value, heading_value, depth_value, timer_value); 00201 00202 vector <int> gui_update_packet; 00203 00204 //ROLL PITCH HEADING(YAW) DEPTH TIMER (sending all at once, at one second intervals) 00205 00206 //take float value, convert to unsigned char, reverse and send 00207 const unsigned char * ptr_roll_value = reinterpret_cast<const unsigned char*>(&roll_value); 00208 const unsigned char * ptr_pitch_value = reinterpret_cast<const unsigned char*>(&pitch_value); 00209 const unsigned char * ptr_heading_value = reinterpret_cast<const unsigned char*>(&heading_value); 00210 const unsigned char * ptr_depth_value = reinterpret_cast<const unsigned char*>(&depth_value); 00211 const unsigned char * ptr_timer_value = reinterpret_cast<const unsigned char*>(&timer_value); 00212 //xbee().printf("DEBUG: timer_value is %f or HEX: %x %x %x %x\n\r",timer_value,ptr_timer_value[3],ptr_timer_value[2],ptr_timer_value[1],ptr_timer_value[0]); 00213 00214 // BE AD GUI GUI LENGTH DATA DATA CC CC 00215 00216 gui_update_packet.clear(); 00217 00218 //DATA PACKET HEADER 00219 gui_update_packet.push_back(121); // y = 0x79 00220 gui_update_packet.push_back(113); // q = 0x71 00221 00222 gui_update_packet.push_back(204); // 0xCC 00223 gui_update_packet.push_back(204); // 0xCC 00224 00225 gui_update_packet.push_back(20); // 0x14 (length) 00226 00227 gui_update_packet.push_back(ptr_roll_value[3]); 00228 gui_update_packet.push_back(ptr_roll_value[2]); 00229 gui_update_packet.push_back(ptr_roll_value[1]); 00230 gui_update_packet.push_back(ptr_roll_value[0]); 00231 00232 gui_update_packet.push_back(ptr_pitch_value[3]); 00233 gui_update_packet.push_back(ptr_pitch_value[2]); 00234 gui_update_packet.push_back(ptr_pitch_value[1]); 00235 gui_update_packet.push_back(ptr_pitch_value[0]); 00236 00237 gui_update_packet.push_back(ptr_heading_value[3]); 00238 gui_update_packet.push_back(ptr_heading_value[2]); 00239 gui_update_packet.push_back(ptr_heading_value[1]); 00240 gui_update_packet.push_back(ptr_heading_value[0]); 00241 00242 gui_update_packet.push_back(ptr_depth_value[3]); 00243 gui_update_packet.push_back(ptr_depth_value[2]); 00244 gui_update_packet.push_back(ptr_depth_value[1]); 00245 gui_update_packet.push_back(ptr_depth_value[0]); 00246 00247 gui_update_packet.push_back(ptr_timer_value[3]); 00248 gui_update_packet.push_back(ptr_timer_value[2]); 00249 gui_update_packet.push_back(ptr_timer_value[1]); 00250 gui_update_packet.push_back(ptr_timer_value[0]); 00251 00252 //CRC CALCULATION 00253 int crc_one = calcCrcOneVector(gui_update_packet); 00254 int crc_two = calcCrcTwoVector(gui_update_packet); 00255 00256 //place the crc bytes into the data packet that is transmitted 00257 gui_update_packet.push_back(crc_one); 00258 gui_update_packet.push_back(crc_two); 00259 00260 //transmit the full packet 00261 transmitDataPacket(gui_update_packet); 00262 }
Generated on Thu Jul 14 2022 10:54:35 by
1.7.2