sensor_commands.cpp

#include "sensor_commands.h"

namespace sensor_commands {
  const uint8_t MAX_CALLBACKS = 20;
  const uint8_t MAX_COMMAND_SIZE = 20;
  uint8_t callbacks_count = 0;
// A callback contains both a function and a pointer to arbitrary data
// that will be passed as argument to the function.
  struct Callback {
    Callback(void (*f)(void*) = 0, void *d = 0) :
        function(f), data(d) {
    }
    void (*function)(void*);
    void *data;
  };

  Callback callbacks[MAX_CALLBACKS];
  char callback_names[MAX_CALLBACKS][MAX_COMMAND_SIZE];

  void defineCallback(const char *command, void (*f)(void*), void *d) {
    if (callbacks_count < MAX_CALLBACKS) {
      callbacks[callbacks_count] = Callback(f, d);
      strlcpy(callback_names[callbacks_count], command, MAX_COMMAND_SIZE);
      callbacks_count++;
    } else {
      Serial.println(F("Too many callbacks have been defined."));
    }
  }

  void run(const char *command) {
    Serial.print(F("Received command : '"));
    Serial.print(command);
    Serial.println("'");
    // Test all the callbacks.
    for (uint8_t i = 0; i < callbacks_count; i++) {
      Serial.print("Trying '");
      Serial.print(callback_names[i]);
      Serial.println("'");
      callbacks[i].function(callbacks[i].data);
    }
    Serial.println("Done.");
  }
}