Add more messages

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':
-                set_wheel_speed (
+            case 's':
+                chassis_speed_mode (
                 		client,
-                		strtol(argv[optind + 0], NULL, 0),
-                		strtol(argv[optind + 1], NULL, 0),
-                		strtol(argv[optind + 2], NULL, 0),
-                		strtol(argv[optind + 3], NULL, 0) );
+                		atof(argv[optind + 0]),
+                		atof(argv[optind + 1]),
+                		atof(argv[optind + 2]) );
+                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;
 	}
     }