Remove pause for enabling image recognition

include/camera.h

@@ -38,3 +38,28 @@ stream_ctrl (
 		enum STREAMRESOLUTION res,
 		enum STREAMCTRL ctrl );
 
+static const uint8_t VISION_HOST = 17;
+static const uint8_t VISION_INDEX = 7;
+
+#define VISION_DETECT_ENABLE_CMD 0xA30A
+
+#define VISION_DETECT_ENABLE_PERSON 2
+#define VISION_DETECT_ENABLE_GESTURE 4
+#define VISION_DETECT_ENABLE_LINE 16
+#define VISION_DETECT_ENABLE_MARKER 32
+#define VISION_DETECT_ENABLE_ROBOT 128
+void
+vision_enable (
+		union Request* req,
+        uint16_t seq,
+        bool ack );
+
+#define VISION_DETECT_INFO_CMD 0xA40A
+enum VISIONTYPE {
+    VISIONTYPE_SHOULDER,
+    VISIONTYPE_PERSON,
+    VISIONTYPE_GESTURE,
+    VISIONTYPE_LINE,
+    VISIONTYPE_MARKER,
+    VISIONTYPE_ROBOT
+};

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(__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
+#define PACKED __attribute__((__packed__))
 
 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,53 @@ PACKED( struct GimbalCtrlSpeedReq
     // Always 0xDC
     uint8_t ctrl;
     struct Footer footer;
-});
+};
 
-PACKED( struct GimbalCtrlSpeedResp
+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 +238,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,59 +272,62 @@ 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 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;
-});
-
-PACKED( struct 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;
-});
-
-PACKED( struct PushPeriodMsg {
-    struct Header header;
-    uint8_t sub_mode;
-    uint8_t msg_id;
+struct PACKED VisionRect {
     union {
         struct {
             float x;
             float y;
-            float z;
-        } position;
+            float w;
+            float h;
+            uint32_t info;
+        } rect;
         struct {
-            uint16_t yaw_ground;
-            uint16_t pitch_ground;
-            uint16_t yaw;
-            uint16_t pitch;
+            float x;
+            float y;
+            float theta;
+            float c;
+            uint32_t info;
+        } line;
         struct {
-                uint8_t option_mode : 2;
-                uint8_t return_center : 1;
+            float x;
+            float y;
+            float w;
+            float h;
+            uint16_t info;
+            uint16_t distance;
+        } marker;
     };
-        } gimbal;
 };
+
+struct PACKED VisionDetectInfo {
+    struct Header header;
+    uint8_t type;
+    uint8_t status;
+    uint16_t errcode;
+    uint8_t count;
+    struct VisionRect rects[];
+};
+
+struct PACKED VisionDetectEnableReq {
+    struct Header header;
+    uint16_t type;
     struct Footer footer;
-});
+};
+
+struct PACKED VisionDetectEnableResp {
+    struct Header header;
+    uint8_t retcode;
+    uint16_t error;
+    struct Footer footer;
+};
 
 union Request {
     struct Header header;
@@ -347,7 +344,7 @@ union Request {
     struct GimbalCtrlSpeedReq gimbspeed;
     struct BlasterFireReq blaster;
     struct StreamCtrlReq stream;
-    struct AddSubMsgReq sub;
+    struct VisionDetectEnableReq enablevision;
 };
 union Response {
     struct Header header;
@@ -364,13 +361,16 @@ union Response {
     struct GimbalCtrlSpeedResp gimbspeed;
     struct BlasterFireResp blaster;
     struct StreamCtrlResp stream;
-    struct AddSubMsgResp sub;
-    struct PushPeriodMsg push;
+    struct VisionDetectEnableResp enablevision;
+};
+union Push {
+    struct VisionDetectInfo vision;
 };
 union Message {
     struct Header header;
     union Request req;
     union Response resp;
+    union Push push;
 };
 
 enum MESSAGEERR {

include/roboeasy.h

@@ -1,11 +1,28 @@
 #pragma once
 
+#include "message.h"
+
 #include <stdbool.h>
 
 // Handle for the high level robot interface
 struct RobotImp;
 typedef struct RobotImp* Robot;
 
+#define FRAGMENT_MESSAGE 1
+#define FRAGMENT_RECTANGLE 2
+#define FRAGMENT_LINE 3
+#define FRAGMENT_MARKER 4
+
+struct Fragment {
+    int type;
+    union {
+        union Message message;
+        struct VisionRect rect;
+    };
+};
+
+int robot_poll(Robot robot, struct Fragment* fragment);
+
 /*
  * Return a handle to the high level robot interface, or NULL on error.
  */

include/sdk.h

@@ -108,34 +108,3 @@ 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

@@ -4,26 +4,10 @@
 #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 <arpa/inet.h>
 #include <string.h>
 #include <fcntl.h>
 #include <stdio.h>
@@ -57,8 +41,8 @@ message_length(int cmd) {
             return sizeof(struct BlasterFireReq);
         case STREAM_CTRL_CMD:
             return sizeof(struct StreamCtrlReq);
-        case ADD_SUB_MSG_CMD:
-            return sizeof(struct AddSubMsgReq);
+        case VISION_DETECT_ENABLE_CMD:
+            return sizeof(struct VisionDetectEnableReq);
         default:
             return 0;
     }
@@ -76,7 +60,6 @@ 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:
@@ -87,6 +70,8 @@ message_module(int cmd) {
             return host2byte(BLASTER_HOST, BLASTER_INDEX);
         case STREAM_CTRL_CMD:
             return host2byte(STREAM_HOST, STREAM_INDEX);
+        case VISION_DETECT_ENABLE_CMD:
+            return host2byte(VISION_HOST, VISION_INDEX);
         default:
             return 0;
     }
@@ -98,58 +83,44 @@ int max_fd = -1;
 
 // TODO: Close the socket and return NULL on error
 struct Connection*
-connection_new(uint16_t source_port, uint32_t source_ip, uint16_t dest_port, uint32_t dest_ip)
+connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* 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) {
-       conn->local_addr.sin_family = AF_INET;
-       conn->local_addr.sin_port = htons(source_port);
-       conn->local_addr.sin_addr.s_addr = 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);
        }
+    }
+ 
+    // 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 = dest_ip;
-
-    return conn;
-}
-
-enum connection_error
-connection_connect(struct Connection* conn) {
-
-#ifdef _WIN32
-    StartupWSA();
-#endif
-
-    // 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
-#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
+    conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip);
 
     // 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 CONNECTION_NO_ERROR;
