|
| 1 | +#include "FirmataSerial.h" |
| 2 | + |
| 3 | +// #define DEBUG_SYSEX |
| 4 | +// #define DEBUG_BASIC |
| 5 | + |
| 6 | +#define VERSION_MAJOR 1 |
| 7 | +#define VERSION_MINOR 6 |
| 8 | + |
| 9 | +bool FirmataSerial::parse_sysex(byte st, byte b) { |
| 10 | + static uint8_t uart_flag, uart_byte; // buffer to assemble uart bytes |
| 11 | + |
| 12 | + // end of sysex message |
| 13 | + if(b == 0xf7) |
| 14 | + return true; |
| 15 | + |
| 16 | + // start of a new message? |
| 17 | + if(st) { |
| 18 | + uart_flag = 0; |
| 19 | + |
| 20 | + switch(b) { |
| 21 | +#ifdef DEBUG_SYSEX |
| 22 | + case 0x5c: |
| 23 | + Serial.print(" QUERY_UART_BAUD_RATE"); |
| 24 | + break; |
| 25 | + case 0x5f: |
| 26 | + Serial.print(" REPORT_INPUT_PINS"); |
| 27 | + break; |
| 28 | + case 0x62: |
| 29 | + Serial.print(" IS_ALIVE"); |
| 30 | + break; |
| 31 | + case 0x64: |
| 32 | + Serial.print(" FIRMATA_MUTE"); |
| 33 | + break; |
| 34 | +#endif |
| 35 | + case 0x66: |
| 36 | +#ifdef DEBUG_SYSEX |
| 37 | + Serial.print(" UART_DATA"); |
| 38 | +#endif |
| 39 | + uart_flag = 1; |
| 40 | + break; |
| 41 | + default: |
| 42 | +#ifdef DEBUG_SYSEX |
| 43 | + Serial.print(" CMD"); |
| 44 | + Serial.print(b, HEX); |
| 45 | +#endif |
| 46 | + break; |
| 47 | + } |
| 48 | + } else { |
| 49 | + // with a sysex command |
| 50 | + if(uart_flag) { |
| 51 | + // uart parsing in progress? |
| 52 | + if(uart_flag & 1) { |
| 53 | + uart_flag = 2; |
| 54 | + uart_byte = b & 0x7f; // actually bit 7 must never be set, anyway |
| 55 | + } else { |
| 56 | + uart_flag = 1; |
| 57 | + uart_byte |= b << 7; |
| 58 | + |
| 59 | + this->uart_buffer_valid = true; |
| 60 | + this->uart_buffer = uart_byte; |
| 61 | + } |
| 62 | + } |
| 63 | +#ifdef DEBUG_SYSEX |
| 64 | + else { |
| 65 | + Serial.print("("); |
| 66 | + Serial.print(b, HEX); |
| 67 | + Serial.print(")"); |
| 68 | + } |
| 69 | +#endif |
| 70 | + } |
| 71 | + return false; |
| 72 | +} |
| 73 | + |
| 74 | +void FirmataSerial::parse(byte b) { |
| 75 | + static int8_t expect = 0; |
| 76 | + |
| 77 | + if(expect) { |
| 78 | + if(expect > 0) { |
| 79 | +#ifdef DEBUG_BASIC |
| 80 | + Serial.print(" "); |
| 81 | + Serial.print(b, HEX); |
| 82 | +#endif |
| 83 | + expect--; |
| 84 | + |
| 85 | +#ifdef DEBUG_BASIC |
| 86 | + if(expect == 0) |
| 87 | + Serial.println(""); |
| 88 | +#endif |
| 89 | + } else { |
| 90 | + if(parse_sysex(expect==-2, b)) { |
| 91 | +#ifdef DEBUG_BASIC |
| 92 | + Serial.println(" <"); |
| 93 | +#endif |
| 94 | + expect = 0; // done parsing sysex |
| 95 | + } else |
| 96 | + expect = -1; // continue parsing sysex |
| 97 | + } |
| 98 | + return; |
| 99 | + } |
| 100 | + |
| 101 | + if((b & 0xf0) == 0xe0) { |
| 102 | +#ifdef DEBUG_BASIC |
| 103 | + Serial.print("analog port message"); |
| 104 | + Serial.print(b&0x0f, DEC); |
| 105 | + Serial.print(" "); |
| 106 | +#endif |
| 107 | + expect = 2; |
| 108 | + } else if((b & 0xf0) == 0x90) { |
| 109 | +#ifdef DEBUG_BASIC |
| 110 | + Serial.print("digital port message"); |
| 111 | + Serial.print(b&0x0f, DEC); |
| 112 | + Serial.print(" "); |
| 113 | +#endif |
| 114 | + expect = 2; |
| 115 | + } else if((b & 0xf0) == 0xc0) { |
| 116 | +#ifdef DEBUG_BASIC |
| 117 | + Serial.print("report analog port "); |
| 118 | + Serial.print(b&0x0f, DEC); |
| 119 | +#endif |
| 120 | + expect = 1; |
| 121 | + } else if((b & 0xf0) == 0xd0) { |
| 122 | +#ifdef DEBUG_BASIC |
| 123 | + Serial.print("report digital port "); |
| 124 | + Serial.print(b&0x0f, DEC); |
| 125 | +#endif |
| 126 | + expect = 1; |
| 127 | + } else if(b == 0xf0) { |
| 128 | +#ifdef DEBUG_BASIC |
| 129 | + Serial.print("sysex"); |
| 130 | +#endif |
| 131 | + expect = -2; |
| 132 | + } else if(b == 0xf4) { |
| 133 | +#ifdef DEBUG_BASIC |
| 134 | + Serial.print("set pin mode"); |
| 135 | +#endif |
| 136 | + expect = 2; |
| 137 | + } else if(b == 0xf5) { |
| 138 | +#ifdef DEBUG_BASIC |
| 139 | + Serial.print("set digital pin value"); |
| 140 | +#endif |
| 141 | + expect = 2; |
| 142 | + } else if(b == 0xf9) { |
| 143 | +#ifdef DEBUG_BASIC |
| 144 | + Serial.println("version"); |
| 145 | +#endif |
| 146 | +#if defined(VERSION_MAJOR) && defined(VERSION_MINOR) |
| 147 | + this->m_s->write(0xf9); |
| 148 | + this->m_s->write(VERSION_MINOR); |
| 149 | + this->m_s->write(VERSION_MAJOR); |
| 150 | +#endif |
| 151 | + expect = 0; |
| 152 | + } else if(b == 0xff) { |
| 153 | +#ifdef DEBUG_BASIC |
| 154 | + Serial.println("system reset"); |
| 155 | +#endif |
| 156 | + expect = 0; |
| 157 | + } |
| 158 | + |
| 159 | +#ifdef DEBUG_BASIC |
| 160 | + else { |
| 161 | + Serial.print("Unknown cmd "); |
| 162 | + Serial.println(b, HEX); |
| 163 | + } |
| 164 | +#endif |
| 165 | +} |
| 166 | + |
| 167 | +FirmataSerial::FirmataSerial(Stream &s) { |
| 168 | + this->uart_buffer_valid = false; |
| 169 | + this->m_s = &s; |
| 170 | +} |
| 171 | + |
| 172 | +void FirmataSerial::begin() { } |
| 173 | + |
| 174 | +void FirmataSerial::end() { } |
| 175 | + |
| 176 | +void FirmataSerial::poll(void) { |
| 177 | + // if there is a byte in the buffer don't read more as |
| 178 | + // we couldn't store any result |
| 179 | + if(this->uart_buffer_valid) |
| 180 | + return; |
| 181 | + |
| 182 | + if(this->m_s->available()) |
| 183 | + parse(this->m_s->read()); |
| 184 | +} |
| 185 | + |
| 186 | +int FirmataSerial::available() { |
| 187 | + poll(); |
| 188 | + return this->uart_buffer_valid?1:0; |
| 189 | +} |
| 190 | + |
| 191 | +int FirmataSerial::availableForWrite() { |
| 192 | + return 1; |
| 193 | +} |
| 194 | + |
| 195 | +int FirmataSerial::read() { |
| 196 | + poll(); |
| 197 | + this->uart_buffer_valid = false; |
| 198 | + return this->uart_buffer; |
| 199 | +} |
| 200 | + |
| 201 | +int FirmataSerial::peek() { |
| 202 | + return this->uart_buffer; |
| 203 | +} |
| 204 | + |
| 205 | +void FirmataSerial::flush() { } |
| 206 | + |
| 207 | +size_t FirmataSerial::write(uint8_t byte) { |
| 208 | + // write as serial |
| 209 | + this->m_s->write(0xf0); // sysex start |
| 210 | + this->m_s->write(0x66); // uart data |
| 211 | + this->m_s->write(byte & 0x7f); // LSB |
| 212 | + this->m_s->write((byte >> 7) & 1); // MSB |
| 213 | + this->m_s->write(0xf7); // sysex end |
| 214 | + |
| 215 | + return 1; |
| 216 | +} |
0 commit comments