Franck Roudet / CmdMessenger

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

CmdMessenger.cpp@1:798b310a5240, 2015-11-26 (annotated)

Committer:
francxk
Date:
Thu Nov 26 17:24:47 2015 +0000
Revision:
1:798b310a5240
Parent:
0:44e65e1ce5ee 
millis ported

Who changed what in which revision?

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