Ugly, but kinda working

ampel-firmware/sensor_commands.cpp

@@ -2,6 +2,7 @@
 
 namespace sensor_commands {
   const uint8_t MAX_CALLBACKS = 20;
+  const uint8_t MAX_COMMAND_SIZE = 30;
 
   uint8_t callbacks_count = 0;
 // A callback contains both a function and a pointer to arbitrary data
@@ -30,25 +31,20 @@ namespace sensor_commands {
     return !strncmp(a, b, strlen(b));
   }
 
-  bool parseCommand(const char *c, char *fname, long *param) {
-    return 0;
-  }
-
-  void run(const char *c) {
-    char command[30];
-    char *function_name;
-    long parameter;
-    parseCommand(c, function_name, &parameter);
-    strlcpy(command, c, 30);
+  bool parseCommand(const char *command, char *function_name, long &parameter) {
+    char split_command[MAX_COMMAND_SIZE];
+    strlcpy(split_command, command, MAX_COMMAND_SIZE);
     Serial.print(F("Received : '"));
     Serial.print(command);
     Serial.println("'");
     char *arg;
-    function_name = strtok(command, " ");
-    if (!function_name) {
+    char *part1;
+    part1 = strtok(split_command, " ");
+    if (!part1) {
       // Empty string
-      return;
+      return 1;
     }
+    strlcpy(function_name, part1, MAX_COMMAND_SIZE);
     arg = strtok(NULL, " ");
     if (arg) {
       char *end;
@@ -60,6 +56,15 @@ namespace sensor_commands {
     } else {
       parameter = -9999;
     }
+    return 0;
+  }
+
+  void run(const char *command) {
+    char function_name[MAX_COMMAND_SIZE];
+    long parameter;
+    if (parseCommand(command, function_name, parameter)) {
+      return;
+    }
     Serial.print("Command : '");
     Serial.print(function_name);
     Serial.print("'. Parameter : ");
@@ -67,7 +72,7 @@ namespace sensor_commands {
 
     // Test all the callbacks.
     for (uint8_t i = 0; i < callbacks_count; i++) {
-      if (startsWith(command, callbacks[i].name)) {
+      if (startsWith(function_name, callbacks[i].name)) {
         Serial.print("OHHHH YES!!!");
       }
       Serial.print("Trying '");