Add command to rotate gimbal to specific coordinates

include/gimbal.h

@@ -18,3 +18,21 @@ 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 );
+void
+gimbal_rotate_to (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		int16_t p,
+		int16_t y );
+
+#define GIMBAL_ACTION_PUSH_CMD 0xb13f

include/message.h

@@ -3,20 +3,14 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#if defined(_MSC_VER)  // Check if compiling with Microsoft Visual Studio
+#define PACKED __attribute__((__packed__))
-#define PACKED(__DECL__) __pragma(pack(push, 1)) __DECL__ __pragma(pack(pop))
-#elif defined(__GNUC__)  // Check if compiling with GCC or any compiler defining __GNUC__, including Clang
-#define PACKED(__DECL__) __DECL__ __attribute__((packed))
-#else
-#error "Unsupported compiler"
-#endif
 
 static const uint8_t CLIENT_HOST = 9;
 static const uint8_t CLIENT_INDEX = 6;
 
 static const uint8_t PREAMBLE = 0x55;
 
-PACKED( struct Header {
+struct PACKED Header {
 
     // The preamble marks the start of a message and is always 0x55
     uint8_t preamble;
@@ -59,13 +53,13 @@ PACKED( struct Header {
         };
     };
 
-});
+};
 
-PACKED( struct Footer {
+struct PACKED Footer {
     uint16_t crc;
-});
+};
 
