Merge pull request #359 from zfields/firmware

firmware callbacks

Firmata.cpp

@@ -94,7 +94,7 @@ FirmataClass::FirmataClass()
   parser.attach(SET_DIGITAL_PIN_VALUE, (FirmataParser::callbackFunction)staticPinValueCallback, (void *)NULL);
   parser.attach(STRING_DATA, (FirmataParser::stringCallbackFunction)staticStringCallback, (void *)NULL);
   parser.attach(START_SYSEX, (FirmataParser::sysexCallbackFunction)staticSysexCallback, (void *)NULL);
-  parser.attach(REPORT_FIRMWARE, (FirmataParser::systemCallbackFunction)staticReportFirmwareCallback, this);
+  parser.attach(REPORT_FIRMWARE, (FirmataParser::versionCallbackFunction)staticReportFirmwareCallback, this);
   parser.attach(REPORT_VERSION, (FirmataParser::systemCallbackFunction)staticReportVersionCallback, this);
   parser.attach(SYSTEM_RESET, (FirmataParser::systemCallbackFunction)staticSystemResetCallback, (void *)NULL);
 }

Firmata.h

@@ -148,9 +148,9 @@ class FirmataClass
     inline static void staticPinValueCallback (void *, uint8_t command, uint16_t value) { if ( currentPinValueCallback ) { currentPinValueCallback(command, (int)value); } }
     inline static void staticReportAnalogCallback (void *, uint8_t command, uint16_t value) { if ( currentReportAnalogCallback ) { currentReportAnalogCallback(command, (int)value); } }
     inline static void staticReportDigitalCallback (void *, uint8_t command, uint16_t value) { if ( currentReportDigitalCallback ) { currentReportDigitalCallback(command, (int)value); } }
-    inline static void staticStringCallback (void *, char * c_str) { if ( currentStringCallback ) { currentStringCallback(c_str); } }
+    inline static void staticStringCallback (void *, const char * c_str) { if ( currentStringCallback ) { currentStringCallback((char *)c_str); } }
     inline static void staticSysexCallback (void *, uint8_t command, size_t argc, uint8_t *argv) { if ( currentSysexCallback ) { currentSysexCallback(command, (uint8_t)argc, argv); } }
-    inline static void staticReportFirmwareCallback (void * context) { if ( context ) { ((FirmataClass *)context)->printFirmwareVersion(); } }
+    inline static void staticReportFirmwareCallback (void * context, size_t, size_t, const char *) { if ( context ) { ((FirmataClass *)context)->printFirmwareVersion(); } }
     inline static void staticReportVersionCallback (void * context) { if ( context ) { ((FirmataClass *)context)->printVersion(); } }
     inline static void staticSystemResetCallback (void *) { if ( currentSystemResetCallback ) { currentSystemResetCallback(); } }
 };

FirmataParser.cpp

@@ -60,7 +60,7 @@ FirmataParser::FirmataParser(uint8_t * const dataBuffer, size_t dataBufferSize)
   currentDataBufferOverflowCallback((dataBufferOverflowCallbackFunction)NULL),
   currentStringCallback((stringCallbackFunction)NULL),
   currentSysexCallback((sysexCallbackFunction)NULL),
