Add gimbal control message

CMakeLists.txt

@@ -15,6 +15,7 @@ add_library(robomaster
     src/message.c
     src/modules/sdk.c
     src/modules/chassis.c
+    src/modules/gimbal.c
     src/connection.c
     src/robomaster.c
     src/robo.c

include/gimbal.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "message.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+static const uint8_t GIMBAL_HOST = 4;
+static const uint8_t GIMBAL_INDEX = 0;
+
+#define GIMBAL_CTRL_SPEED_CMD 0x0C04
+
+void
+gimbal_ctrl_speed (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		int16_t p,
+		int16_t y,
+		int16_t r );

include/message.h

@@ -168,6 +168,30 @@ struct PACKED SetChassisWheelSpeedResp
     struct Footer footer;
 };
 
+struct PACKED GimbalCtrlSpeedReq
+{
+    struct Header header;
+    // Values between -360 and 360
+    union {
+        int16_t yrp[3];
+        struct {
+            int16_t yaw;
+            int16_t roll;
+            int16_t pitch;
+        };
+    };
+    // Always 0xDC
+    uint8_t ctrl;
+    struct Footer footer;
+};
+
+struct PACKED GimbalCtrlSpeedResp
+{
+    struct Header header;
+    int8_t retcode;
+    struct Footer footer;
+};
+
 struct PACKED SetWheelSpeedReq
 {
     struct Header header;
@@ -236,6 +260,7 @@ union Request {
     struct SetRobotModeReq mvmode;
     struct SubNodeResetReq subnodereset;
     struct SubscribeAddNodeReq subnodeadd;
+    struct GimbalCtrlSpeedReq gimbspeed;
 };
 union Response {
     struct Header header;
@@ -249,6 +274,7 @@ union Response {
     struct SetRobotModeResp mvmode;
     struct SubNodeResetResp subnodereset;
     struct SubscribeAddNodeResp subnodeadd;
+    struct GimbalCtrlSpeedResp gimbspeed;
 };
 union Message {
     struct Header header;

include/robomaster.h

@@ -7,6 +7,7 @@ typedef struct Client* Client;
 #include "message.h"
 #include "chassis.h"
 #include "sdk.h"
+#include "gimbal.h"
 
 Client client_new();
 void client_connect(Client client);

src/connection.c

@@ -35,6 +35,8 @@ message_length(int cmd) {
             return sizeof(struct SetWheelSpeedReq);
         case CHASSIS_SPEED_MODE_CMD:
             return sizeof(struct ChassisSpeedModeReq);
+        case GIMBAL_CTRL_SPEED_CMD:
+            return sizeof(struct GimbalCtrlSpeedReq);
         default:
             return 0;
     }
@@ -56,6 +58,8 @@ message_module(int cmd) {
         case SET_WHEEL_SPEED_CMD:
         case CHASSIS_SPEED_MODE_CMD:
             return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
+        case GIMBAL_CTRL_SPEED_CMD:
+            return host2byte(GIMBAL_HOST, GIMBAL_INDEX);
         default:
             return 0;
     }

src/modules/gimbal.c

@@ -0,0 +1,18 @@
+#include "message.h"
+#include "connection.h"
+#include "robomaster.h"
+
+void
+gimbal_ctrl_speed (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		int16_t p,
+		int16_t y,
+		int16_t r ) {
+    req->gimbspeed.yaw = y;
+    req->gimbspeed.roll = r;
+    req->gimbspeed.pitch = p;
+    req->gimbspeed.ctrl = 0xDC;
+    req_finalize(seq, GIMBAL_CTRL_SPEED_CMD, ack, req);
+}