-PACKED( struct SetSystemLedReq {
+struct PACKED SetSystemLedReq {
 
     struct Header header;
 
@@ -94,87 +88,87 @@ PACKED( struct SetSystemLedReq {
 
     struct Footer footer;
 
-});
+};
 
-PACKED( struct SetSystemLedResp {
+struct PACKED SetSystemLedResp {
     struct Header header;
     uint8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct SubscribeAddNodeReq
+struct PACKED SubscribeAddNodeReq
 {
     struct Header header;
     uint8_t hostbyte;
     uint32_t sub_vision; // always 0x03000000
     struct Footer footer;
-});
+};
 
-PACKED( struct SubscribeAddNodeResp
+struct PACKED SubscribeAddNodeResp
 {
     struct Header header;
     uint8_t retcode; // 0x00 or 0x50 are acceptable here
     uint8_t hostbyte;
     struct Footer footer;
-});
+};
 
-PACKED( struct SubNodeResetReq
+struct PACKED SubNodeResetReq
 {
     struct Header header;
     uint8_t hostbyte;
     struct Footer footer;
-});
+};
 
-PACKED( struct SubNodeResetResp
+struct PACKED SubNodeResetResp
 {
     struct Header header;
     uint8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct SetRobotModeReq
+struct PACKED SetRobotModeReq
 {
     struct Header header;
     uint8_t mode;
     struct Footer footer;
-});
+};
 
-PACKED( struct SetRobotModeResp
+struct PACKED SetRobotModeResp
 {
     struct Header header;
     uint8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct ChassisSpeedModeReq
+struct PACKED ChassisSpeedModeReq
 {
     struct Header header;
     float speed[3];
     struct Footer footer;
-});
+};
 
-PACKED( struct ChassisSpeedModeResp
+struct PACKED ChassisSpeedModeResp
 {
     struct Header header;
     int8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct SetChassisWheelSpeedReq
+struct PACKED SetChassisWheelSpeedReq
 {
     struct Header header;
     int8_t wheel_speed[4];
     struct Footer footer;
-});
+};
 
-PACKED( struct SetChassisWheelSpeedResp
+struct PACKED SetChassisWheelSpeedResp
 {
     struct Header header;
     int8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct GimbalCtrlSpeedReq
+struct PACKED GimbalCtrlSpeedReq
 {
     struct Header header;
     // Values between -360 and 360
@@ -189,53 +183,120 @@ PACKED( struct GimbalCtrlSpeedReq
     // Always 0xDC
     uint8_t ctrl;
     struct Footer footer;
-});
+};
 
-PACKED( struct GimbalCtrlSpeedResp
+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 GimbalCtrlSpeedResp
 {
     struct Header header;
     int8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct SetWheelSpeedReq
+struct PACKED SetWheelSpeedReq
 {
     struct Header header;
     int16_t wheel_speed[4];
     struct Footer footer;
-});
+};
 
-PACKED( struct SetWheelSpeedResp
+struct PACKED SetWheelSpeedResp
 {
     struct Header header;
     int8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct SdkHeartbeatReq {
+struct PACKED SdkHeartbeatReq {
     struct Header header;
     struct Footer footer;
-});
+};
 
-PACKED( struct SdkHeartbeatResp {
+struct PACKED SdkHeartbeatResp {
     struct Header header;
     uint8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct SetSdkModeReq {
+struct PACKED SetSdkModeReq {
     struct Header header;
     uint8_t enable;
     struct Footer footer;
-});
+};
 
-PACKED( struct SetSdkModeResp {
+struct PACKED SetSdkModeResp {
     struct Header header;
     uint8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct SetSdkConnectionReq {
+struct PACKED SetSdkConnectionReq {
     struct Header header;
     uint8_t control;
     uint8_t host;
@@ -244,32 +305,32 @@ PACKED( struct SetSdkConnectionReq {
     uint32_t ip_address;
     uint16_t port;
     struct Footer footer;
-});
+};
 
-PACKED( struct SetSdkConnectionResp {
+struct PACKED SetSdkConnectionResp {
     struct Header header;
     uint8_t retcode;
     uint8_t state;
     uint32_t config_ip;
     struct Footer footer;
-});
+};
 
-PACKED( struct BlasterFireReq {
+struct PACKED BlasterFireReq {
     struct Header header;
     struct {
         uint8_t type : 4;
         uint8_t times : 4;
     };
     struct Footer footer;
-});
+};
 
-PACKED( struct BlasterFireResp {
+struct PACKED BlasterFireResp {
     struct Header header;
     uint8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct StreamCtrlReq {
+struct PACKED StreamCtrlReq {
     struct Header header;
     uint8_t ctrl;
     struct {
@@ -278,15 +339,15 @@ PACKED( struct StreamCtrlReq {
     };
     uint8_t resolution;
     struct Footer footer;
-});
+};
 
-PACKED( struct StreamCtrlResp {
+struct PACKED StreamCtrlResp {
     struct Header header;
     uint8_t retcode;
     struct Footer footer;
-});
+};
 
-PACKED( struct AddSubMsgReq {
+struct PACKED AddSubMsgReq {
     struct Header header;
     uint8_t node_id;
     uint8_t msg_id;
@@ -296,9 +357,9 @@ PACKED( struct AddSubMsgReq {
     uint64_t subject_uuid;
     uint16_t freq;
     struct Footer footer;
-});
+};
 
-PACKED( struct AddSubMsgResp {
+struct PACKED AddSubMsgResp {
     struct Header header;
     uint8_t retcode;
     uint8_t pub_node_id;
@@ -306,9 +367,9 @@ PACKED( struct AddSubMsgResp {
     uint8_t ack_msg_id;
     uint16_t ack_err_uid_data;
     struct Footer footer;
-});
+};
 
-PACKED( struct PushPeriodMsg {
+struct PACKED PushPeriodMsg {
     struct Header header;
     uint8_t sub_mode;
     uint8_t msg_id;
@@ -330,7 +391,7 @@ PACKED( struct PushPeriodMsg {
         } gimbal;
     };
     struct Footer footer;
-});
+};
 
 union Request {
     struct Header header;
@@ -345,6 +406,7 @@ union Request {
     struct SubNodeResetReq subnodereset;
     struct SubscribeAddNodeReq subnodeadd;
     struct GimbalCtrlSpeedReq gimbspeed;
+    struct GimbalRotateReq gimbalrot;
     struct BlasterFireReq blaster;
     struct StreamCtrlReq stream;
     struct AddSubMsgReq sub;
@@ -362,10 +424,12 @@ union Response {
     struct SubNodeResetResp subnodereset;
     struct SubscribeAddNodeResp subnodeadd;
     struct GimbalCtrlSpeedResp gimbspeed;
+    struct GimbalRotateResp gimbalrot;
     struct BlasterFireResp blaster;
     struct StreamCtrlResp stream;
     struct AddSubMsgResp sub;
     struct PushPeriodMsg push;
+    struct GimbalActionPush gimbalaction;
 };
 union Message {
     struct Header header;

include/roboeasy.h

@@ -42,6 +42,9 @@ int robot_stop(Robot robot);
  */
 int robot_aim(Robot, float p, float y);
 
+int robot_target(Robot robot, float pitch, float yaw);
+int robot_target_to(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.

src/connection.c

@@ -4,26 +4,9 @@
 #include "connection.h"
 
 #include <stdlib.h>
-
-#ifdef _WIN32
-bool StartupWSA() {
-    static bool wsastarted = false;
-    if (!wsastarted) {
-        WORD		wVersionRequested;
-        WSADATA		wsaData;
-
-        wVersionRequested = MAKEWORD(2, 2);
-        if (WSAStartup(wVersionRequested, &wsaData) == 0) {
-            wsastarted = true;
-        }
-    }
-    return wsastarted;
-}
-#else
 #include <sys/socket.h>
 #include <sys/select.h>
 #include <netinet/in.h>
-#endif
 #include <string.h>
 #include <fcntl.h>
 #include <stdio.h>
@@ -53,6 +36,8 @@ message_length(int cmd) {
             return sizeof(struct ChassisSpeedModeReq);
         case GIMBAL_CTRL_SPEED_CMD:
             return sizeof(struct GimbalCtrlSpeedReq);
+        case GIMBAL_ROTATE_CMD:
+            return sizeof(struct GimbalRotateReq);
         case BLASTER_FIRE_CMD:
             return sizeof(struct BlasterFireReq);
         case STREAM_CTRL_CMD:
@@ -82,6 +67,7 @@ message_module(int cmd) {
         case CHASSIS_SPEED_MODE_CMD:
             return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
         case GIMBAL_CTRL_SPEED_CMD:
+        case GIMBAL_ROTATE_CMD:
             return host2byte(GIMBAL_HOST, GIMBAL_INDEX);
         case BLASTER_FIRE_CMD:
             return host2byte(BLASTER_HOST, BLASTER_INDEX);
@@ -123,10 +109,6 @@ connection_new(uint16_t source_port, uint32_t source_ip, uint16_t dest_port, uin
 enum connection_error
 connection_connect(struct Connection* conn) {
 
-#ifdef _WIN32
-    StartupWSA();
-#endif
-
     // Request a UDP socket
     conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
 
@@ -135,13 +117,8 @@ connection_connect(struct Connection* conn) {
         return CONNECTION_LOCAL_BIND;
  
     // Make the socket non-blocking
-#ifdef _WIN32
-    static unsigned long sock_opts = 1;
-    ioctlsocket(conn->sockfd, FIONBIO, &sock_opts);	//Non-Blocking
-#else
     int flags = fcntl(conn->sockfd, F_GETFL);
     fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK);
-#endif
 
     // File descriptors are numbers that count up sequentially,
     // so save the last one as the greatest file descriptor.

src/modules/gimbal.c

@@ -16,3 +16,67 @@ gimbal_ctrl_speed (
     req->gimbspeed.ctrl = 0xDC;
     req_finalize(seq, GIMBAL_CTRL_SPEED_CMD, ack, req);
 }
+
+void
+gimbal_rotate (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		int16_t p,
+		int16_t y ) {
+    //req->gimbalrot.coordinate = 1;
+    //req->gimbalrot.yaw_valid = 1;
+    //req->gimbalrot.pitch_valid = 1;
+    //req->gimbalrot.roll_valid = 0;
+    req->gimbalrot.coord_bitfield = 0;
+    req->gimbalrot.coord_bitfield = 1 | 0 << 1 | 1 << 2 | 1 << 3;
+
+    //req->gimbalrot.frequency = 2;
+    //req->gimbalrot.control = 0;
+    req->gimbalrot.control_bitfield = 0;
+    req->gimbalrot.control_bitfield = 0 | 1 << 2;
+
+    req->gimbalrot.id = 33;
+    req->gimbalrot.yaw = 0;
+    req->gimbalrot.yaw = y;// >> 8;//(y >> 8 | y << 8);
+    req->gimbalrot.roll = 0;
+    req->gimbalrot.pitch = 0;
+    req->gimbalrot.pitch = p;// >> 8;//(p >> 8 | y << 8);
+    req->gimbalrot.error = 0;
+    req->gimbalrot.yaw_speed = 30;
+    req->gimbalrot.roll_speed = 0;
+    req->gimbalrot.pitch_speed = 30;
+    req_finalize(seq, GIMBAL_ROTATE_CMD, ack, req);
+}
+
+void
+gimbal_rotate_to (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		int16_t p,
+		int16_t y ) {
+    //req->gimbalrot.coordinate = 1;
+    //req->gimbalrot.yaw_valid = 1;
+    //req->gimbalrot.pitch_valid = 1;
+    //req->gimbalrot.roll_valid = 0;
+    req->gimbalrot.coord_bitfield = 0;
+    req->gimbalrot.coord_bitfield = 1 | 0 << 1 | 1 << 2 | 4 << 3;
+
+    //req->gimbalrot.frequency = 2;
+    //req->gimbalrot.control = 0;
+    req->gimbalrot.control_bitfield = 0;
+    req->gimbalrot.control_bitfield = 0 | 1 << 2;
+
+    req->gimbalrot.id = 33;
+    req->gimbalrot.yaw = 0;
+    req->gimbalrot.yaw = y;// >> 8;//(y >> 8 | y << 8);
+    req->gimbalrot.roll = 0;
+    req->gimbalrot.pitch = 0;
+    req->gimbalrot.pitch = p;// >> 8;//(p >> 8 | y << 8);
+    req->gimbalrot.error = 0;
+    req->gimbalrot.yaw_speed = 30;
+    req->gimbalrot.roll_speed = 0;
+    req->gimbalrot.pitch_speed = 30;
+    req_finalize(seq, GIMBAL_ROTATE_CMD, ack, req);
+}

src/robo.c

@@ -5,6 +5,7 @@
 #include <string.h>
 #include <stdlib.h>
 #include <arpa/inet.h>
+#include <math.h>
 
 #include <stdio.h>
 
@@ -35,6 +36,11 @@ struct RobotImp {
     uint16_t pitch_ground;
     uint16_t yaw;
     uint16_t pitch;
+    enum {
+        TARGET_NONE,
+        TARGET_REL,
+        TARGET_ABS
+    } dirty_target;
 
     enum {
         CONNECTING_TO_PROXY_PORT,
@@ -87,6 +93,44 @@ robot_aim(Robot robot, float p, float y) {
     return 1;
 }
 
+int
+robot_target(Robot robot, float pitch, float yaw) {
+    // Get the FOV angle of the point in radians
+    float FOV = 120 * (M_PI / 180);
+    yaw = yaw * (FOV / 2);
+    pitch = pitch * (FOV / 2);
+
+    // Convert to degrees
+    yaw = yaw * (180 / M_PI);
+    pitch = pitch * (180 / M_PI);
+
+    // Convert for Robomaster
+    robot->yaw = yaw * 10;
+    robot->pitch = -pitch * 10;
+
+    robot->dirty_target = TARGET_REL;
+    return 1;
+};
+
+int
+robot_target_to(Robot robot, float pitch, float yaw) {
+    // Get the FOV angle of the point in radians
+    float FOV = 120 * (M_PI / 180);
+    yaw = yaw * (FOV / 2);
+    pitch = pitch * (FOV / 2);
+
+    // Convert to degrees
+    yaw = yaw * (180 / M_PI);
+    pitch = pitch * (180 / M_PI);
+
+    // Convert for Robomaster
+    robot->yaw = yaw * 10;
+    robot->pitch = -pitch * 10;
+
+    robot->dirty_target = TARGET_ABS;
+    return 1;
+};
+
 int
 robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
     robot->colors[0] = r;
@@ -219,6 +263,20 @@ robot_work(Robot robot) {
                 req_send(robot->client->dev_conn, &req);
                 robot->dirty_gimbal = false;
             }
+            if(robot->dirty_target) {
+                if(robot->dirty_target == TARGET_REL)
+                    gimbal_rotate (
+                            &req, robot->seq++, true,
+                            robot->pitch,
+                            robot->yaw );
+                else if(robot->dirty_target == TARGET_ABS)
+                    gimbal_rotate_to (
+                            &req, robot->seq++, true,
+                            robot->pitch,
+                            robot->yaw );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_target = TARGET_NONE;
+            }
             if(robot->dirty_colors) {
                 set_system_led (
                         &req, robot->seq++, false,