-
+    return conn;
 }
 
 struct Connection*

src/connection.h

@@ -6,25 +6,14 @@
 #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(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();
+connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip);
 
 struct Connection*
 connection_poll_ready(struct Client* client);

src/message.c

@@ -1,9 +1,14 @@
 #include "message.h"
+#include "camera.h" // FIXME: hack
 #include "crc.h"
 
 enum MESSAGEERR
 message_validate(const union Message* message) {
 
+    // FIXME: hack
+    if(message->header.cmd == VISION_DETECT_INFO_CMD)
+        return MESSAGEERR_NONE;
+
     uint16_t length = message->header.length & 0x1FFF & ~0x400;
 
     if(message->header.crc != crc8(message, 3))

src/modules/camera.c

@@ -17,4 +17,12 @@ stream_ctrl (
     req_finalize(seq, STREAM_CTRL_CMD, ack, req);
 }
 
+void
+vision_enable (
+		union Request* req,
+        uint16_t seq,
+        bool ack ) {
+    req->enablevision.type = VISION_DETECT_ENABLE_PERSON;
+    req_finalize(seq, VISION_DETECT_ENABLE_CMD, ack, req);
+}
 

src/modules/sdk.c

@@ -96,24 +96,3 @@ 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,12 +4,21 @@
 
 #include <string.h>
 #include <stdlib.h>
-#include <arpa/inet.h>
 
 #include <stdio.h>
 
+struct FragmentLink {
+    struct Fragment fragment;
+    struct FragmentLink *next;
+};
+
 struct RobotImp {
 
+    struct {
+        struct FragmentLink *head;
+        struct FragmentLink *free;
+    } frag_queue;
+
     struct Client* client;
     uint16_t seq;
 
@@ -29,13 +38,6 @@ struct RobotImp {
 
     bool sdk_mode;
 
-    float x, y, z;
-
-    uint16_t yaw_ground;
-    uint16_t pitch_ground;
-    uint16_t yaw;
-    uint16_t pitch;
-
     enum {
         CONNECTING_TO_PROXY_PORT,
         SETTING_SDK_CONNECTION,
@@ -44,8 +46,6 @@ struct RobotImp {
         RESETTING_SUBNODE,
         SUSBCRIBING_SUBNODE,
         SETTING_MOVEMENT_MODE,
-        SUBSCRIBING_POSITION_STATUS,
-        SUBSCRIBING_GIMBAL_STATUS,
         SENDING_HEARTBEAT,
         WAITING,
         READY,
@@ -108,6 +108,61 @@ robot_heartbeat(Robot robot) {
 
 }
 
+inline
+static
+void
+enqueue_fragment (
+        Robot robot,
+        const struct Fragment fragment ) {
+
+    struct FragmentLink *link;
+    if(robot->frag_queue.free) {
+        link = robot->frag_queue.free;
+        robot->frag_queue.free = link->next;
+    } else {
+        link = malloc(sizeof(struct FragmentLink));
+        link->next = NULL;
+    }
+
+    link->fragment = fragment;
+
+    if(robot->frag_queue.head) {
+        robot->frag_queue.head->next = link;
+    } else {
+        robot->frag_queue.head = link;
+    }
+
+}
+
+inline
+static
+void
+dequeue_fragment (
+        Robot robot,
+        struct Fragment *fragment ) {
+
+    fragment->type = 0;
+    if(!robot->frag_queue.head)
+        return;
+
+    struct FragmentLink *link = robot->frag_queue.head;
+    *fragment = link->fragment;
+
+    robot->frag_queue.head = link->next;
+    link->next = NULL;
+    if(robot->frag_queue.free) {
+        robot->frag_queue.free->next = link;
+    } else {
+        robot->frag_queue.free = link;
+    }
+
+}
+
+int robot_poll(Robot robot, struct Fragment* fragment) {
+    dequeue_fragment(robot, fragment);
+    return fragment->type;
+}
+
 int
 robot_work(Robot robot) {
 
@@ -120,8 +175,7 @@ robot_work(Robot robot) {
 
         case CONNECTING_TO_PROXY_PORT:
         {
-            robot->client->sdk_conn = connection_new(0, 0, 30030, ROBOT_DEFAULT_WIFI_ADDR);
-            connection_connect(robot->client->sdk_conn);
+            robot->client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
             robot->state = SETTING_SDK_CONNECTION;
             break;
         }
@@ -136,8 +190,7 @@ robot_work(Robot robot) {
 
         case CONNECTING_TO_DEVICE_PORT:
         {
-            if(connection_connect(robot->client->dev_conn))
-                robot->state = STOPPED;
+            robot->client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
             robot->state = SETTING_SDK_MODE;
             break;
         }
@@ -182,22 +235,6 @@ 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) {
@@ -269,10 +306,37 @@ robot_work(Robot robot) {
 
     union Message resp;
     poll_message(robot->client, &resp);
+    if(resp.header.cmd) {
+        const struct Fragment fragment = {
+            .type = FRAGMENT_MESSAGE,
+            .message = resp
+        };
+        enqueue_fragment(robot, fragment);
+    }
     switch(resp.header.cmd) {
         case 0:
             break;
-        default:
+        case VISION_DETECT_INFO_CMD:
+            // enqueue rectangles
+            for(int i = 0; i < resp.push.vision.count; i++) {
+                int type;
+                switch(resp.push.vision.type) {
+                    case VISIONTYPE_SHOULDER:
+                    case VISIONTYPE_PERSON:
+                    case VISIONTYPE_GESTURE:
+                    case VISIONTYPE_ROBOT:
+                        type = FRAGMENT_RECTANGLE;
+                    case VISIONTYPE_LINE:
+                        type = FRAGMENT_LINE;
+                    case VISIONTYPE_MARKER:
+                        type = FRAGMENT_MARKER;
+                }
+                const struct Fragment fragment = {
+                    .type = type,
+                    .rect = resp.push.vision.rects[i]
+                };
+                enqueue_fragment(robot, fragment);
+            }
             break;
         case SET_SDK_CONNECTION_CMD:
             // TODO: Do more with this
@@ -280,7 +344,6 @@ robot_work(Robot robot) {
                 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:
@@ -310,13 +373,6 @@ 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:
@@ -326,20 +382,6 @@ 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;

src/sdl.c

@@ -1,3 +1,4 @@
+#include "robomaster.h"
 #include "roboeasy.h"
 #include "SDL2/SDL.h"
 
@@ -60,6 +61,64 @@ int main(int argc, char* argv[]) {
     SDL_GetWindowSize(win, &w, &h);
 
     while(robot_work(robot)) {
+
+        struct Fragment fragment;
+        while(robot_poll(robot, &fragment)) {
+            switch(fragment.type) {
+                case FRAGMENT_RECTANGLE:
+                case FRAGMENT_LINE:
+                case FRAGMENT_MARKER:
+                    printf("Rect(%f,%f,%f,%f)\n",
+                            fragment.rect.rect.x,
+                            fragment.rect.rect.y,
+                            fragment.rect.rect.w,
+                            fragment.rect.rect.h );
+                    break;
+                case FRAGMENT_MESSAGE:
+                    switch(fragment.message.header.cmd) {
+                        case VISION_DETECT_INFO_CMD:
+                            if(fragment.message.push.vision.errcode) {
+                                printf("Vision error %d\n", fragment.message.push.vision.errcode);
+                                break;
+                            }
+                            printf("Received %d rectangles\n", fragment.message.push.vision.count);
+                            printf("\tVision type %d\n", fragment.message.push.vision.type);
+                            printf("\tVision status %d\n", fragment.message.push.vision.status);
+                            break;
+                        case SDK_HEARTBEAT_CMD:
+                            if(fragment.message.resp.heartbeat.retcode) {
+                                printf("ERROR: Heartbeat error\n");
+                                break;
+                            }
+                            printf("Heartbeat\n");
+                            break;
+                        case STREAM_CTRL_CMD:
+                            if(fragment.message.resp.stream.retcode) {
+                                printf("ERROR: Stream not enabled\n");
+                                break;
+                            }
+                            printf("Stream enabled\n");
+                            break;
+                        case VISION_DETECT_ENABLE_CMD:
+                            if(fragment.message.resp.enablevision.retcode) {
+                                printf("ERROR: Vision not enabled\n");
+                                break;
+                            }
+                            if(fragment.message.resp.enablevision.error) {
+                                printf("ERROR: Vision not enabled %d\n",
+                                    fragment.message.resp.enablevision.error);
+                                break;
+                            }
+                            printf("Vision enabled\n");
+                            break;
+                    }
+                    break;
+                default:
+                    printf("Unhandled fragment type\n");
+                    break;
+            }
+        }
+
         SDL_Event event;
         while(SDL_PollEvent(&event)) {
             switch(event.type) {