Add message to rotate gimbal to specific coordinates

include/gimbal.h

@@ -18,3 +18,14 @@ gimbal_ctrl_speed (
 		int16_t p,
 		int16_t y,
 		int16_t r );
+
+#define GIMBAL_ROTATE_CMD 0xb03f
+void
+gimbal_rotate (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		int16_t p,
+		int16_t y );
+
+#define GIMBAL_ACTION_PUSH_CMD 0xb13f

include/message.h

@@ -316,6 +316,73 @@ struct PACKED VisionDetectInfo {
     struct VisionRect rects[];
 };
 
+struct PACKED GimbalActionPush {
+    struct Header header;
+    uint8_t id;
+    uint8_t progress;
+    uint8_t state;
+    int16_t yaw;
+    int16_t roll;
+    int16_t pitch;
+};
+
+struct PACKED GimbalRotateReq {
+    struct Header header;
+    uint8_t id;
+
+    //union {
+        uint8_t control_bitfield;
+    //    struct {
+    //        // always 0 for start
+    //        uint8_t control : 1;
+    //        uint8_t frequency : 2;
+    //    };
+    //};
+
+    //union {
+        uint8_t coord_bitfield;
+    //    struct {
+    //        uint8_t coordinate : 3;
+    //        // always true
+    //        uint8_t pitch_valid : 1;
+    //        // always false
+    //        uint8_t roll_valid : 1;
+    //        // always true
+    //        uint8_t yaw_valid : 1;
+    //    };
+    //};
+
+    union {
+        int16_t yaw;
+        struct {
+            int8_t l_yaw;
+            int8_t h_yaw;
+        };
+    };
+    int16_t roll;
+    union {
+        int16_t pitch;
+        struct {
+            int8_t l_pitch;
+            int8_t h_pitch;
+        };
+    };
+
+    uint16_t error;
+    uint16_t yaw_speed;
+    uint16_t roll_speed;
+    uint16_t pitch_speed;
+
+    struct Footer footer;
+};
+
+struct PACKED GimbalRotateResp {
+    struct Header header;
+    uint8_t retcode;
+    uint8_t accept;
+    struct Footer footer;
+};
+
 struct PACKED VisionDetectEnableReq {
     struct Header header;
     uint16_t type;
@@ -345,6 +412,7 @@ union Request {
     struct BlasterFireReq blaster;
     struct StreamCtrlReq stream;
     struct VisionDetectEnableReq enablevision;
+    struct GimbalRotateReq gimbalrot;
 };
 union Response {
     struct Header header;
@@ -362,10 +430,14 @@ union Response {
     struct BlasterFireResp blaster;
     struct StreamCtrlResp stream;
     struct VisionDetectEnableResp enablevision;
+    struct GimbalRotateResp gimbalrot;
 };
+
 union Push {
+    struct GimbalActionPush gimbalaction;
     struct VisionDetectInfo vision;
 };
+
 union Message {
     struct Header header;
     union Request req;
@@ -373,6 +445,13 @@ union Message {
     union Push push;
 };
 
+enum ACTIONSTATE {
+    ACTION_RUNNING,
+    ACTION_SUCCEEDED,
+    ACTION_FAILED,
+    ACTION_STARTED
+};
+
 enum MESSAGEERR {
     MESSAGEERR_NONE,
     MESSAGEERR_HEADERCRC,

include/roboeasy.h

@@ -59,6 +59,8 @@ int robot_stop(Robot robot);
  */
 int robot_aim(Robot, float p, float y);
 
+int robot_target(Robot robot, float pitch, float yaw);
+
 /*
  * Set the velocity of the robot chassis. A packet will be sent to the robot on
  * the next ready tick of the work function.