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

GUI/Gui.cpp@92:52a91656458a, 2019-05-13 (annotated)

Committer:
joel_ssc
Date:
Mon May 13 19:25:26 2019 +0000
Revision:
92:52a91656458a
Parent:
74:d281aaef9766 
version for first flight test, timeouts not yet set correctly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 73:f6f378311c8d 1 #include "Gui.hpp"
tnhnrl 73:f6f378311c8d 2 #include "StaticDefs.hpp"
tnhnrl 73:f6f378311c8d 3
tnhnrl 73:f6f378311c8d 4 // 0x FE ED CMD PL CRC1 CRC2
tnhnrl 73:f6f378311c8d 5
tnhnrl 73:f6f378311c8d 6 // 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
tnhnrl 73:f6f378311c8d 7
tnhnrl 73:f6f378311c8d 8 Gui::Gui() {
tnhnrl 73:f6f378311c8d 9 }
tnhnrl 73:f6f378311c8d 10
tnhnrl 73:f6f378311c8d 11 //calculate the crc with an integer array
tnhnrl 73:f6f378311c8d 12 int Gui::guiCalcCrc1(int *input_array, int array_length) {
tnhnrl 73:f6f378311c8d 13 int crc = 0;
tnhnrl 73:f6f378311c8d 14
tnhnrl 73:f6f378311c8d 15 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};
tnhnrl 73:f6f378311c8d 16
tnhnrl 73:f6f378311c8d 17 for (int z = 0; z < array_length; z++) {
tnhnrl 73:f6f378311c8d 18 crc = (crc_table[(input_array[z] ^ crc) & 0xff] ^ (crc >> 8)) & 0xFFFF;
tnhnrl 73:f6f378311c8d 19 }
tnhnrl 73:f6f378311c8d 20
tnhnrl 73:f6f378311c8d 21 return crc / 256; //second-to-last byte
tnhnrl 73:f6f378311c8d 22 }
tnhnrl 73:f6f378311c8d 23
tnhnrl 73:f6f378311c8d 24 int Gui::guiCalcCrc2(int *input_array, int array_length) {
tnhnrl 73:f6f378311c8d 25 int crc = 0;
tnhnrl 73:f6f378311c8d 26 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};
tnhnrl 73:f6f378311c8d 27
tnhnrl 73:f6f378311c8d 28 for (int z = 0; z < array_length; z++) {
tnhnrl 73:f6f378311c8d 29 crc = (crc_table[(input_array[z] ^ crc) & 0xff] ^ (crc >> 8)) & 0xFFFF;
tnhnrl 73:f6f378311c8d 30 }
tnhnrl 73:f6f378311c8d 31 return crc % 256; //second-to-last byte
tnhnrl 73:f6f378311c8d 32 }
tnhnrl 73:f6f378311c8d 33
tnhnrl 73:f6f378311c8d 34 int Gui::calcCrcOneVector(vector <int> crc_packet) {
tnhnrl 73:f6f378311c8d 35 std::vector<int>::iterator itr;
tnhnrl 73:f6f378311c8d 36
tnhnrl 73:f6f378311c8d 37 //can't initialize the table in the constructor in c++
tnhnrl 73:f6f378311c8d 38 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};
tnhnrl 73:f6f378311c8d 39
tnhnrl 73:f6f378311c8d 40 int crc = 0;
tnhnrl 73:f6f378311c8d 41 for (itr = crc_packet.begin(); itr < crc_packet.end(); itr++)
tnhnrl 73:f6f378311c8d 42 crc = (crc_table[(*itr ^ crc) & 0xff] ^ (crc >> 8)) & 0xFFFF;
tnhnrl 73:f6f378311c8d 43
tnhnrl 73:f6f378311c8d 44 return crc / 256; //second-to-last byte
tnhnrl 73:f6f378311c8d 45 }
tnhnrl 73:f6f378311c8d 46
tnhnrl 73:f6f378311c8d 47 int Gui::calcCrcTwoVector(vector <int> crc_packet) {
tnhnrl 73:f6f378311c8d 48 std::vector<int>::iterator itr;
tnhnrl 73:f6f378311c8d 49
tnhnrl 73:f6f378311c8d 50 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};
tnhnrl 73:f6f378311c8d 51
tnhnrl 73:f6f378311c8d 52 int crc = 0;
tnhnrl 73:f6f378311c8d 53 for (itr = crc_packet.begin(); itr < crc_packet.end(); itr++)
tnhnrl 73:f6f378311c8d 54 crc = (crc_table[(*itr ^ crc) & 0xff] ^ (crc >> 8)) & 0xFFFF;
tnhnrl 73:f6f378311c8d 55
tnhnrl 73:f6f378311c8d 56 return crc % 256; //last byte
tnhnrl 73:f6f378311c8d 57 }
tnhnrl 73:f6f378311c8d 58
tnhnrl 73:f6f378311c8d 59 void Gui::transmitDataPacket(vector <int> full_packet) {
tnhnrl 73:f6f378311c8d 60 std::vector<int>::iterator itr;
tnhnrl 73:f6f378311c8d 61
tnhnrl 73:f6f378311c8d 62 //transmit full packet
tnhnrl 73:f6f378311c8d 63 for (itr = full_packet.begin(); itr < full_packet.end(); itr++) {
tnhnrl 74:d281aaef9766 64 xbee().putc(*itr); //send integers over serial port one byte at a time
tnhnrl 73:f6f378311c8d 65 }
tnhnrl 73:f6f378311c8d 66 }
tnhnrl 73:f6f378311c8d 67
tnhnrl 73:f6f378311c8d 68 // get command one byte at a time
tnhnrl 73:f6f378311c8d 69 void Gui::getCommandFSM() {
tnhnrl 73:f6f378311c8d 70 static int fsm_command = -1;
tnhnrl 73:f6f378311c8d 71 static int command_state = HEADER_FE; //state in switch statement
tnhnrl 73:f6f378311c8d 72 int incoming_byte = -1; //reset each time a character is read
tnhnrl 73:f6f378311c8d 73 static int input_packet[4]; //changed from char in previous iteration 03/28/2018
tnhnrl 73:f6f378311c8d 74 static int command_crc_one = 0; //hold crc values until they're reset with calculations
tnhnrl 73:f6f378311c8d 75 static int command_crc_two = 0;
tnhnrl 73:f6f378311c8d 76 static int command_packet_size = -1;
tnhnrl 73:f6f378311c8d 77
tnhnrl 74:d281aaef9766 78 if (xbee().readable()) {
tnhnrl 74:d281aaef9766 79 incoming_byte = xbee().getc();
tnhnrl 73:f6f378311c8d 80
tnhnrl 73:f6f378311c8d 81 switch(command_state) {
tnhnrl 73:f6f378311c8d 82 case HEADER_FE:
tnhnrl 73:f6f378311c8d 83 //continue processing
tnhnrl 73:f6f378311c8d 84 if (incoming_byte == 254){ //FE
tnhnrl 73:f6f378311c8d 85 command_state = HEADER_ED;
tnhnrl 73:f6f378311c8d 86 input_packet[0] = 254;
tnhnrl 73:f6f378311c8d 87 }
tnhnrl 73:f6f378311c8d 88 //did not receive byte 1
tnhnrl 73:f6f378311c8d 89 else {
tnhnrl 73:f6f378311c8d 90 command_state = HEADER_FE; //go back to checking the first packet
tnhnrl 73:f6f378311c8d 91 }
tnhnrl 73:f6f378311c8d 92 break;
tnhnrl 73:f6f378311c8d 93
tnhnrl 73:f6f378311c8d 94 case HEADER_ED:
tnhnrl 73:f6f378311c8d 95 if(incoming_byte == 237) { //ED
tnhnrl 73:f6f378311c8d 96 command_state = COMMAND_PACKET;
tnhnrl 73:f6f378311c8d 97 input_packet[1] = 237;
tnhnrl 73:f6f378311c8d 98 }
tnhnrl 73:f6f378311c8d 99
tnhnrl 73:f6f378311c8d 100 //did not get the correct second byte
tnhnrl 73:f6f378311c8d 101 else {
tnhnrl 73:f6f378311c8d 102 command_state = HEADER_FE; //go back to checking the first packet
tnhnrl 73:f6f378311c8d 103 }
tnhnrl 73:f6f378311c8d 104
tnhnrl 73:f6f378311c8d 105 break;
tnhnrl 73:f6f378311c8d 106
tnhnrl 73:f6f378311c8d 107 case COMMAND_PACKET:
tnhnrl 73:f6f378311c8d 108 // EMERGENCY_CLIMB, MULTI_DIVE, MULTI_RISE, POSITION_DIVE, POSITION_RISE, KEYBOARD, TRANSMIT_LOG, RECEIVE_SEQUENCE, PITCH_TUNER_DEPTH, PITCH_TUNER_RUN, SEND_STATUS
tnhnrl 73:f6f378311c8d 109
tnhnrl 73:f6f378311c8d 110 //note these ENUM values come from the StateMachine
tnhnrl 73:f6f378311c8d 111
tnhnrl 73:f6f378311c8d 112 if (incoming_byte == 1)
tnhnrl 73:f6f378311c8d 113 fsm_command = CHECK_TUNING;
tnhnrl 73:f6f378311c8d 114 else if (incoming_byte == 2)
tnhnrl 73:f6f378311c8d 115 fsm_command = FIND_NEUTRAL;
tnhnrl 73:f6f378311c8d 116 else if (incoming_byte == 3)
tnhnrl 73:f6f378311c8d 117 fsm_command = DIVE;
tnhnrl 73:f6f378311c8d 118 else if (incoming_byte == 4)
tnhnrl 73:f6f378311c8d 119 fsm_command = RISE;
tnhnrl 73:f6f378311c8d 120 else if (incoming_byte == 5)
tnhnrl 73:f6f378311c8d 121 fsm_command = FLOAT_LEVEL;
tnhnrl 73:f6f378311c8d 122 else if (incoming_byte == 6)
tnhnrl 73:f6f378311c8d 123 fsm_command = FLOAT_BROADCAST;
tnhnrl 73:f6f378311c8d 124 else if (incoming_byte == 7)
tnhnrl 73:f6f378311c8d 125 fsm_command = EMERGENCY_CLIMB;
tnhnrl 73:f6f378311c8d 126 else if (incoming_byte == 8)
tnhnrl 73:f6f378311c8d 127 fsm_command = MULTI_DIVE;
tnhnrl 73:f6f378311c8d 128 else if (incoming_byte == 9)
tnhnrl 73:f6f378311c8d 129 fsm_command = MULTI_RISE;
tnhnrl 73:f6f378311c8d 130 else if (incoming_byte == 10)
tnhnrl 73:f6f378311c8d 131 fsm_command = POSITION_DIVE;
tnhnrl 73:f6f378311c8d 132 else if (incoming_byte == 11)
tnhnrl 73:f6f378311c8d 133 fsm_command = POSITION_RISE;
tnhnrl 73:f6f378311c8d 134 //SKIP 13
tnhnrl 73:f6f378311c8d 135 else if (incoming_byte == 12)
tnhnrl 73:f6f378311c8d 136 fsm_command = TX_MBED_LOG;
tnhnrl 73:f6f378311c8d 137 else if (incoming_byte == 13)
tnhnrl 74:d281aaef9766 138 fsm_command = RX_SEQUENCE;
tnhnrl 73:f6f378311c8d 139 else {
tnhnrl 73:f6f378311c8d 140 command_state = HEADER_FE; //go back to checking the first packet
tnhnrl 73:f6f378311c8d 141 }
tnhnrl 73:f6f378311c8d 142
tnhnrl 73:f6f378311c8d 143 //state
tnhnrl 73:f6f378311c8d 144 command_state = FSM_PACKET_SIZE;
tnhnrl 73:f6f378311c8d 145
tnhnrl 73:f6f378311c8d 146 input_packet[2] = incoming_byte;
tnhnrl 73:f6f378311c8d 147
tnhnrl 73:f6f378311c8d 148 break;
tnhnrl 73:f6f378311c8d 149
tnhnrl 73:f6f378311c8d 150 case FSM_PACKET_SIZE:
tnhnrl 73:f6f378311c8d 151 if (incoming_byte >= 0) {
tnhnrl 73:f6f378311c8d 152 command_packet_size = incoming_byte;
tnhnrl 73:f6f378311c8d 153 }
tnhnrl 73:f6f378311c8d 154
tnhnrl 73:f6f378311c8d 155 command_state = FSM_CRC_ONE;
tnhnrl 73:f6f378311c8d 156
tnhnrl 73:f6f378311c8d 157 input_packet[3] = incoming_byte;
tnhnrl 73:f6f378311c8d 158
tnhnrl 73:f6f378311c8d 159 break;
tnhnrl 73:f6f378311c8d 160
tnhnrl 73:f6f378311c8d 161 case FSM_CRC_ONE:
tnhnrl 73:f6f378311c8d 162 command_crc_one = guiCalcCrc1(input_packet, 4); //calc CRC 1 from the input packet (size 4)
tnhnrl 73:f6f378311c8d 163
tnhnrl 73:f6f378311c8d 164 if (incoming_byte == command_crc_one) {
tnhnrl 73:f6f378311c8d 165 command_state = FSM_CRC_TWO;
tnhnrl 73:f6f378311c8d 166 }
tnhnrl 73:f6f378311c8d 167
tnhnrl 73:f6f378311c8d 168 else
tnhnrl 73:f6f378311c8d 169 command_state = HEADER_FE;
tnhnrl 73:f6f378311c8d 170 //or state remains the same?
tnhnrl 73:f6f378311c8d 171
tnhnrl 73:f6f378311c8d 172 break;
tnhnrl 73:f6f378311c8d 173
tnhnrl 73:f6f378311c8d 174 case FSM_CRC_TWO:
tnhnrl 73:f6f378311c8d 175 command_crc_two = guiCalcCrc2(input_packet, 4); //calc CRC 2 from the input packet (size 4)
tnhnrl 73:f6f378311c8d 176
tnhnrl 73:f6f378311c8d 177 //check if CRC TWO is correct (then send full packet)
tnhnrl 73:f6f378311c8d 178 if (incoming_byte == command_crc_two) {
tnhnrl 73:f6f378311c8d 179 //set state of statemachine
tnhnrl 73:f6f378311c8d 180 stateMachine().setState(fsm_command);
tnhnrl 74:d281aaef9766 181 xbee().printf("CRC 1 and CRC 2 IS GOOD! fsm_command is %d\n\r", fsm_command);
tnhnrl 73:f6f378311c8d 182 }
tnhnrl 73:f6f378311c8d 183
tnhnrl 73:f6f378311c8d 184 command_state = HEADER_FE;
tnhnrl 73:f6f378311c8d 185
tnhnrl 73:f6f378311c8d 186 break;
tnhnrl 73:f6f378311c8d 187 } /* switch statement complete */
tnhnrl 73:f6f378311c8d 188 } /* end of pc readable */
tnhnrl 73:f6f378311c8d 189 }
tnhnrl 73:f6f378311c8d 190
tnhnrl 73:f6f378311c8d 191 //remove class vector?
tnhnrl 73:f6f378311c8d 192
tnhnrl 73:f6f378311c8d 193 void Gui::updateGUI() {
tnhnrl 73:f6f378311c8d 194 float roll_value = imu().getRoll();
tnhnrl 73:f6f378311c8d 195 float pitch_value = imu().getPitch();
tnhnrl 73:f6f378311c8d 196 float heading_value = imu().getHeading();
tnhnrl 73:f6f378311c8d 197 float depth_value = depthLoop().getPosition(); //filtered depth position
tnhnrl 73:f6f378311c8d 198 float timer_value = stateMachine().getTimerValue();
tnhnrl 73:f6f378311c8d 199
tnhnrl 74:d281aaef9766 200 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);
tnhnrl 73:f6f378311c8d 201
tnhnrl 73:f6f378311c8d 202 vector <int> gui_update_packet;
tnhnrl 73:f6f378311c8d 203
tnhnrl 73:f6f378311c8d 204 //ROLL PITCH HEADING(YAW) DEPTH TIMER (sending all at once, at one second intervals)
tnhnrl 73:f6f378311c8d 205
tnhnrl 73:f6f378311c8d 206 //take float value, convert to unsigned char, reverse and send
tnhnrl 73:f6f378311c8d 207 const unsigned char * ptr_roll_value = reinterpret_cast<const unsigned char*>(&roll_value);
tnhnrl 73:f6f378311c8d 208 const unsigned char * ptr_pitch_value = reinterpret_cast<const unsigned char*>(&pitch_value);
tnhnrl 73:f6f378311c8d 209 const unsigned char * ptr_heading_value = reinterpret_cast<const unsigned char*>(&heading_value);
tnhnrl 73:f6f378311c8d 210 const unsigned char * ptr_depth_value = reinterpret_cast<const unsigned char*>(&depth_value);
tnhnrl 73:f6f378311c8d 211 const unsigned char * ptr_timer_value = reinterpret_cast<const unsigned char*>(&timer_value);
tnhnrl 74:d281aaef9766 212 //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]);
tnhnrl 73:f6f378311c8d 213
tnhnrl 73:f6f378311c8d 214 // BE AD GUI GUI LENGTH DATA DATA CC CC
tnhnrl 73:f6f378311c8d 215
tnhnrl 73:f6f378311c8d 216 gui_update_packet.clear();
tnhnrl 73:f6f378311c8d 217
tnhnrl 73:f6f378311c8d 218 //DATA PACKET HEADER
tnhnrl 73:f6f378311c8d 219 gui_update_packet.push_back(121); // y = 0x79
tnhnrl 73:f6f378311c8d 220 gui_update_packet.push_back(113); // q = 0x71
tnhnrl 73:f6f378311c8d 221
tnhnrl 73:f6f378311c8d 222 gui_update_packet.push_back(204); // 0xCC
tnhnrl 73:f6f378311c8d 223 gui_update_packet.push_back(204); // 0xCC
tnhnrl 73:f6f378311c8d 224
tnhnrl 73:f6f378311c8d 225 gui_update_packet.push_back(20); // 0x14 (length)
tnhnrl 73:f6f378311c8d 226
tnhnrl 73:f6f378311c8d 227 gui_update_packet.push_back(ptr_roll_value[3]);
tnhnrl 73:f6f378311c8d 228 gui_update_packet.push_back(ptr_roll_value[2]);
tnhnrl 73:f6f378311c8d 229 gui_update_packet.push_back(ptr_roll_value[1]);
tnhnrl 73:f6f378311c8d 230 gui_update_packet.push_back(ptr_roll_value[0]);
tnhnrl 73:f6f378311c8d 231
tnhnrl 73:f6f378311c8d 232 gui_update_packet.push_back(ptr_pitch_value[3]);
tnhnrl 73:f6f378311c8d 233 gui_update_packet.push_back(ptr_pitch_value[2]);
tnhnrl 73:f6f378311c8d 234 gui_update_packet.push_back(ptr_pitch_value[1]);
tnhnrl 73:f6f378311c8d 235 gui_update_packet.push_back(ptr_pitch_value[0]);
tnhnrl 73:f6f378311c8d 236
tnhnrl 73:f6f378311c8d 237 gui_update_packet.push_back(ptr_heading_value[3]);
tnhnrl 73:f6f378311c8d 238 gui_update_packet.push_back(ptr_heading_value[2]);
tnhnrl 73:f6f378311c8d 239 gui_update_packet.push_back(ptr_heading_value[1]);
tnhnrl 73:f6f378311c8d 240 gui_update_packet.push_back(ptr_heading_value[0]);
tnhnrl 73:f6f378311c8d 241
tnhnrl 73:f6f378311c8d 242 gui_update_packet.push_back(ptr_depth_value[3]);
tnhnrl 73:f6f378311c8d 243 gui_update_packet.push_back(ptr_depth_value[2]);
tnhnrl 73:f6f378311c8d 244 gui_update_packet.push_back(ptr_depth_value[1]);
tnhnrl 73:f6f378311c8d 245 gui_update_packet.push_back(ptr_depth_value[0]);
tnhnrl 73:f6f378311c8d 246
tnhnrl 73:f6f378311c8d 247 gui_update_packet.push_back(ptr_timer_value[3]);
tnhnrl 73:f6f378311c8d 248 gui_update_packet.push_back(ptr_timer_value[2]);
tnhnrl 73:f6f378311c8d 249 gui_update_packet.push_back(ptr_timer_value[1]);
tnhnrl 73:f6f378311c8d 250 gui_update_packet.push_back(ptr_timer_value[0]);
tnhnrl 73:f6f378311c8d 251
tnhnrl 73:f6f378311c8d 252 //CRC CALCULATION
tnhnrl 73:f6f378311c8d 253 int crc_one = calcCrcOneVector(gui_update_packet);
tnhnrl 73:f6f378311c8d 254 int crc_two = calcCrcTwoVector(gui_update_packet);
tnhnrl 73:f6f378311c8d 255
tnhnrl 73:f6f378311c8d 256 //place the crc bytes into the data packet that is transmitted
tnhnrl 73:f6f378311c8d 257 gui_update_packet.push_back(crc_one);
tnhnrl 73:f6f378311c8d 258 gui_update_packet.push_back(crc_two);
tnhnrl 73:f6f378311c8d 259
tnhnrl 73:f6f378311c8d 260 //transmit the full packet
tnhnrl 73:f6f378311c8d 261 transmitDataPacket(gui_update_packet);
tnhnrl 73:f6f378311c8d 262 }