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

@@ -185,6 +185,73 @@ struct PACKED GimbalCtrlSpeedReq
     struct Footer footer;
 };
 
+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;
@@ -280,6 +347,52 @@ struct PACKED StreamCtrlResp {
     struct Footer footer;
 };
 
+struct PACKED AddSubMsgReq {
+    struct Header header;
+    uint8_t node_id;
+    uint8_t msg_id;
+    uint8_t reserved;
+    uint8_t sub_mode;
+    uint8_t subject_count;
+    uint64_t subject_uuid;
+    uint16_t freq;
+    struct Footer footer;
+};
+
+struct PACKED AddSubMsgResp {
+    struct Header header;
+    uint8_t retcode;
+    uint8_t pub_node_id;
+    uint8_t ack_sub_mode;
+    uint8_t ack_msg_id;
+    uint16_t ack_err_uid_data;
+    struct Footer footer;
+};
+
+struct PACKED PushPeriodMsg {
+    struct Header header;
+    uint8_t sub_mode;
+    uint8_t msg_id;
+    union {
+        struct {
+            float x;
+            float y;
+            float z;
+        } position;
+        struct {
+            uint16_t yaw_ground;
+            uint16_t pitch_ground;
+            uint16_t yaw;
+            uint16_t pitch;
+            struct {
+                uint8_t option_mode : 2;
+                uint8_t return_center : 1;
+            };
+        } gimbal;
+    };
+    struct Footer footer;
+};
+
 union Request {
     struct Header header;
     struct SetSdkConnectionReq sdkconn;
@@ -293,8 +406,10 @@ union Request {
     struct SubNodeResetReq subnodereset;
     struct SubscribeAddNodeReq subnodeadd;
     struct GimbalCtrlSpeedReq gimbspeed;
+    struct GimbalRotateReq gimbalrot;
     struct BlasterFireReq blaster;
     struct StreamCtrlReq stream;
+    struct AddSubMsgReq sub;
 };
 union Response {
     struct Header header;
@@ -309,8 +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.

include/sdk.h

@@ -108,3 +108,34 @@ subscribe_add_node (
         uint16_t seq,
         bool ack );
 
+#define ADD_SUB_MSG_CMD 0x0348
+
+#define PUSH_PERIOD_MSG_CMD 0x0848
+
+#define DDS_BATTERY 0x000200096862229f
+#define DDS_GIMBAL_BASE 0x00020009f5882874
+#define DDS_VELOCITY 0x0002000949a4009c
+#define DDS_ESC 0x00020009c14cb7c5
+#define DDS_ATTITUDE 0x000200096b986306
+#define DDS_IMU 0x00020009a7985b8d
+#define DDS_POSITION 0x00020009eeb7cece
+#define DDS_SA_STATUS 0x000200094a2c6d55
+#define DDS_CHASSIS_MODE 0x000200094fcb1146
+#define DDS_SBUS 0x0002000988223568
+#define DDS_SERVO 0x000200095f0059e7
+#define DDS_ARM 0x0002000926abd64d
+#define DDS_GRIPPER 0x00020009124d156a
+#define DDS_GIMBAL_POS 0x00020009f79b3c97
+#define DDS_STICK 0x0002000955e9a0fa
+#define DDS_MOVE_MODE 0x00020009784c7bfd
+#define DDS_TOF 0x0002000986e4c05a
+#define DDS_PINBOARD 0x00020009eebb9ffc
+
+void
+add_sub_msg (
+        union Request* req,
+        uint16_t seq,
+        bool ack,
+        uint8_t msg_id,
+        uint8_t freq,
+        uint64_t subject_uuid );

src/connection.c

@@ -7,7 +7,6 @@
 #include <sys/socket.h>
 #include <sys/select.h>
 #include <netinet/in.h>
-#include <arpa/inet.h>
 #include <string.h>
 #include <fcntl.h>
 #include <stdio.h>
@@ -37,10 +36,14 @@ 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:
             return sizeof(struct StreamCtrlReq);
+        case ADD_SUB_MSG_CMD:
+            return sizeof(struct AddSubMsgReq);
         default:
             return 0;
     }
@@ -58,11 +61,13 @@ message_module(int cmd) {
         case SET_ROBOT_MODE_CMD:
         case SUBNODE_RESET_CMD:
         case SUBSCRIBE_ADD_NODE_CMD:
+        case ADD_SUB_MSG_CMD:
             return host2byte(SDK_HOST, SDK_INDEX);
         case SET_WHEEL_SPEED_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);
@@ -79,44 +84,49 @@ int max_fd = -1;
 
 // TODO: Close the socket and return NULL on error
 struct Connection*
-connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip)
+connection_new(uint16_t source_port, uint32_t source_ip, uint16_t dest_port, uint32_t dest_ip)
 {
     struct Connection* conn = malloc(sizeof(struct Connection));
     memset(conn, 0, sizeof(struct Connection));
 
-    // Request a UDP socket
-    conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
-
     // Set the source address and port if they are provided
     if(source_port && source_ip) {
-       struct sockaddr_in loc_addr;
-       loc_addr.sin_family = AF_INET;
-       loc_addr.sin_port = htons(source_port);
-       loc_addr.sin_addr.s_addr = inet_addr(source_ip);
-       if(bind(conn->sockfd, (struct sockaddr*)&loc_addr, sizeof(loc_addr)) < 0)
-       {
-           perror("unable to bind local port");
-           exit(EXIT_FAILURE);
-       }
+       conn->local_addr.sin_family = AF_INET;
+       conn->local_addr.sin_port = htons(source_port);
+       conn->local_addr.sin_addr.s_addr = source_ip;
     }
 
-    // Make the socket non-blocking
-    int flags = fcntl(conn->sockfd, F_GETFL);
-    fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK);
-
     // Set the address of the drone
     memset(&conn->remote_addr, 0, sizeof(conn->remote_addr));
     conn->addrlen = sizeof(conn->remote_addr);
     conn->remote_addr.sin_family = AF_INET;
     conn->remote_addr.sin_port = htons(dest_port);
