Newer
Older
TestStandRepository / Software / Arduino / libraries / Arduino-Libraries / CmdMessenger / CmdMessenger.cpp
  1. /*
  2. CmdMessenger - library that provides command based messaging
  3. Permission is hereby granted, free of charge, to any person obtaining
  4. a copy of this software and associated documentation files (the
  5. "Software"), to deal in the Software without restriction, including
  6. without limitation the rights to use, copy, modify, merge, publish,
  7. distribute, sublicense, and/or sell copies of the Software, and to
  8. permit persons to whom the Software is furnished to do so, subject to
  9. the following conditions:
  10.  
  11. The above copyright notice and this permission notice shall be
  12. included in all copies or substantial portions of the Software.
  13.  
  14. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  15. EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  16. MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  17. NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
  18. LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
  19. OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
  20. WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  21.  
  22. Initial Messenger Library - Thomas Ouellet Fredericks.
  23. CmdMessenger Version 1 - Neil Dudman.
  24. CmdMessenger Version 2 - Dreamcat4.
  25. CmdMessenger Version 3 - Thijs Elenbaas.
  26. 3.6 - Fixes
  27. - Better compatibility between platforms
  28. - Unit tests
  29. 3.5 - Fixes, speed improvements for Teensy
  30. 3.4 - Internal update
  31. 3.3 - Fixed warnings
  32. - Some code optimization
  33. 3.2 - Small fixes and sending long argument support
  34. 3.1 - Added examples
  35. 3.0 - Bugfixes on 2.2
  36. - Wait for acknowlegde
  37. - Sending of common type arguments (float, int, char)
  38. - Multi-argument commands
  39. - Escaping of special characters
  40. - Sending of binary data of any type (uses escaping)
  41. */
  42.  
  43. extern "C" {
  44. #include <stdlib.h>
  45. #include <stdarg.h>
  46. }
  47. #include <stdio.h>
  48. #include <CmdMessenger.h>
  49.  
  50. #define _CMDMESSENGER_VERSION 3_6 // software version of this library
  51.  
  52. // **** Initialization ****
  53.  
  54. /**
  55. * CmdMessenger constructor
  56. */
  57. CmdMessenger::CmdMessenger(Stream &ccomms, const char fld_separator, const char cmd_separator, const char esc_character)
  58. {
  59. init(ccomms,fld_separator,cmd_separator, esc_character);
  60. }
  61.  
  62. /**
  63. * Enables printing newline after a sent command
  64. */
  65. void CmdMessenger::init(Stream &ccomms, const char fld_separator, const char cmd_separator, const char esc_character)
  66. {
  67. default_callback = NULL;
  68. comms = &ccomms;
  69. print_newlines = false;
  70. field_separator = fld_separator;
  71. command_separator = cmd_separator;
  72. escape_character = esc_character;
  73. bufferLength = MESSENGERBUFFERSIZE;
  74. bufferLastIndex = MESSENGERBUFFERSIZE -1;
  75. reset();
  76.  
  77. default_callback = NULL;
  78. for (int i = 0; i < MAXCALLBACKS; i++)
  79. callbackList[i] = NULL;
  80.  
  81. pauseProcessing = false;
  82. }
  83.  
  84. /**
  85. * Resets the command buffer and message state
  86. */
  87. void CmdMessenger::reset()
  88. {
  89. bufferIndex = 0;
  90. current = NULL;
  91. last = NULL;
  92. dumped = true;
  93. }
  94.  
  95. /**
  96. * Enables printing newline after a sent command
  97. */
  98. void CmdMessenger::printLfCr(bool addNewLine)
  99. {
  100. print_newlines = addNewLine;
  101. }
  102.  
  103. /**
  104. * Attaches an default function for commands that are not explicitly attached
  105. */
  106. void CmdMessenger::attach(messengerCallbackFunction newFunction)
  107. {
  108. default_callback = newFunction;
  109. }
  110.  
  111. /**
  112. * Attaches a function to a command ID
  113. */
  114. void CmdMessenger::attach(byte msgId, messengerCallbackFunction newFunction)
  115. {
  116. if (msgId >= 0 && msgId < MAXCALLBACKS)
  117. callbackList[msgId] = newFunction;
  118. }
  119.  
  120. // **** Command processing ****
  121.  
  122. /**
  123. * Feeds serial data in CmdMessenger
  124. */
  125. void CmdMessenger::feedinSerialData()
  126. {
  127. while ( !pauseProcessing && comms->available() )
  128. {
  129. // The Stream class has a readBytes() function that reads many bytes at once. On Teensy 2.0 and 3.0, readBytes() is optimized.
  130. // Benchmarks about the incredible difference it makes: http://www.pjrc.com/teensy/benchmark_usb_serial_receive.html
  131.  
  132. size_t bytesAvailable = min(comms->available(),MAXSTREAMBUFFERSIZE);
  133. comms->readBytes(streamBuffer, bytesAvailable);
  134. // Process the bytes in the stream buffer, and handles dispatches callbacks, if commands are received
  135. for (size_t byteNo = 0; byteNo < bytesAvailable ; byteNo++)
  136. {
  137. int messageState = processLine(streamBuffer[byteNo]);
  138.  
  139. // If waiting for acknowledge command
  140. if ( messageState == kEndOfMessage )
  141. {
  142. handleMessage();
  143. }
  144. }
  145. }
  146. }
  147.  
  148. /**
  149. * Processes bytes and determines message state
  150. */
  151. uint8_t CmdMessenger::processLine(char serialChar)
  152. {
  153. messageState = kProccesingMessage;
  154. //char serialChar = (char)serialByte;
  155. bool escaped = isEscaped(&serialChar,escape_character,&CmdlastChar);
  156. if((serialChar == command_separator) && !escaped) {
  157. commandBuffer[bufferIndex]=0;
  158. if(bufferIndex > 0) {
  159. messageState = kEndOfMessage;
  160. current = commandBuffer;
  161. CmdlastChar='\0';
  162. }
  163. reset();
  164. } else {
  165. commandBuffer[bufferIndex]=serialChar;
  166. bufferIndex++;
  167. if (bufferIndex >= bufferLastIndex) reset();
  168. }
  169. return messageState;
  170. }
  171.  
  172. /**
  173. * Dispatches attached callbacks based on command
  174. */
  175. void CmdMessenger::handleMessage()
  176. {
  177. lastCommandId = readInt16Arg();
  178. // if command attached, we will call it
  179. if (lastCommandId >= 0 && lastCommandId < MAXCALLBACKS && ArgOk && callbackList[lastCommandId] != NULL)
  180. (*callbackList[lastCommandId])();
  181. else // If command not attached, call default callback (if attached)
  182. if (default_callback!=NULL) (*default_callback)();
  183. }
  184.  
  185. /**
  186. * Waits for reply from sender or timeout before continuing
  187. */
  188. bool CmdMessenger::blockedTillReply(unsigned long timeout, int ackCmdId)
  189. {
  190. unsigned long time = millis();
  191. unsigned long start = time;
  192. bool receivedAck = false;
  193. while( (time - start ) < timeout && !receivedAck) {
  194. time = millis();
  195. receivedAck = CheckForAck(ackCmdId);
  196. }
  197. return receivedAck;
  198. }
  199.  
  200. /**
  201. * Loops as long data is available to determine if acknowledge has come in
  202. */
  203. bool CmdMessenger::CheckForAck(int AckCommand)
  204. {
  205. while ( comms->available() ) {
  206. //Processes a byte and determines if an acknowlegde has come in
  207. int messageState = processLine(comms->read());
  208. if ( messageState == kEndOfMessage ) {
  209. int id = readInt16Arg();
  210. if (AckCommand==id && ArgOk) {
  211. return true;
  212. } else {
  213. return false;
  214. }
  215. }
  216. return false;
  217. }
  218. return false;
  219. }
  220.  
  221. /**
  222. * Gets next argument. Returns true if an argument is available
  223. */
  224. bool CmdMessenger::next()
  225. {
  226. char * temppointer= NULL;
  227. // Currently, cmd messenger only supports 1 char for the field seperator
  228. switch (messageState) {
  229. case kProccesingMessage:
  230. return false;
  231. case kEndOfMessage:
  232. temppointer = commandBuffer;
  233. messageState = kProcessingArguments;
  234. default:
  235. if (dumped)
  236. current = split_r(temppointer,field_separator,&last);
  237. if (current != NULL) {
  238. dumped = true;
  239. return true;
  240. }
  241. }
  242. return false;
  243. }
  244.  
  245. /**
  246. * Returns if an argument is available. Alias for next()
  247. */
  248. bool CmdMessenger::available()
  249. {
  250. return next();
  251. }
  252.  
  253. /**
  254. * Returns if the latest argument is well formed.
  255. */
  256. bool CmdMessenger::isArgOk ()
  257. {
  258. return ArgOk;
  259. }
  260.  
  261. /**
  262. * Returns the CommandID of the current command
  263. */
  264. uint8_t CmdMessenger::CommandID()
  265. {
  266. return lastCommandId;
  267. }
  268.  
  269. // **** Command sending ****
  270.  
  271. /**
  272. * Send start of command. This makes it easy to send multiple arguments per command
  273. */
  274. void CmdMessenger::sendCmdStart(int cmdId)
  275. {
  276. if (!startCommand) {
  277. startCommand = true;
  278. pauseProcessing = true;
  279. comms->print(cmdId);
  280. }
  281. }
  282.  
  283. /**
  284. * Send an escaped command argument
  285. */
  286. void CmdMessenger::sendCmdEscArg(char* arg)
  287. {
  288. if (startCommand) {
  289. comms->print(field_separator);
  290. printEsc(arg);
  291. }
  292. }
  293.  
  294. /**
  295. * Send formatted argument.
  296. * Note that floating points are not supported and resulting string is limited to 128 chars
  297. */
  298. void CmdMessenger::sendCmdfArg(char *fmt, ...)
  299. {
  300. const int maxMessageSize = 128;
  301. if (startCommand) {
  302. char msg[maxMessageSize];
  303. va_list args;
  304. va_start (args, fmt );
  305. vsnprintf(msg, maxMessageSize, fmt, args);
  306. va_end (args);
  307.  
  308. comms->print(field_separator);
  309. comms->print(msg);
  310. }
  311. }
  312.  
  313. /**
  314. * Send double argument in scientific format.
  315. * This will overcome the boundary of normal float sending which is limited to abs(f) <= MAXLONG
  316. */
  317. void CmdMessenger::sendCmdSciArg (double arg, int n)
  318. {
  319. if (startCommand)
  320. {
  321. comms->print (field_separator);
  322. printSci (arg, n);
  323. }
  324. }
  325.  
  326. /**
  327. * Send end of command
  328. */
  329. bool CmdMessenger::sendCmdEnd(bool reqAc, int ackCmdId, int timeout)
  330. {
  331. bool ackReply = false;
  332. if (startCommand) {
  333. comms->print(command_separator);
  334. if(print_newlines)
  335. comms->println(); // should append BOTH \r\n
  336. if (reqAc) {
  337. ackReply = blockedTillReply(timeout, ackCmdId);
  338. }
  339. }
  340. pauseProcessing = false;
  341. startCommand = false;
  342. return ackReply;
  343. }
  344.  
  345. /**
  346. * Send a command without arguments, with acknowledge
  347. */
  348. bool CmdMessenger::sendCmd (int cmdId, bool reqAc, int ackCmdId)
  349. {
  350. if (!startCommand) {
  351. sendCmdStart (cmdId);
  352. return sendCmdEnd (reqAc, ackCmdId, DEFAULT_TIMEOUT);
  353. }
  354. return false;
  355. }
  356.  
  357. /**
  358. * Send a command without arguments, without acknowledge
  359. */
  360. bool CmdMessenger::sendCmd (int cmdId)
  361. {
  362. if (!startCommand) {
  363. sendCmdStart (cmdId);
  364. return sendCmdEnd (false, 1, DEFAULT_TIMEOUT);
  365. }
  366. return false;
  367. }
  368.  
  369. // **** Command receiving ****
  370.  
  371. /**
  372. * Find next argument in command
  373. */
  374. int CmdMessenger::findNext(char *str, char delim)
  375. {
  376. int pos = 0;
  377. bool escaped = false;
  378. bool EOL = false;
  379. ArglastChar = '\0';
  380. while (true) {
  381. escaped = isEscaped(str,escape_character,&ArglastChar);
  382. EOL = (*str == '\0' && !escaped);
  383. if (EOL) {
  384. return pos;
  385. }
  386. if (*str==field_separator && !escaped) {
  387. return pos;
  388. } else {
  389. str++;
  390. pos++;
  391. }
  392. }
  393. return pos;
  394. }
  395.  
  396. /**
  397. * Read the next argument as int
  398. */
  399. int16_t CmdMessenger::readInt16Arg()
  400. {
  401. if (next()) {
  402. dumped = true;
  403. ArgOk = true;
  404. return atoi(current);
  405. }
  406. ArgOk = false;
  407. return 0;
  408. }
  409.  
  410. /**
  411. * Read the next argument as int
  412. */
  413. int32_t CmdMessenger::readInt32Arg()
  414. {
  415. if (next()) {
  416. dumped = true;
  417. ArgOk = true;
  418. return atol(current);
  419. }
  420. ArgOk = false;
  421. return 0L;
  422. }
  423.  
  424. /**
  425. * Read the next argument as bool
  426. */
  427. bool CmdMessenger::readBoolArg()
  428. {
  429. return (readInt16Arg()!=0)?true:false;
  430. }
  431.  
  432. /**
  433. * Read the next argument as char
  434. */
  435. char CmdMessenger::readCharArg()
  436. {
  437. if (next()) {
  438. dumped = true;
  439. ArgOk = true;
  440. return current[0];
  441. }
  442. ArgOk = false;
  443. return 0;
  444. }
  445.  
  446. /**
  447. * Read the next argument as float
  448. */
  449. float CmdMessenger::readFloatArg()
  450. {
  451. if (next()) {
  452. dumped = true;
  453. ArgOk = true;
  454. //return atof(current);
  455. return strtod(current,NULL);
  456. }
  457. ArgOk = false;
  458. return 0;
  459. }
  460.  
  461. /**
  462. * Read the next argument as double
  463. */
  464. double CmdMessenger::readDoubleArg()
  465. {
  466. if (next()) {
  467. dumped = true;
  468. ArgOk = true;
  469. return strtod(current,NULL);
  470. }
  471. ArgOk = false;
  472. return 0;
  473. }
  474.  
  475. /**
  476. * Read next argument as string.
  477. * Note that the String is valid until the current command is replaced
  478. */
  479. char* CmdMessenger::readStringArg()
  480. {
  481. if (next()) {
  482. dumped = true;
  483. ArgOk = true;
  484. return current;
  485. }
  486. ArgOk = false;
  487. return '\0';
  488. }
  489.  
  490. /**
  491. * Return next argument as a new string
  492. * Note that this is useful if the string needs to be persisted
  493. */
  494. void CmdMessenger::copyStringArg(char *string, uint8_t size)
  495. {
  496. if (next()) {
  497. dumped = true;
  498. ArgOk = true;
  499. strlcpy(string,current,size);
  500. } else {
  501. ArgOk = false;
  502. if ( size ) string[0] = '\0';
  503. }
  504. }
  505.  
  506. /**
  507. * Compare the next argument with a string
  508. */
  509. uint8_t CmdMessenger::compareStringArg(char *string)
  510. {
  511. if (next()) {
  512. if ( strcmp(string,current) == 0 ) {
  513. dumped = true;
  514. ArgOk = true;
  515. return 1;
  516. } else {
  517. ArgOk = false;
  518. return 0;
  519. }
  520. }
  521. return 0;
  522. }
  523.  
  524. // **** Escaping tools ****
  525.  
  526. /**
  527. * Unescapes a string
  528. * Note that this is done inline
  529. */
  530. void CmdMessenger::unescape(char *fromChar)
  531. {
  532. // Move unescaped characters right
  533. char *toChar = fromChar;
  534. while (*fromChar != '\0') {
  535. if (*fromChar==escape_character) {
  536. fromChar++;
  537. }
  538. *toChar++=*fromChar++;
  539. }
  540. // Pad string with \0 if string was shortened
  541. for (; toChar<fromChar; toChar++) {
  542. *toChar='\0';
  543. }
  544. }
  545.  
  546. /**
  547. * Split string in different tokens, based on delimiter
  548. * Note that this is basically strtok_r, but with support for an escape character
  549. */
  550. char* CmdMessenger::split_r(char *str, const char delim, char **nextp)
  551. {
  552. char *ret;
  553. // if input null, this is not the first call, use the nextp pointer instead
  554. if (str == NULL) {
  555. str = *nextp;
  556. }
  557. // Strip leading delimiters
  558. while (findNext(str, delim)==0 && *str) {
  559. str++;
  560. }
  561. // If this is a \0 char, return null
  562. if (*str == '\0') {
  563. return NULL;
  564. }
  565. // Set start of return pointer to this position
  566. ret = str;
  567. // Find next delimiter
  568. str += findNext(str, delim);
  569. // and exchange this for a a \0 char. This will terminate the char
  570. if (*str) {
  571. *str++ = '\0';
  572. }
  573. // Set the next pointer to this char
  574. *nextp = str;
  575. // return current pointer
  576. return ret;
  577. }
  578.  
  579. /**
  580. * Indicates if the current character is escaped
  581. */
  582. bool CmdMessenger::isEscaped(char *currChar, const char escapeChar, char *lastChar)
  583. {
  584. bool escaped;
  585. escaped = (*lastChar==escapeChar);
  586. *lastChar = *currChar;
  587.  
  588. // special case: the escape char has been escaped:
  589. if (*lastChar == escape_character && escaped) {
  590. *lastChar = '\0';
  591. }
  592. return escaped;
  593. }
  594.  
  595. /**
  596. * Escape and print a string
  597. */
  598. void CmdMessenger::printEsc(char *str)
  599. {
  600. while (*str != '\0') {
  601. printEsc(*str++);
  602. }
  603. }
  604.  
  605. /**
  606. * Escape and print a character
  607. */
  608. void CmdMessenger::printEsc(char str)
  609. {
  610. if (str==field_separator || str==command_separator || str==escape_character || str=='\0') {
  611. comms->print(escape_character);
  612. }
  613. comms->print(str);
  614. }
  615.  
  616. /**
  617. * Print float and double in scientific format
  618. */
  619. void CmdMessenger::printSci(double f, unsigned int digits)
  620. {
  621. // handle sign
  622. if (f < 0.0)
  623. {
  624. Serial.print('-');
  625. f = -f;
  626. }
  627. // handle infinite values
  628. if (isinf(f))
  629. {
  630. Serial.print("INF");
  631. return;
  632. }
  633. // handle Not a Number
  634. if (isnan(f))
  635. {
  636. Serial.print("NaN");
  637. return;
  638. }
  639.  
  640. // max digits
  641. if (digits > 6) digits = 6;
  642. long multiplier = pow(10, digits); // fix int => long
  643.  
  644. int exponent;
  645. if (abs(f) < 10.0) {
  646. exponent = 0;
  647. } else {
  648. exponent = int(log10(f));
  649. }
  650. float g = f / pow(10, exponent);
  651. if ((g < 1.0) && (g != 0.0))
  652. {
  653. g *= 10;
  654. exponent--;
  655. }
  656. long whole = long(g); // single digit
  657. long part = long((g-whole)*multiplier+0.5); // # digits
  658. // Check for rounding above .99:
  659. if (part == 100) {
  660. whole++;
  661. part = 0;
  662. }
  663. char format[16];
  664. sprintf(format, "%%ld.%%0%dldE%%+d", digits);
  665. char output[16];
  666. sprintf(output,format, whole, part, exponent);
  667. comms->print(output);
  668. }