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?
User | Revision | Line number | New 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 | } |