Add more messages

include/message.h

@@ -92,6 +92,78 @@ struct PACKED SetSystemLedResp {
     struct Footer footer;
 };
 
+struct PACKED SubscribeAddNodeReq
+{
+    struct Header header;
+    uint8_t hostbyte;
+    uint32_t sub_vision; // always 0x03000000
+    struct Footer footer;
+};
+
+struct PACKED SubscribeAddNodeResp
+{
+    struct Header header;
+    uint8_t retcode; // 0x00 or 0x50 are acceptable here
+    uint8_t hostbyte;
+    struct Footer footer;
+};
+
+struct PACKED SubNodeResetReq
+{
+    struct Header header;
+    uint8_t hostbyte;
+    struct Footer footer;
+};
+
+struct PACKED SubNodeResetResp
+{
+    struct Header header;
+    uint8_t retcode;
+    struct Footer footer;
+};
+
+struct PACKED SetRobotModeReq
+{
+    struct Header header;
+    uint8_t mode;
+    struct Footer footer;
+};
+
+struct PACKED SetRobotModeResp
+{
+    struct Header header;
+    uint8_t retcode;
+    struct Footer footer;
+};
+
+struct PACKED ChassisSpeedModeReq
+{
+    struct Header header;
+    float speed[3];
+    struct Footer footer;
+};
+
+struct PACKED ChassisSpeedModeResp
+{
+    struct Header header;
+    int8_t retcode;
+    struct Footer footer;
+};
+
+struct PACKED SetChassisWheelSpeedReq
+{
+    struct Header header;
+    int8_t wheel_speed[4];
+    struct Footer footer;
+};
+
+struct PACKED SetChassisWheelSpeedResp
+{
+    struct Header header;
+    int8_t retcode;
+    struct Footer footer;
+};
+
 struct PACKED SetWheelSpeedReq
 {
     struct Header header;
@@ -143,6 +215,11 @@ union Request {
     struct SetSdkModeReq sdkmode;
     struct SetSystemLedReq led;
     struct SetWheelSpeedReq wheel;
+    struct SetChassisWheelSpeedReq chswheel;
+    struct ChassisSpeedModeReq chsspeed;
+    struct SetRobotModeReq mvmode;
+    struct SubNodeResetReq subnodereset;
+    struct SubscribeAddNodeReq subnodeadd;
 };
 union Response {
     struct Header header;
@@ -150,6 +227,11 @@ union Response {
     struct SetSdkModeResp sdkmode;
     struct SetSystemLedResp led;
     struct SetWheelSpeedResp wheel;
+    struct SetChassisWheelSpeedResp chswheel;
+    struct ChassisSpeedModeResp chsspeed;
+    struct SetRobotModeResp mvmode;
+    struct SubNodeResetResp subnodereset;
+    struct SubscribeAddNodeResp subnodeadd;
 };
 union Message {
     struct Header header;

include/wheel.h

@@ -14,3 +14,49 @@ set_wheel_speed (
 		int16_t w2,
 		int16_t w3,
 		int16_t w4 );
+
+static const uint8_t SET_CHASSIS_WHEEL_SPEED_CMDID = 0x26;
+
+void
+set_chassis_wheel_speed (
+		Client session,
+		int8_t w1,
+		int8_t w2,
+		int8_t w3,
+		int8_t w4 );
+
+static const uint8_t CHASSIS_SPEED_MODE_CMDID = 0x21;
+
+void
+chassis_speed_mode (
+		Client session,
+		float x,
+		float y,
+		float z );
+
+static const uint8_t SET_ROBOT_MODE_CMDID = 0x46;
+
+enum MOVEMENTMODE {
+    MOVEMENTMODE_FREE,
+    MOVEMENTMODE_GIMBAL_LEAD,
+    MOVEMENTMODE_CHASSIS_LEAD
+};
+
+void
+set_robot_mode (
+		Client session,
+		enum MOVEMENTMODE mode );
+
+// cmdset 0x48
+static const uint8_t SUBNODE_RESET_CMDID = 0x02;
+
+void
+subnode_reset (
+		Client session );
+
+// cmdset 0x48
+static const uint8_t SUBSCRIBE_ADD_NODE_CMDID = 0x01;
+
+void
+subscribe_add_node (
+		Client session );

src/messages/set_wheel_speed.c

@@ -15,5 +15,64 @@ set_wheel_speed (
     req.wheel.wheel_speed[2] = w3;
     req.wheel.wheel_speed[3] = w4;
     req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, sizeof(struct SetWheelSpeedReq), &req);
-    req_send(session->sdk_conn, &req, sizeof(struct SetWheelSpeedReq));
+    req_send(session->dev_conn, &req, sizeof(struct SetWheelSpeedReq));
+}
+
+void
+set_chassis_wheel_speed (
+		Client session,
+		int8_t w1,
+		int8_t w2,
+		int8_t w3,
+		int8_t w4 ) {
+    union Request req = {0};
+    req.chswheel.wheel_speed[0] = w1;
+    req.chswheel.wheel_speed[1] = w2;
+    req.chswheel.wheel_speed[2] = w3;
+    req.chswheel.wheel_speed[3] = w4;
+    req_finalize(session, 0x3F, SET_CHASSIS_WHEEL_SPEED_CMDID, sizeof(struct SetChassisWheelSpeedReq), &req);
+    req_send(session->dev_conn, &req, sizeof(struct SetChassisWheelSpeedReq));
+}
+
+void
+chassis_speed_mode (
+		Client session,
+		float x,
+		float y,
+		float z ) {
+    union Request req = {0};
+    req.chsspeed.speed[0] = x;
+    req.chsspeed.speed[1] = y;
+    req.chsspeed.speed[2] = z;
+    req_finalize(session, 0x3F, CHASSIS_SPEED_MODE_CMDID, sizeof(struct ChassisSpeedModeReq), &req);
+    req_send(session->dev_conn, &req, sizeof(struct ChassisSpeedModeReq));
+}
+
+void
+set_robot_mode (
+		Client session,
+		enum MOVEMENTMODE mode ) {
+    union Request req = {0};
+    req.mvmode.mode = mode;
+    req_finalize(session, 0x3F, SET_ROBOT_MODE_CMDID, sizeof(struct SetRobotModeReq), &req);
+    req_send(session->dev_conn, &req, sizeof(struct SetRobotModeReq));
+}
+
+void
+subnode_reset (
+		Client session ) {
+    union Request req = {0};
+    req.subnodereset.hostbyte = session->hostbyte;
+    req_finalize(session, 0x48, SUBNODE_RESET_CMDID, sizeof(struct SubNodeResetReq), &req);
+    req_send(session->dev_conn, &req, sizeof(struct SubNodeResetReq));
+}
+
+void
+subscribe_add_node (
+		Client session ) {
+    union Request req = {0};
+    req.subnodeadd.hostbyte = session->hostbyte;
+    req.subnodeadd.sub_vision = 0x03000000;
+    req_finalize(session, 0x48, SUBSCRIBE_ADD_NODE_CMDID, sizeof(struct SubscribeAddNodeReq), &req);
+    req_send(session->dev_conn, &req, sizeof(struct SubscribeAddNodeReq));
 }

src/robomastersh.c

@@ -18,13 +18,34 @@ int main(int argc, char* argv[])
 
     set_sdk_mode(client, true);
     poll_message(client, &msg);
-    if(msg.header.cmdid == SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) {
+    if(msg.header.cmdid != SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) {
         fprintf(stderr, "Could not set SDK mode\n");
         return 1;
     }
 
+    subnode_reset(client);
+    poll_message(client, &msg);
+    if(msg.header.cmdid != SUBNODE_RESET_CMDID || msg.resp.subnodereset.retcode) {
+        fprintf(stderr, "Could not reset subnode subscription\n");
+        return 1;
+    }
+
+    subscribe_add_node(client);
+    poll_message(client, &msg);
+    if(msg.header.cmdid != SUBSCRIBE_ADD_NODE_CMDID || (msg.resp.subnodeadd.retcode && msg.resp.subnodeadd.retcode != 0x50)) {
+        fprintf(stderr, "Could not subscribe node\n");
+        return 1;
+    }
+
+    set_robot_mode(client, MOVEMENTMODE_FREE);
+    poll_message(client, &msg);
+    if(msg.header.cmdid != SET_ROBOT_MODE_CMDID || msg.resp.mvmode.retcode) {
+        fprintf(stderr, "Could not set move mode\n");
+        return 1;
+    }
+
     int c;
-    while((c = getopt(argc, argv, "hl:::w::::"))) {
+    while((c = getopt(argc, argv, "hl:::s:::")) != -1) {
         switch(c) {
             case 'l':
                 set_system_led (
@@ -38,25 +59,29 @@ int main(int argc, char* argv[])
                 		100,
                 		100 );
                 poll_message(client, &msg);
-                if(msg.header.cmdid == SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) {
+                if(msg.header.cmdid != SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) {
                     fprintf(stderr, "Could not set LED color\n");
                     return 1;
                 }
 		break;
-            case 'w':
+            case 's':
-                set_wheel_speed (
+                chassis_speed_mode (
                 		client,
-                		strtol(argv[optind + 0], NULL, 0),
+                		atof(argv[optind + 0]),
-                		strtol(argv[optind + 1], NULL, 0),
+                		atof(argv[optind + 1]),
-                		strtol(argv[optind + 2], NULL, 0),
+                		atof(argv[optind + 2]) );
-                		strtol(argv[optind + 3], NULL, 0) );
+                poll_message(client, &msg);
+                if(msg.header.cmdid != CHASSIS_SPEED_MODE_CMDID || msg.resp.chsspeed.retcode) {
+                    fprintf(stderr, "Could not set speed\n");
+                    return 1;
+                }
 		break;
             case '?':
             default:
 		fprintf(stderr, "Unknown argument %c\n", optopt);
             case 'h':
 		printf("Usage: [-h] [-l r g b] [-w w1 w2 w3 w4]");
-		break;
+		return 0;
 	}
     }