-    conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip);
+    conn->remote_addr.sin_addr.s_addr = dest_ip;
+
+    return conn;
+}
+
+enum connection_error
+connection_connect(struct Connection* conn) {
+
+    // Request a UDP socket
+    conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+
+    if(conn->local_addr.sin_addr.s_addr
+            && bind(conn->sockfd, (struct sockaddr*)&conn->local_addr, sizeof(conn->local_addr)) < 0)
+        return CONNECTION_LOCAL_BIND;
+ 
+    // Make the socket non-blocking
+    int flags = fcntl(conn->sockfd, F_GETFL);
+    fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK);
 
     // File descriptors are numbers that count up sequentially,
     // so save the last one as the greatest file descriptor.
     // This is needed for polling the sockets later.
     max_fd = conn->sockfd;
 
-    return conn;
+    return CONNECTION_NO_ERROR;
+
 }
 
 struct Connection*

src/connection.h

@@ -6,14 +6,25 @@
 #include <stddef.h>
 #include <netinet/in.h>
 
+#define ROBOT_DEFAULT_WIFI_ADDR 0X102A8C0
+
 struct Connection {
     int sockfd;
     socklen_t addrlen;
     struct sockaddr_in remote_addr;
+    struct sockaddr_in local_addr;
 };
 
 struct Connection*
-connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip);
+connection_new(uint16_t source_port, uint32_t source_ip, uint16_t dest_port, uint32_t dest_ip);
+
+enum connection_error {
+    CONNECTION_NO_ERROR,
+    CONNECTION_LOCAL_BIND
+};
+
+enum connection_error
+connection_connect();
 
 struct Connection*
 connection_poll_ready(struct Client* client);

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/modules/sdk.c

@@ -96,3 +96,24 @@ set_system_led (
     req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req);
 
 }
+
+void
+add_sub_msg (
+        union Request* req,
+        uint16_t seq,
+        bool ack,
+        uint8_t msg_id,
+        uint8_t freq,
+        uint64_t subject_uuid ) {
+
+    req->sub.node_id = host2byte(CLIENT_HOST, CLIENT_INDEX);
+    req->sub.msg_id = msg_id;
+    req->sub.reserved = 0; // NOTE: Always 0
+    req->sub.sub_mode = 0; // NOTE: Always 0
+    req->sub.subject_count = 1; // TODO: Add support for multiple
+    req->sub.subject_uuid = subject_uuid; // TODO: Add support for multiple
+    req->sub.freq = freq;
+
+    req_finalize(seq, ADD_SUB_MSG_CMD, ack, req);
+
+}

src/robo.c

@@ -4,6 +4,8 @@
 
 #include <string.h>
 #include <stdlib.h>
+#include <arpa/inet.h>
+#include <math.h>
 
 #include <stdio.h>
 
