///////////////////////////////////////////////////////////////// PRIVATE
| 262 | // PRIVATE |
| 263 | // |
| 264 | bool MTP40::request(uint8_t *data, uint8_t commandLength, uint8_t answerLength) |
| 265 | { |
| 266 | // generic or specific address |
| 267 | if (_useAddress) |
| 268 | { |
| 269 | data[0] = _address; |
| 270 | } |
| 271 | else |
| 272 | { |
| 273 | data[0] = 0xFE; // broadcast |
| 274 | } |
| 275 | // calculate CRC of command |
| 276 | uint16_t crc = CRC(data, commandLength - 2); |
| 277 | data[commandLength - 1] = crc / 256; |
| 278 | data[commandLength - 2] = crc & 0xFF; |
| 279 | while (commandLength--) |
| 280 | { |
| 281 | #ifdef MTP40_DEBUG |
| 282 | if (*data < 0x10) _ser->print(0); |
| 283 | _ser->print(*data++, HEX); |
| 284 | _ser->print(" "); |
| 285 | #else |
| 286 | _ser->write(*data++); |
| 287 | #endif |
| 288 | yield(); // because baud rate is low! |
| 289 | } |
| 290 | |
| 291 | uint32_t start = millis(); |
| 292 | uint8_t i = 0; |
| 293 | while (answerLength) |
| 294 | { |
| 295 | if (millis() - start > _timeout) return false; |
| 296 | if (_ser->available()) |
| 297 | { |
| 298 | _buffer[i] = _ser->read(); |
| 299 | i++; |
| 300 | answerLength--; |
| 301 | } |
| 302 | yield(); // because baud rate is low! |
| 303 | } |
| 304 | return true; |
| 305 | } |
| 306 | |
| 307 | |
| 308 | // from datasheet |