Add function to get module ID from command ID

The module ID is always the same for a given command ID,
so it can be easily mapped with a switch function.

include/message.h

@@ -262,6 +262,15 @@ enum MESSAGEERR {
 enum MESSAGEERR
 message_validate(const union Message* message);
 
+static inline uint8_t host2byte(uint8_t host, uint8_t index) {
+    return index * 32 + host;
+}
+
+static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) {
+    *host = (b & 0x1F);
+    *index = b >> 5;
+}
+
 static
 inline
 size_t
@@ -281,6 +290,31 @@ message_length(uint16_t cmd) {
             return sizeof(struct SubNodeResetReq);
         case SUBSCRIBE_ADD_NODE_CMD:
             return sizeof(struct SubscribeAddNodeReq);
+        case SET_WHEEL_SPEED_CMD:
+            return sizeof(struct SetWheelSpeedReq);
+        case CHASSIS_SPEED_MODE_CMD:
+            return sizeof(struct ChassisSpeedModeReq);
+        default:
+            return 0;
+    }
+}
+
+static
+inline
+uint8_t
+message_module(uint16_t cmd) {
+    switch(cmd) {
+        case SET_SDK_CONNECTION_CMD:
+        case SDK_HEARTBEAT_CMD:
+        case SET_SDK_MODE_CMD:
+        case SET_SYSTEM_LED_CMD:
+        case SET_ROBOT_MODE_CMD:
+        case SUBNODE_RESET_CMD:
+        case SUBSCRIBE_ADD_NODE_CMD:
+            return host2byte(SDK_HOST, SDK_INDEX);
+        case SET_WHEEL_SPEED_CMD:
+        case CHASSIS_SPEED_MODE_CMD:
+            return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
         default:
             return 0;
     }

include/robomaster.h

@@ -12,12 +12,3 @@ Client client_new();
 void client_connect(Client client);
 void poll_message(Client client, union Message* message);
 
-static inline uint8_t host2byte(uint8_t host, uint8_t index) {
-    return index * 32 + host;
-}
-
-static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) {
-    *host = (b & 0x1F);
-    *index = b >> 5;
-}
-