-  currentReportFirmwareCallback((systemCallbackFunction)NULL),
+  currentReportFirmwareCallback((versionCallbackFunction)NULL),
   currentReportVersionCallback((systemCallbackFunction)NULL),
   currentSystemResetCallback((systemCallbackFunction)NULL)
 {
@@ -244,21 +244,34 @@ void FirmataParser::attach(uint8_t command, callbackFunction newFunction, void *
 }
 
 /**
- * Attach a system callback function (options are: REPORT_FIRMWARE, REPORT_VERSION
- * and SYSTEM_RESET).
+ * Attach a version callback function (supported option: REPORT_FIRMWARE).
  * @param command The ID of the command to attach a callback function to.
  * @param newFunction A reference to the callback function to attach.
  * @param context An optional context to be provided to the callback function (NULL by default).
  * @note The context parameter is provided so you can pass a parameter, by reference, to
  *       your callback function.
  */
-void FirmataParser::attach(uint8_t command, systemCallbackFunction newFunction, void * context)
+void FirmataParser::attach(uint8_t command, versionCallbackFunction newFunction, void * context)
 {
   switch (command) {
     case REPORT_FIRMWARE:
       currentReportFirmwareCallback = newFunction;
       currentReportFirmwareCallbackContext = context;
       break;
+  }
+}
+
+/**
+ * Attach a system callback function (supported options are: SYSTEM_RESET, REPORT_VERSION).
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the callback function to attach.
+ * @param context An optional context to be provided to the callback function (NULL by default).
+ * @note The context parameter is provided so you can pass a parameter, by reference, to
+ *       your callback function.
+ */
+void FirmataParser::attach(uint8_t command, systemCallbackFunction newFunction, void * context)
+{
+  switch (command) {
     case REPORT_VERSION:
       currentReportVersionCallback = newFunction;
       currentReportVersionCallbackContext = context;
@@ -325,6 +338,8 @@ void FirmataParser::detach(uint8_t command)
 {
   switch (command) {
     case REPORT_FIRMWARE:
+      attach(command, (versionCallbackFunction)NULL, NULL);
+      break;
     case REPORT_VERSION:
     case SYSTEM_RESET:
       attach(command, (systemCallbackFunction)NULL, NULL);
@@ -394,14 +409,24 @@ void FirmataParser::processSysexMessage(void)
 {
   switch (dataBuffer[0]) { //first byte in buffer is command
     case REPORT_FIRMWARE:
-      if (currentReportFirmwareCallback)
-        (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext);
+      if (currentReportFirmwareCallback) {
+        size_t sv_major = dataBuffer[1], sv_minor = dataBuffer[2];
+        size_t i = 0, j = 3;
+        while (j < sysexBytesRead) {
+          // The string length will only be at most half the size of the
+          // stored input buffer so we can decode the string within the buffer.
+          bufferDataAtPosition(dataBuffer[j], i);
+          ++i;
+          ++j;
+        }
+        bufferDataAtPosition('\0', i); // Terminate the string
+        (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, sv_major, sv_minor, (const char *)&dataBuffer[0]);
+      }
       break;
     case STRING_DATA:
       if (currentStringCallback) {
         size_t bufferLength = (sysexBytesRead - 1) / 2;
-        size_t i = 1;
-        size_t j = 0;
+        size_t i = 1, j = 0;
         while (j < bufferLength) {
           // The string length will only be at most half the size of the
           // stored input buffer so we can decode the string within the buffer.
@@ -417,7 +442,7 @@ void FirmataParser::processSysexMessage(void)
         if (dataBuffer[j - 1] != '\0') {
           bufferDataAtPosition('\0', j);
         }
-        (*currentStringCallback)(currentStringCallbackContext, (char *)&dataBuffer[0]);
+        (*currentStringCallback)(currentStringCallbackContext, (const char *)&dataBuffer[0]);
       }
       break;
     default:

FirmataParser.h

@@ -30,9 +30,10 @@ class FirmataParser
     /* callback function types */
     typedef void (*callbackFunction)(void * context, uint8_t command, uint16_t value);
     typedef void (*dataBufferOverflowCallbackFunction)(void * context);
-    typedef void (*stringCallbackFunction)(void * context, char * c_str);
+    typedef void (*stringCallbackFunction)(void * context, const char * c_str);
     typedef void (*sysexCallbackFunction)(void * context, uint8_t command, size_t argc, uint8_t * argv);
     typedef void (*systemCallbackFunction)(void * context);
+    typedef void (*versionCallbackFunction)(void * context, size_t sv_major, size_t sv_minor, const char * firmware);
 
     FirmataParser(uint8_t * dataBuffer = (uint8_t *)NULL, size_t dataBufferSize = 0);
 
@@ -47,6 +48,7 @@ class FirmataParser
     void attach(uint8_t command, stringCallbackFunction newFunction, void * context = NULL);
     void attach(uint8_t command, sysexCallbackFunction newFunction, void * context = NULL);
     void attach(uint8_t command, systemCallbackFunction newFunction, void * context = NULL);
+    void attach(uint8_t command, versionCallbackFunction newFunction, void * context = NULL);
     void detach(uint8_t command);
     void detach(dataBufferOverflowCallbackFunction);
 
@@ -87,14 +89,14 @@ class FirmataParser
     dataBufferOverflowCallbackFunction currentDataBufferOverflowCallback;
     stringCallbackFunction currentStringCallback;
     sysexCallbackFunction currentSysexCallback;
-    systemCallbackFunction currentReportFirmwareCallback;
+    versionCallbackFunction currentReportFirmwareCallback;
     systemCallbackFunction currentReportVersionCallback;
     systemCallbackFunction currentSystemResetCallback;
 
     /* private methods ------------------------------ */
+    bool bufferDataAtPosition(const uint8_t data, const size_t pos);
     void processSysexMessage(void);
     void systemReset(void);
-    bool bufferDataAtPosition(const uint8_t data, const size_t pos);
 };
 
 } // firmata