@@ -28,6 +30,18 @@ struct RobotImp {
 
     bool sdk_mode;
 
+    float x, y, z;
+
+    uint16_t yaw_ground;
+    uint16_t pitch_ground;
+    uint16_t yaw;
+    uint16_t pitch;
+    enum {
+        TARGET_NONE,
+        TARGET_REL,
+        TARGET_ABS
+    } dirty_target;
+
     enum {
         CONNECTING_TO_PROXY_PORT,
         SETTING_SDK_CONNECTION,
@@ -36,6 +50,8 @@ struct RobotImp {
         RESETTING_SUBNODE,
         SUSBCRIBING_SUBNODE,
         SETTING_MOVEMENT_MODE,
+        SUBSCRIBING_POSITION_STATUS,
+        SUBSCRIBING_GIMBAL_STATUS,
         SENDING_HEARTBEAT,
         WAITING,
         READY,
@@ -77,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;
@@ -110,7 +164,8 @@ robot_work(Robot robot) {
 
         case CONNECTING_TO_PROXY_PORT:
         {
-            robot->client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
+            robot->client->sdk_conn = connection_new(0, 0, 30030, ROBOT_DEFAULT_WIFI_ADDR);
+            connection_connect(robot->client->sdk_conn);
             robot->state = SETTING_SDK_CONNECTION;
             break;
         }
@@ -125,7 +180,8 @@ robot_work(Robot robot) {
 
         case CONNECTING_TO_DEVICE_PORT:
         {
-            robot->client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
+            if(connection_connect(robot->client->dev_conn))
+                robot->state = STOPPED;
             robot->state = SETTING_SDK_MODE;
             break;
         }
@@ -170,6 +226,22 @@ robot_work(Robot robot) {
             break;
         }
 
+        case SUBSCRIBING_POSITION_STATUS:
+        {
+            add_sub_msg ( &req, robot->seq++, true, 20, 1, DDS_POSITION );
+            req_send(robot->client->dev_conn, &req);
+            robot->state = SUBSCRIBING_GIMBAL_STATUS;
+            break;
+        }
+
+        case SUBSCRIBING_GIMBAL_STATUS:
+        {
+            add_sub_msg ( &req, robot->seq++, true, 21, 1, DDS_GIMBAL_POS );
+            req_send(robot->client->dev_conn, &req);
+            robot->state = WAITING;
+            break;
+        }
+
         case READY:
         {
             if(robot->dirty_wheels) {
@@ -191,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,
@@ -244,12 +330,15 @@ robot_work(Robot robot) {
     switch(resp.header.cmd) {
         case 0:
             break;
+        default:
+            break;
         case SET_SDK_CONNECTION_CMD:
             // TODO: Do more with this
             if(resp.resp.sdkconn.retcode) {
                 robot->state = STOPPED;
                 break;
             }
+            robot->client->dev_conn = connection_new(10010, resp.resp.sdkconn.config_ip, 20020, ROBOT_DEFAULT_WIFI_ADDR);
             robot->state = CONNECTING_TO_DEVICE_PORT;
             break;
         case SET_SDK_MODE_CMD:
@@ -279,6 +368,13 @@ robot_work(Robot robot) {
                 robot->state = STOPPED;
                 break;
             }
+            robot->state = SUBSCRIBING_POSITION_STATUS;
+            break;
+        case ADD_SUB_MSG_CMD:
+            if(resp.resp.sub.retcode) {
+                robot->state = STOPPED;
+                break;
+            }
             robot->state = SENDING_HEARTBEAT;
             break;
         case SDK_HEARTBEAT_CMD:
@@ -288,6 +384,20 @@ robot_work(Robot robot) {
             }
             robot->state = READY;
             break;
+        case PUSH_PERIOD_MSG_CMD:
+            switch(resp.resp.push.msg_id) {
+                case 20:
+                    robot->x = resp.resp.push.position.x;
+                    robot->y = resp.resp.push.position.y;
+                    robot->z = resp.resp.push.position.z;
+                    break;
+                case 21:
+                    robot->yaw_ground = resp.resp.push.gimbal.yaw_ground;
+                    robot->pitch_ground = resp.resp.push.gimbal.pitch_ground;
+                    robot->yaw = resp.resp.push.gimbal.yaw;
+                    robot->pitch = resp.resp.push.gimbal.pitch;
+            }
+            break;
     }
 
     return 1;