Add OpenCV and FFMPEG to CMake

The low level API needed to change to accomodate the hight level API,
which is a major revision.

CMakeLists.txt

@@ -2,8 +2,8 @@ cmake_minimum_required(VERSION 3.15)
 
 project (
     RoboMasterSDK
-    VERSION 1.0.0
-    LANGUAGES C
+    VERSION 2.0.0
+    LANGUAGES C CXX
 )
 
 set_property(GLOBAL PROPERTY C_STANDARD 17)
@@ -15,8 +15,12 @@ add_library(robomaster
     src/message.c
     src/modules/sdk.c
     src/modules/chassis.c
+    src/modules/gimbal.c
+    src/modules/blaster.c
+    src/modules/camera.c
     src/connection.c
     src/robomaster.c
+    src/robo.c
 )
 
 target_include_directories(robomaster
@@ -26,14 +30,27 @@ target_include_directories(robomaster
     include
 )
 
-find_package(SDL2)
+find_package(SDL2 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(PkgConfig REQUIRED)
 
-if(SDL2_FOUND)
-    add_executable(robomasterapp
-        src/sdl.c
-    )
-    target_link_libraries(robomasterapp
-        robomaster
-	SDL2::SDL2-static
-    )
-endif()
+pkg_check_modules(FFMPEG REQUIRED IMPORTED_TARGET libavcodec libavformat libavutil libswscale)
+
+add_executable(robomasterapp
+    src/sdl.cpp
+)
+target_link_libraries(robomasterapp
+    robomaster
+    SDL2::SDL2
+    ${OpenCV_LIBS}
+    PkgConfig::FFMPEG
+)
+
+add_executable(stream
+    src/stream.cpp
+)
+target_link_libraries(stream
+    SDL2::SDL2
+    ${OpenCV_LIBS}
+    PkgConfig::FFMPEG
+)

include/blaster.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "message.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+static const uint8_t BLASTER_HOST = 23;
+static const uint8_t BLASTER_INDEX = 0;
+
+#define BLASTER_FIRE_CMD 0x513F
+
+enum FIRETYPE {
+    FIRETYPE_WATER = 0,
+    FIRETYPE_INFRARED = 1
+};
+
+enum FIRETIMES {
+    ONE = 1,
+    TWO = 2,
+    THREE = 3,
+    FOUR = 4,
+    FIVE = 5
+};
+
+void
+blaster_fire (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		enum FIRETYPE type,
+		enum FIRETIMES times );
+

include/camera.h

@@ -0,0 +1,65 @@
+#pragma once
+
+#include "message.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+static const uint8_t STREAM_HOST = 1;
+static const uint8_t STREAM_INDEX = 0;
+
+#define STREAM_CTRL_CMD 0xD23F
+
+#define VIDEO_STREAM_PORT 40921
+
+enum STREAMSTATE {
+    STREAMSTATE_OFF = 0,
+    STREAMSTATE_ON = 1
+};
+
+enum STREAMRESOLUTION {
+    RES_720P = 0,
+    RES_360P = 1,
+    RES_540P = 2
+};
+
+enum STREAMCTRL {
+    STREAMCTRL_SDK = 1,
+    STREAMCTRL_VIDEO = 2,
+    STREAMCTRL_AUDIO = 3
+};
+
+void
+stream_ctrl (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		enum STREAMSTATE state,
+		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/chassis.h

@@ -8,21 +8,25 @@
 static const uint8_t CHASSIS_HOST = 3;
 static const uint8_t CHASSIS_INDEX = 6;
 
-static const uint16_t SET_WHEEL_SPEED_CMD = 0x203F;
+#define SET_WHEEL_SPEED_CMD 0x203F
 
 void
 set_wheel_speed (
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		int16_t w1,
 		int16_t w2,
 		int16_t w3,
 		int16_t w4 );
 
-static const uint16_t CHASSIS_SPEED_MODE_CMD = 0x213F;
+#define CHASSIS_SPEED_MODE_CMD 0x213F
 
 void
 chassis_speed_mode (
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		float x,
 		float y,
 		float z );

include/connection.h

@@ -0,0 +1,13 @@
+#pragma once
+
+struct Connection;
+typedef Connection* Connection;
+
+struct Connection*
+conn_new (
+        struct Connection* connection,
+        unsigned int source_port,
+        const char* source_ip,
+        unsigned int dest_port,
+        const char* dest_ip );
+extern const size_t SIZEOF_CONN;

include/gimbal.h

@@ -0,0 +1,31 @@
+#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 );
+
+#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

@@ -5,7 +5,10 @@
 
 #define PACKED __attribute__((__packed__))
 
-const uint8_t PREAMBLE = 0x55;
+static const uint8_t CLIENT_HOST = 9;
+static const uint8_t CLIENT_INDEX = 6;
+
+static const uint8_t PREAMBLE = 0x55;
 
 struct PACKED Header {
 
@@ -165,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;
@@ -221,6 +248,154 @@ struct PACKED SetSdkConnectionResp {
     struct Footer footer;
 };
 
+struct PACKED BlasterFireReq {
+    struct Header header;
+    struct {
+        uint8_t type : 4;
+        uint8_t times : 4;
+    };
+    struct Footer footer;
+};
+
+struct PACKED BlasterFireResp {
+    struct Header header;
+    uint8_t retcode;
+    struct Footer footer;
+};
+
+struct PACKED StreamCtrlReq {
+    struct Header header;
+    uint8_t ctrl;
+    struct {
+        uint8_t state : 4;
+        uint8_t conn_type : 4;
+    };
+    uint8_t resolution;
+    struct Footer footer;
+};
+
+struct PACKED StreamCtrlResp {
+    struct Header header;
+    uint8_t retcode;
+    struct Footer footer;
+};
+
+struct PACKED VisionRect {
+    union {
+        struct {
+            float x;
+            float y;
+            float w;
+            float h;
+            uint32_t info;
+        } rect;
+        struct {
+            float x;
+            float y;
+            float theta;
+            float c;
+            uint32_t info;
+        } line;
+        struct {
+            float x;
+            float y;
+            float w;
+            float h;
+            uint16_t info;
+            uint16_t distance;
+        } marker;
+    };
+};
+
+struct PACKED VisionDetectInfo {
+    struct Header header;
+    uint8_t type;
+    uint8_t status;
+    uint16_t errcode;
+    uint8_t count;
+    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;
+    struct Footer footer;
+};
+
+struct PACKED VisionDetectEnableResp {
+    struct Header header;
+    uint8_t retcode;
+    uint16_t error;
+    struct Footer footer;
+};
+
 union Request {
     struct Header header;
     struct SetSdkConnectionReq sdkconn;
@@ -233,6 +408,11 @@ union Request {
     struct SetRobotModeReq mvmode;
     struct SubNodeResetReq subnodereset;
     struct SubscribeAddNodeReq subnodeadd;
+    struct GimbalCtrlSpeedReq gimbspeed;
+    struct BlasterFireReq blaster;
+    struct StreamCtrlReq stream;
+    struct VisionDetectEnableReq enablevision;
+    struct GimbalRotateReq gimbalrot;
 };
 union Response {
     struct Header header;
@@ -246,11 +426,30 @@ union Response {
     struct SetRobotModeResp mvmode;
     struct SubNodeResetResp subnodereset;
     struct SubscribeAddNodeResp subnodeadd;
+    struct GimbalCtrlSpeedResp gimbspeed;
+    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;
     union Response resp;
+    union Push push;
+};
+
+enum ACTIONSTATE {
+    ACTION_RUNNING,
+    ACTION_SUCCEEDED,
+    ACTION_FAILED,
+    ACTION_STARTED
 };
 
 enum MESSAGEERR {
@@ -261,61 +460,3 @@ enum MESSAGEERR {
 
 enum MESSAGEERR
 message_validate(const union Message* message);
-
-static inline uint8_t host2byte(uint8_t host, uint8_t index) {
-    return index * 32 + host;
-}
-
-static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) {
-    *host = (b & 0x1F);
-    *index = b >> 5;
-}
-
-static
-inline
-size_t
-message_length(uint16_t cmd) {
-    switch(cmd) {
-        case SET_SDK_CONNECTION_CMD:
-            return sizeof(struct SetSdkConnectionReq);
-        case SDK_HEARTBEAT_CMD:
-            return sizeof(struct SdkHeartbeatReq);
-        case SET_SDK_MODE_CMD:
-            return sizeof(struct SetSdkModeReq);
-        case SET_SYSTEM_LED_CMD:
-            return sizeof(struct SetSystemLedReq);
-        case SET_ROBOT_MODE_CMD:
-            return sizeof(struct SetRobotModeReq);
-        case SUBNODE_RESET_CMD:
-            return sizeof(struct SubNodeResetReq);
-        case SUBSCRIBE_ADD_NODE_CMD:
-            return sizeof(struct SubscribeAddNodeReq);
-        case SET_WHEEL_SPEED_CMD:
-            return sizeof(struct SetWheelSpeedReq);
-        case CHASSIS_SPEED_MODE_CMD:
-            return sizeof(struct ChassisSpeedModeReq);
-        default:
-            return 0;
-    }
-}
-
-static
-inline
-uint8_t
-message_module(uint16_t cmd) {
-    switch(cmd) {
-        case SET_SDK_CONNECTION_CMD:
-        case SDK_HEARTBEAT_CMD:
-        case SET_SDK_MODE_CMD:
-        case SET_SYSTEM_LED_CMD:
-        case SET_ROBOT_MODE_CMD:
-        case SUBNODE_RESET_CMD:
-        case SUBSCRIBE_ADD_NODE_CMD:
-            return host2byte(SDK_HOST, SDK_INDEX);
-        case SET_WHEEL_SPEED_CMD:
-        case CHASSIS_SPEED_MODE_CMD:
-            return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
-        default:
-            return 0;
-    }
-}

include/roboeasy.h

@@ -1,8 +1,27 @@
 #pragma once
 
+#include "message.h"
+
+#include <stdbool.h>
+
 // Handle for the high level robot interface
-struct Robot;
-typedef struct Robot* Robot;
+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.
@@ -29,6 +48,19 @@ int robot_init(Robot robot);
  */
 int robot_stop(Robot robot);
 
+/*
+ * Set the gimble speed. A packet will be sent to the robot on
+ * the next ready tick of the work function.
+ *
+ * robot: The robot to set the velocity of
+ * p: pitch speed
+ * y: yaw speed
+ * returns: 0 on success, non-zero on failure
+ */
+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.
@@ -54,6 +86,10 @@ int robot_drive(Robot, float x, float y, float r);
  */
 int robot_led(Robot, unsigned char r, unsigned char g, unsigned char b);
 
+int robot_blast(Robot);
+
+int robot_stream(Robot, bool);
+
 /*
  * If the robot is in ready mode, this makes the robot send a heartbeat in
  * the next tick of the work function. The work function will not do anything

include/robomaster.h

@@ -1,14 +1,26 @@
 #pragma once
 
 // Public stuff
-struct Client;
-typedef struct Client* Client;
+struct ClientImp;
+typedef struct ClientImp* Client;
 
 #include "message.h"
 #include "chassis.h"
 #include "sdk.h"
+#include "gimbal.h"
+#include "blaster.h"
+#include "camera.h"
 
 Client client_new();
 void client_connect(Client client);
 void poll_message(Client client, union Message* message);
 
+static inline uint8_t host2byte(uint8_t host, uint8_t index) {
+    return index * 32 + host;
+}
+
+static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) {
+    *host = (b & 0x1F);
+    *index = b >> 5;
+}
+

include/sdk.h

@@ -8,7 +8,7 @@
 static const uint8_t SDK_HOST = 9;
 static const uint8_t SDK_INDEX = 0;
 
-static const uint16_t SET_SDK_CONNECTION_CMD = 0xD43F;
+#define SET_SDK_CONNECTION_CMD 0xD43F
 
 enum CONNECTION {
     CONNECTION_WIFI_AP = 0,
@@ -18,23 +18,31 @@ enum CONNECTION {
 
 void
 set_sdk_connection(
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		enum CONNECTION connection_type,
 		uint32_t ip_address,
 		uint16_t port );
 
-static const uint16_t SDK_HEARTBEAT_CMD = 0xD53F;
+#define SDK_HEARTBEAT_CMD 0xD53F
 
 void
 sdk_heartbeat(
-		Client session );
+		union Request* req,
+        uint16_t seq,
+        bool ack );
 
-static const uint16_t SET_SDK_MODE_CMD = 0xD13F;
+#define SET_SDK_MODE_CMD 0xD13F
 
 void
-set_sdk_mode(Client session, bool enable);
+set_sdk_mode(
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		bool enable );
 
-static const uint16_t SET_SYSTEM_LED_CMD = 0x333F;
+#define SET_SYSTEM_LED_CMD 0x333F
 
 enum LEDCOMP {
     LEDCOMP_BOTTOM_BACK = 0x1,
@@ -57,7 +65,9 @@ enum LEDEFFECT {
 };
 
 void set_system_led (
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		uint8_t red,
 		uint8_t green,
 		uint8_t blue,
@@ -67,7 +77,7 @@ void set_system_led (
 		uint16_t t1,
 		uint16_t t2 );
 
-static const uint16_t SET_ROBOT_MODE_CMD = 0x463F;
+#define SET_ROBOT_MODE_CMD 0x463F
 
 enum MOVEMENTMODE {
     MOVEMENTMODE_FREE,
@@ -77,18 +87,24 @@ enum MOVEMENTMODE {
 
 void
 set_robot_mode (
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		enum MOVEMENTMODE mode );
 
-static const uint16_t SUBNODE_RESET_CMD = 0x0248;
+#define SUBNODE_RESET_CMD 0x0248
 
 void
 subnode_reset (
-		Client session );
+		union Request* req,
+        uint16_t seq,
+        bool ack );
 
-static const uint16_t SUBSCRIBE_ADD_NODE_CMD = 0x0148;
+#define SUBSCRIBE_ADD_NODE_CMD 0x0148
 
 void
 subscribe_add_node (
-		Client session );
+		union Request* req,
+        uint16_t seq,
+        bool ack );
 

src/client.h

@@ -2,11 +2,9 @@
 
 #include <stdint.h>
 
-struct Client {
+struct ClientImp {
 
-   uint8_t hostbyte;
-
-   int16_t seq;
+    uint16_t seq;
 
    union {
        struct {
@@ -18,6 +16,3 @@ struct Client {
 
 };
 
-static const uint8_t CLIENT_HOST = 9;
-static const uint8_t CLIENT_INDEX = 6;
-

src/connection.c

@@ -1,4 +1,4 @@
-#include "message.h"
+#include "robomaster.h"
 #include "client.h"
 #include "crc.h"
 #include "connection.h"
@@ -12,6 +12,74 @@
 #include <fcntl.h>
 #include <stdio.h>
 
+static
+inline
+size_t
+message_length(int cmd) {
+    switch(cmd) {
+        case SET_SDK_CONNECTION_CMD:
+            return sizeof(struct SetSdkConnectionReq);
+        case SDK_HEARTBEAT_CMD:
+            return sizeof(struct SdkHeartbeatReq);
+        case SET_SDK_MODE_CMD:
+            return sizeof(struct SetSdkModeReq);
+        case SET_SYSTEM_LED_CMD:
+            return sizeof(struct SetSystemLedReq);
+        case SET_ROBOT_MODE_CMD:
+            return sizeof(struct SetRobotModeReq);
+        case SUBNODE_RESET_CMD:
+            return sizeof(struct SubNodeResetReq);
+        case SUBSCRIBE_ADD_NODE_CMD:
+            return sizeof(struct SubscribeAddNodeReq);
+        case SET_WHEEL_SPEED_CMD:
+            return sizeof(struct SetWheelSpeedReq);
+        case CHASSIS_SPEED_MODE_CMD:
+            return sizeof(struct ChassisSpeedModeReq);
+        case GIMBAL_CTRL_SPEED_CMD:
+            return sizeof(struct GimbalCtrlSpeedReq);
+        case BLASTER_FIRE_CMD:
+            return sizeof(struct BlasterFireReq);
+        case STREAM_CTRL_CMD:
+            return sizeof(struct StreamCtrlReq);
+        case VISION_DETECT_ENABLE_CMD:
+            return sizeof(struct VisionDetectEnableReq);
+        case GIMBAL_ROTATE_CMD:
+            return sizeof(struct GimbalRotateReq);
+        default:
+            return 0;
+    }
+}
+
+static
+inline
+uint8_t
+message_module(int cmd) {
+    switch(cmd) {
+        case SET_SDK_CONNECTION_CMD:
+        case SDK_HEARTBEAT_CMD:
+        case SET_SDK_MODE_CMD:
+        case SET_SYSTEM_LED_CMD:
+        case SET_ROBOT_MODE_CMD:
+        case SUBNODE_RESET_CMD:
+        case SUBSCRIBE_ADD_NODE_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);
+        case STREAM_CTRL_CMD:
+            return host2byte(STREAM_HOST, STREAM_INDEX);
+        case VISION_DETECT_ENABLE_CMD:
+            return host2byte(VISION_HOST, VISION_INDEX);
+        default:
+            return 0;
+    }
+}
+
 // The greated file descriptor is needed for polling the sockets.
 // It needs to be global for the whole process.
 int max_fd = -1;
@@ -59,7 +127,7 @@ connection_new(unsigned int source_port, const char* source_ip, unsigned int des
 }
 
 struct Connection*
-connection_poll_ready(struct Client* client) {
+connection_poll_ready(struct ClientImp* client) {
 
     // Return a null connection if no sockets have been opened
     if(max_fd < 0)
@@ -72,8 +140,8 @@ connection_poll_ready(struct Client* client) {
         if(client->conns[i])
             FD_SET(client->conns[i]->sockfd, &read_fds);
 
-    //struct timeval timeout = {0, 0};
-    int result = select(max_fd + 1, &read_fds, NULL, NULL, NULL);
+    struct timeval timeout = {0, 0};
+    int result = select(max_fd + 1, &read_fds, NULL, NULL, &timeout);
 
     // Check for socket polling errors
     if(result < 0) {
@@ -118,26 +186,24 @@ connection_read(struct Connection* conn, union Message* resp) {
 }
 
 void
-req_send(struct Connection* conn, union Request* req, size_t length) {
+req_send(struct Connection* conn, union Request* req) {
     if(!conn || !req) return;
+    size_t length = message_length(req->header.cmd);
     sendto(conn->sockfd, req, length, 0, (struct sockaddr*)&conn->remote_addr, conn->addrlen);
 }
 
 void
-req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, uint8_t hostbyte, bool need_ack, size_t length, union Request* req) {
+req_finalize(uint16_t seq, uint16_t cmd, bool need_ack, union Request* req) {
 
+    size_t length = message_length(cmd);
     req->header.preamble = 0x55;
-    //req->header.length = length & 0x1FFF | 0x400;
-    req->header.length = length;
+    req->header.length = (length & 0x1FFF) | 0x400;
     req->header.crc = crc8(req, 3);
-    req->header.seq_id = client->seq++;
-    req->header.sender = client->hostbyte;
-    // TODO: Figure out what this is supposed to be
-    //req->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX);
-    req->header.receiver = hostbyte;
+    req->header.seq_id = seq;
+    req->header.sender = host2byte(CLIENT_HOST, CLIENT_INDEX);
+    req->header.receiver = message_module(cmd);
     req->header.ack_needed = need_ack;
-    req->header.cmdset = cmdset;
-    req->header.cmdid = cmdid;
+    req->header.cmd = cmd;
 
     struct Footer* footer = (void*)req + length - sizeof(struct Footer);
     uint16_t crc = crc16(req, length - sizeof(struct Footer));

src/connection.h

@@ -16,14 +16,14 @@ struct Connection*
 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);
+connection_poll_ready(struct ClientImp* client);
 
 void
 connection_read(struct Connection* connection, union Message* resp);
 
 void
-req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, uint8_t hostbyte, bool need_ack, size_t length, union Request* req);
+req_finalize(uint16_t seq, uint16_t cmd, bool need_ack, union Request* req);
 
 void
-req_send(struct Connection* conn, union Request* req, size_t length);
+req_send(struct Connection* conn, union Request* req);
 

src/message.c

@@ -1,10 +1,15 @@
 #include "message.h"
+#include "camera.h" // FIXME: hack
 #include "crc.h"
 
 enum MESSAGEERR
 message_validate(const union Message* message) {
 
-    uint16_t length = message->header.length;
+    // 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))
         return MESSAGEERR_HEADERCRC;

src/modules/blaster.c

@@ -0,0 +1,16 @@
+#include "message.h"
+#include "connection.h"
+#include "robomaster.h"
+
+void
+blaster_fire (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		enum FIRETYPE type,
+		enum FIRETIMES times ) {
+    req->blaster.type = type;
+    req->blaster.times = times;
+    req_finalize(seq, BLASTER_FIRE_CMD, ack, req);
+}
+

src/modules/camera.c

@@ -0,0 +1,28 @@
+#include "message.h"
+#include "connection.h"
+#include "robomaster.h"
+
+void
+stream_ctrl (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		enum STREAMSTATE state,
+		enum STREAMRESOLUTION res,
+		enum STREAMCTRL ctrl ) {
+    req->stream.state = state;
+    req->stream.resolution = res;
+    req->stream.ctrl = ctrl;
+    req->stream.conn_type = 0; // Hardcode to WiFi
+    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/chassis.c

@@ -4,30 +4,30 @@
 
 void
 set_wheel_speed (
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		int16_t w1,
 		int16_t w2,
 		int16_t w3,
 		int16_t w4 ) {
-    union Request req = {0};
-    req.wheel.wheel_speed[0] = w1;
-    req.wheel.wheel_speed[1] = w2;
-    req.wheel.wheel_speed[2] = w3;
-    req.wheel.wheel_speed[3] = w4;
-    req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, host2byte(CHASSIS_HOST, CHASSIS_INDEX), false, sizeof(struct SetWheelSpeedReq), &req);
-    req_send(session->dev_conn, &req, sizeof(struct SetWheelSpeedReq));
+    req->wheel.wheel_speed[0] = w1;
+    req->wheel.wheel_speed[1] = w2;
+    req->wheel.wheel_speed[2] = w3;
+    req->wheel.wheel_speed[3] = w4;
+    req_finalize(seq, SET_WHEEL_SPEED_CMD, ack, req);
 }
 
 void
 chassis_speed_mode (
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		float x,
 		float y,
 		float z ) {
-    union Request req = {0};
-    req.chsspeed.speed[0] = x;
-    req.chsspeed.speed[1] = y;
-    req.chsspeed.speed[2] = z;
-    req_finalize(session, 0x3F, CHASSIS_SPEED_MODE_CMDID, host2byte(CHASSIS_HOST, CHASSIS_INDEX), false, sizeof(struct ChassisSpeedModeReq), &req);
-    req_send(session->dev_conn, &req, sizeof(struct ChassisSpeedModeReq));
+    req->chsspeed.speed[0] = x;
+    req->chsspeed.speed[1] = y;
+    req->chsspeed.speed[2] = z;
+    req_finalize(seq, CHASSIS_SPEED_MODE_CMD, ack, req);
 }

src/modules/gimbal.c

@@ -0,0 +1,50 @@
+#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);
+}
+
+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 = 15;
+    req->gimbalrot.roll_speed = 0;
+    req->gimbalrot.pitch_speed = 15;
+    req_finalize(seq, GIMBAL_ROTATE_CMD, ack, req);
+}

src/modules/sdk.c

@@ -4,72 +4,74 @@
 
 void
 set_sdk_connection(
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		enum CONNECTION connection_type,
 		uint32_t ip_address,
 		uint16_t port ) {
 
-    union Request req = {0};
-    req.sdkconn.control = 0;
-    req.sdkconn.host = session->hostbyte;
-    req.sdkconn.connection = connection_type;
-    req.sdkconn.protocol = 0;
-    req.sdkconn.ip_address = ip_address;
-    req.sdkconn.port = port;
-    req_finalize(session, 0x3F, SET_SDK_CONNECTION_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSdkConnectionReq), &req);
-    req_send(session->sdk_conn, &req, sizeof(struct SetSdkConnectionReq));
+    req->sdkconn.control = 0;
+    req->sdkconn.host = host2byte(CLIENT_HOST, CLIENT_INDEX);
+    req->sdkconn.connection = connection_type;
+    req->sdkconn.protocol = 0;
+    req->sdkconn.ip_address = ip_address;
+    req->sdkconn.port = port;
+    req_finalize(seq, SET_SDK_CONNECTION_CMD, ack, req);
 }
 
 void
 set_sdk_mode(
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		bool enable ) {
-    union Request req = {0};
-    req.sdkmode.enable = enable;
-    req_finalize(session, 0x3F, SET_SDK_MODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSdkModeReq), &req);
-    req_send(session->dev_conn, &req, sizeof(struct SetSdkModeReq));
+    req->sdkmode.enable = enable;
+    req_finalize(seq, SET_SDK_MODE_CMD, ack, req);
 }
 
 void
 sdk_heartbeat(
-		Client session ) {
-    union Request req = {0};
-    req_finalize(session, 0x3F, SDK_HEARTBEAT_CMDID, host2byte(SDK_HOST, SDK_INDEX), false, sizeof(struct SdkHeartbeatReq), &req);
-    req_send(session->dev_conn, &req, sizeof(struct SdkHeartbeatReq));
+		union Request* req,
+        uint16_t seq,
+        bool ack ) {
+    req_finalize(seq, SDK_HEARTBEAT_CMD, ack, req);
 }
 
 void
 set_robot_mode (
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		enum MOVEMENTMODE mode ) {
-    union Request req = {0};
-    req.mvmode.mode = mode;
-    req_finalize(session, 0x3F, SET_ROBOT_MODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetRobotModeReq), &req);
-    req_send(session->dev_conn, &req, sizeof(struct SetRobotModeReq));
+    req->mvmode.mode = mode;
+    req_finalize(seq, SET_ROBOT_MODE_CMD, ack, req);
 }
 
 void
 subnode_reset (
-		Client session ) {
-    union Request req = {0};
-    req.subnodereset.hostbyte = session->hostbyte;
-    req_finalize(session, 0x48, SUBNODE_RESET_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SubNodeResetReq), &req);
-    req_send(session->dev_conn, &req, sizeof(struct SubNodeResetReq));
+		union Request* req,
+        uint16_t seq,
+        bool ack ) {
+    req->subnodereset.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
+    req_finalize(seq, SUBNODE_RESET_CMD, ack, req);
 }
 
 void
 subscribe_add_node (
-		Client session ) {
-    union Request req = {0};
-    req.subnodeadd.hostbyte = session->hostbyte;
-    req.subnodeadd.sub_vision = 0x03000000;
-    req_finalize(session, 0x48, SUBSCRIBE_ADD_NODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SubscribeAddNodeReq), &req);
-    req_send(session->dev_conn, &req, sizeof(struct SubscribeAddNodeReq));
+		union Request* req,
+        uint16_t seq,
+        bool ack ) {
+    req->subnodeadd.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
+    req->subnodeadd.sub_vision = 0x03000000;
+    req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, ack, req);
 }
 
 void
 set_system_led (
-		Client session,
+		union Request* req,
+        uint16_t seq,
+        bool ack,
 		uint8_t red,
 		uint8_t green,
 		uint8_t blue,
@@ -79,20 +81,18 @@ set_system_led (
 		uint16_t t1,
 		uint16_t t2 ) {
 
-    union Request req = {0};
 
-    req.led.comp_mask = comp;
-    req.led.led_mask = led_mask;
-    req.led.effect_mode = effect;
-    req.led.control_mode = 7;
-    req.led.red = red;
-    req.led.green = green;
-    req.led.blue = blue;
-    req.led.loop = 0;
-    req.led.t1 = t1;
-    req.led.t2 = t2;
+    req->led.comp_mask = comp;
+    req->led.led_mask = led_mask;
+    req->led.effect_mode = effect;
+    req->led.control_mode = 7;
+    req->led.red = red;
+    req->led.green = green;
+    req->led.blue = blue;
+    req->led.loop = 0;
+    req->led.t1 = t1;
+    req->led.t2 = t2;
 
-    req_finalize(session, 0x3F, SET_SYSTEM_LED_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSystemLedReq), &req);
-    req_send(session->dev_conn, &req, sizeof(struct SetSystemLedReq));
+    req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req);
 
 }

src/robo.c

@@ -0,0 +1,440 @@
+#include "roboeasy.h"
+#include "robomaster.h"
+#include "connection.h"
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <stdio.h>
+
+struct FragmentLink {
+    struct Fragment fragment;
+    struct FragmentLink *next;
+};
+
+struct RobotImp {
+
+    struct {
+        struct FragmentLink *head;
+        struct FragmentLink *free;
+    } frag_queue;
+
+    struct ClientImp* client;
+    uint16_t seq;
+
+    int16_t wheels[4];
+    bool dirty_wheels;
+
+    uint8_t colors[3];
+    bool dirty_colors;
+
+    int16_t gimbal[2];
+    bool dirty_gimbal;
+
+    int16_t pitch;
+    int16_t yaw;
+    bool dirty_target;
+
+    bool dirty_blaster;
+
+    bool stream_state;
+    bool dirty_stream;
+
+    bool sdk_mode;
+
+    enum {
+        CONNECTING_TO_PROXY_PORT,
+        SETTING_SDK_CONNECTION,
+        CONNECTING_TO_DEVICE_PORT,
+        SETTING_SDK_MODE,
+        RESETTING_SUBNODE,
+        SUSBCRIBING_SUBNODE,
+        SETTING_MOVEMENT_MODE,
+        SENDING_HEARTBEAT,
+        WAITING,
+        READY,
+        STOPPED
+    } state;
+
+};
+
+int
+robot_stream(Robot robot, bool state) {
+    robot->stream_state = state;
+    robot->dirty_stream = true;
+    return 1;
+}
+
+int
+robot_blast(Robot robot) {
+    robot->dirty_blaster = true;
+    return 1;
+}
+
+int
+robot_drive(Robot robot, float x, float y, float r) {
+    // TODO: move individual wheel speed calculation to work function
+    // This function may be used in a timer handler, so fast as possible
+    robot->wheels[0] = (y + x + r) * 1000;
+    robot->wheels[1] = -(y - x - r) * 1000;
+    robot->wheels[2] = -(y + x - r) * 1000;
+    robot->wheels[3] = (y - x + r) * 1000;
+    robot->dirty_wheels = true;
+    return 1;
+}
+
+int
+robot_aim(Robot robot, float p, float y) {
+    robot->gimbal[0] = p * 360;
+    robot->gimbal[1] = y * 360;
+    robot->dirty_gimbal = true;
+    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 = true;
+    return 1;
+};
+
+int
+robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
+    robot->colors[0] = r;
+    robot->colors[1] = g;
+    robot->colors[2] = b;
+    robot->dirty_colors = true;
+    return 1;
+}
+
+int
+robot_heartbeat(Robot robot) {
+
+    if(robot->state != READY)
+        return 0;
+
+    robot->state = SENDING_HEARTBEAT;
+
+    return 1;
+
+}
+
+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) {
+
+    union Request req = {0};
+
+    switch(robot->state) {
+
+        case WAITING:
+            break;
+
+        case CONNECTING_TO_PROXY_PORT:
+        {
+            robot->client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
+            robot->state = SETTING_SDK_CONNECTION;
+            break;
+        }
+
+        case SETTING_SDK_CONNECTION:
+        {
+            set_sdk_connection(&req, robot->seq++, true, CONNECTION_WIFI_AP, 0, 10010);
+            req_send(robot->client->sdk_conn, &req);
+            robot->state = WAITING;
+            break;
+        }
+
+        case CONNECTING_TO_DEVICE_PORT:
+        {
+            robot->client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
+            robot->state = SETTING_SDK_MODE;
+            break;
+        }
+
+        case SETTING_SDK_MODE:
+        {
+            set_sdk_mode(&req, robot->seq++, true, robot->sdk_mode);
+            req_send(robot->client->dev_conn, &req);
+            robot->state = WAITING;
+            break;
+        }
+
+        case RESETTING_SUBNODE:
+        {
+            subnode_reset(&req, robot->seq++, true);
+            req_send(robot->client->dev_conn, &req);
+            robot->state = WAITING;
+            break;
+        }
+
+        case SUSBCRIBING_SUBNODE:
+        {
+            subscribe_add_node(&req, robot->seq++, true);
+            req_send(robot->client->dev_conn, &req);
+            robot->state = WAITING;
+            break;
+        }
+
+        case SETTING_MOVEMENT_MODE:
+        {
+            set_robot_mode(&req, robot->seq++, true, MOVEMENTMODE_GIMBAL_LEAD);
+            req_send(robot->client->dev_conn, &req);
+            robot->state = WAITING;
+            break;
+        }
+
+        case SENDING_HEARTBEAT:
+        {
+            sdk_heartbeat(&req, robot->seq++, true);
+            req_send(robot->client->sdk_conn, &req);
+            robot->state = WAITING;
+            break;
+        }
+
+        case READY:
+        {
+            if(robot->dirty_wheels) {
+                set_wheel_speed (
+                        &req, robot->seq++, false,
+                        robot->wheels[0],
+                        robot->wheels[1],
+                        robot->wheels[2],
+                        robot->wheels[3] );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_wheels = false;
+            }
+            if(robot->dirty_gimbal) {
+                gimbal_ctrl_speed (
+                        &req, robot->seq++, false,
+                        robot->gimbal[0],
+                        robot->gimbal[1],
+                        0 );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_gimbal = false;
+            }
+            if(robot->dirty_target) {
+                gimbal_rotate (
+                        &req, robot->seq++, true,
+                        robot->pitch,
+                        robot->yaw );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_target = false;
+            }
+            if(robot->dirty_colors) {
+                set_system_led (
+                        &req, robot->seq++, false,
+                        robot->colors[0],
+                        robot->colors[1],
+                        robot->colors[2],
+                        LEDCOMP_ALL,
+                        0xFFFF,
+                        LEDEFFECT_ON,
+                        100,
+                        100 );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_colors = false;
+            }
+            if(robot->dirty_blaster) {
+                blaster_fire (
+                    &req, robot->seq++, false,
+                    FIRETYPE_INFRARED,
+                    ONE
+                );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_blaster = false;
+            }
+            if(robot->dirty_stream) {
+                stream_ctrl (
+                    &req, robot->seq++, true,
+                    robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
+                    RES_720P,
+                    STREAMCTRL_SDK
+                );
+                req_send(robot->client->dev_conn, &req);
+                stream_ctrl (
+                    &req, robot->seq++, true,
+                    robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
+                    RES_720P,
+                    STREAMCTRL_VIDEO
+                );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_stream = false;
+            }
+            break;
+        }
+
+        case STOPPED:
+            return 0;
+
+    }
+
+    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;
+        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
+            if(resp.resp.sdkconn.retcode) {
+                robot->state = STOPPED;
+                break;
+            }
+            robot->state = CONNECTING_TO_DEVICE_PORT;
+            break;
+        case SET_SDK_MODE_CMD:
+            // TODO: Do more with this
+            if(resp.resp.sdkmode.retcode) {
+                robot->state = STOPPED;
+                break;
+            }
+            robot->state = robot->sdk_mode ? RESETTING_SUBNODE : STOPPED;
+            break;
+        case SUBNODE_RESET_CMD:
+            if(resp.resp.subnodereset.retcode) {
+                robot->state = STOPPED;
+                break;
+            }
+            robot->state = SUSBCRIBING_SUBNODE;
+            break;
+        case SUBSCRIBE_ADD_NODE_CMD:
+            if(resp.resp.subnodeadd.retcode && resp.resp.subnodeadd.retcode != 0x50) {
+                robot->state = STOPPED;
+                break;
+            }
+            robot->state = SETTING_MOVEMENT_MODE;
+            break;
+        case SET_ROBOT_MODE_CMD:
+            if(resp.resp.mvmode.retcode) {
+                robot->state = STOPPED;
+                break;
+            }
+            robot->state = SENDING_HEARTBEAT;
+            break;
+        case SDK_HEARTBEAT_CMD:
+            if(resp.resp.heartbeat.retcode) {
+                robot->state = STOPPED;
+                break;
+            }
+            robot->state = READY;
+            break;
+    }
+
+    return 1;
+
+}
+
+Robot robot_new() {
+    struct RobotImp* robot = malloc(sizeof(struct RobotImp));
+    memset(robot, 0, sizeof(struct RobotImp));
+    return robot;
+}
+
+int robot_init(Robot robot) {
+    robot->client = client_new();
+    robot->sdk_mode = true;
+    robot->state = CONNECTING_TO_PROXY_PORT;
+    return 0;
+}
+
+int robot_stop(Robot robot) {
+    robot->sdk_mode = false;
+    robot->state = SETTING_SDK_MODE;
+    return 0;
+}

src/robomaster.c

@@ -8,10 +8,9 @@
 #include <string.h>
 
 Client client_new() {
-    struct Client* client = malloc(sizeof(struct Client));
-    memset(client, 0, sizeof(struct Client));
+    struct ClientImp* client = malloc(sizeof(struct ClientImp));
+    memset(client, 0, sizeof(struct ClientImp));
     // TODO: Make this configurable
-    client->hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
     return client;
 }
 
@@ -19,8 +18,6 @@ void client_connect(Client client) {
     client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
     // TODO: This should probably use the source IP in the response body
     client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
-
-    set_sdk_connection(client, CONNECTION_WIFI_AP, 0, 10010);
 }
 
 void poll_message(Client client, union Message* resp) {
@@ -32,5 +29,7 @@ void poll_message(Client client, union Message* resp) {
 
 void drive(Client client, float x, float y, float r) {
     float w1 = y + x + r, w2 = y - x -r, w3 = y + x -r, w4 = y - x + r;
-    set_wheel_speed(client, w1 * 1000, -w2 * 1000, -w3 * 1000, w4 * 1000);
+    union Request req;
+    set_wheel_speed(&req, client->seq, false, w1 * 1000, -w2 * 1000, -w3 * 1000, w4 * 1000);
+    req_send(client->dev_conn, &req);
 }

src/sdl.c

@@ -1,122 +0,0 @@
-#include "robomaster.h"
-#include "SDL2/SDL.h"
-
-#include <stdio.h>
-#include <stdlib.h>
-
-int main(int argc, char* argv[]) {
-    if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_GAMECONTROLLER) < 0) {
-        fprintf(stderr, "%s", SDL_GetError());
-	return 1;
-    }
-    SDL_Window* win = SDL_CreateWindow(
-		    "Robomaster",
-		    SDL_WINDOWPOS_UNDEFINED,
-		    SDL_WINDOWPOS_UNDEFINED,
-		    800, 300,
-		    SDL_WINDOW_RESIZABLE );
-    if(!win) {
-        fprintf(stderr, "%s", SDL_GetError());
-	return 1;
-    }
-
-    Client client = client_new();
-    client_connect(client);
-
-    union Message msg;
-    poll_message(client, &msg);
-    if(msg.header.cmdid != SET_SDK_CONNECTION_CMDID || msg.resp.sdkconn.retcode) {
-        fprintf(stderr, "Could not set SDK connection\n");
-        return 1;
-    }
-
-    set_sdk_mode(client, true);
-    poll_message(client, &msg);
-    if(msg.header.cmdid != SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) {
-        fprintf(stderr, "Could not set SDK mode\n");
-        return 1;
-    }
-
-    subnode_reset(client);
-    poll_message(client, &msg);
-    if(msg.header.cmdid != SUBNODE_RESET_CMDID || msg.resp.subnodereset.retcode) {
-        fprintf(stderr, "Could not reset subnode subscription\n");
-        return 1;
-    }
-
-    subscribe_add_node(client);
-    poll_message(client, &msg);
-    if(msg.header.cmdid != SUBSCRIBE_ADD_NODE_CMDID || (msg.resp.subnodeadd.retcode && msg.resp.subnodeadd.retcode != 0x50)) {
-        fprintf(stderr, "Could not subscribe node\n");
-        return 1;
-    }
-
-    set_robot_mode(client, MOVEMENTMODE_FREE);
-    poll_message(client, &msg);
-    if(msg.header.cmdid != SET_ROBOT_MODE_CMDID || msg.resp.mvmode.retcode) {
-        fprintf(stderr, "Could not set move mode\n");
-        return 1;
-    }
-
-    bool quit = false;
-    Uint32 time = SDL_GetTicks();
-    float x = 0, y = 0, z = 0;
-    while(!quit) {
-        SDL_Event event;
-        while(SDL_PollEvent(&event)) {
-            switch(event.type) {
-                case SDL_KEYUP:
-                case SDL_KEYDOWN:
-                    switch(event.key.keysym.scancode) {
-                        case SDL_SCANCODE_Q:
-                            z = event.type == SDL_KEYUP ? 0 : 250;
-                            break;
-                        case SDL_SCANCODE_E:
-                            z = event.type == SDL_KEYUP ? 0 : -250;
-                            break;
-                        case SDL_SCANCODE_LEFT:
-                        case SDL_SCANCODE_A:
-                            x = event.type == SDL_KEYUP ? 0 : 250;
-                            break;
-                        case SDL_SCANCODE_RIGHT:
-                        case SDL_SCANCODE_D:
-                            x = event.type == SDL_KEYUP ? 0 : -250;
-                            break;
-                        case SDL_SCANCODE_UP:
-                        case SDL_SCANCODE_W:
-                            y = event.type == SDL_KEYUP ? 0 : 250;
-                            break;
-                        case SDL_SCANCODE_DOWN:
-                        case SDL_SCANCODE_S:
-                            y = event.type == SDL_KEYUP ? 0 : -250;
-                            break;
-                        default: break;
-		    }
-                case SDL_WINDOWEVENT:
-                    if(event.window.event != SDL_WINDOWEVENT_CLOSE) break;
-                case SDL_QUIT:
-                    quit = true;
-                default: break;
-	    }
-	}
-	Uint32 currtime = SDL_GetTicks();
-	if(currtime - time >= 75)
-	{
-            time = currtime;
-            sdk_heartbeat(client);
-            float w1 = y + x + z, w2 = y - x -z, w3 = y + x -z, w4 = y - x + z;
-            set_wheel_speed(client, w1, -w2, -w3, w4);
-	}
-    }
-
-    set_sdk_mode(client, false);
-    poll_message(client, &msg);
-    if(msg.header.cmdid != SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) {
-        fprintf(stderr, "Could not disable SDK mode\n");
-        return 1;
-    }
-
-    SDL_Quit();
-    return 0;
-}
-

src/sdl.cpp

@@ -0,0 +1,417 @@
+extern "C" {
+#include "robomaster.h"
+#include "roboeasy.h"
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libswscale/swscale.h>
+#include <libavutil/imgutils.h>
+}
+
+#include <SDL2/SDL.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/dnn/dnn.hpp>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <thread>
+#include <mutex>
+
+struct {
+    unsigned char r;
+    unsigned char g;
+    unsigned char b;
+} const colors[] = {
+    {0xFF, 0x00, 0x00},
+    {0x00, 0xFF, 0x00},
+    {0x00, 0x00, 0xFF}
+};
+int color = 0;
+
+float x = 0, y = 0, z = 0;
+int target_x = 0, target_y = 0;
+int yaw_cur = 0, pitch_cur = 0;
+static Uint32 drive_timer_handler(Uint32 interval, void* param) {
+    robot_drive((Robot)param, x, y, z);
+    return 75;
+}
+static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) {
+    robot_heartbeat((Robot)param);
+    return 1000;
+}
+
+std::mutex mtx;
+cv::Mat img;
+std::vector<cv::Rect> found;
+std::vector<double> weights;
+unsigned target;
+
+bool stop = false;
+bool track = false;
+
+static void processFrameThread(Robot robot) {
+    cv::dnn::Net net = cv::dnn::readNet("yolov3-tiny.weights", "yolov3-tiny.cfg");
+
+    while(!stop) {
+        std::unique_lock<std::mutex> lock(mtx);
+
+        if(!img.empty()) {
+            found.clear();
+            weights.clear();
+            cv::Mat blob = cv::dnn::blobFromImage(img, 1/255.0, cv::Size(416, 416), cv::Scalar(0, 0, 0), true, false);
+            net.setInput(blob);
+            std::vector<cv::Mat> outs;
+            net.forward(outs, net.getUnconnectedOutLayersNames());
+            float *data = (float*)outs[0].data;
+            for(int i = 0; i < outs[0].rows; ++i) {
+                float final_score = data[4] * data[5];
+                if(final_score >= 0.0075)
+                {
+                    weights.push_back(final_score);
+                    int cx = data[0] * img.cols;
+                    int cy = data[1] * img.rows;
+                    int width = data[2] * img.cols;
+                    int height = data[3] * img.rows;
+                    int left = cx - width / 2;
+                    int top = cy - height / 2;
+                    found.push_back(cv::Rect(left, top, width, height));
+                }
+                data += 85;
+            }
+
+            target = 0;
+            for(unsigned i = 0; i < weights.size(); i++) {
+                if(weights[i] > weights[target])
+                    target = i;
+            }
+
+            if(track && !weights.empty()) {
+                float yaw = target_x = found[target].x;
+                float pitch = target_y = found[target].y;
+
+                // Normalize the coordinates
+                yaw = 2 * (yaw - img.cols / 2) / img.cols;
+                pitch = 2 * (pitch - img.rows / 2) / img.rows;
+
+                robot_target(robot, pitch, yaw);
+            }
+        }
+
+        lock.unlock();
+
+        SDL_Delay(250);
+    }
+
+}
+
+static void captureFrameThread(SDL_Window* window, const char* fname) {
+
+    SDL_Delay(750);
+
+    av_register_all();
+    avcodec_register_all();
+    AVFormatContext* pFormatCtx = avformat_alloc_context();
+    
+    if (avformat_open_input(&pFormatCtx, fname, NULL, NULL) != 0) {
+        std::cerr << "Couldn't open stream\n";
+        return;
+    }
+    
+    if (avformat_find_stream_info(pFormatCtx, NULL) < 0) {
+        std::cerr << "Couldn't find stream information\n";
+        return;
+    }
+
+    int videoStream = -1;
+    for (unsigned int i = 0; i < pFormatCtx->nb_streams; i++) {
+        if (pFormatCtx->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_VIDEO) {
+            videoStream = i;
+            break;
+        }
+    }
+
+    if (videoStream == -1) {
+        std::cerr << "Didn't find a video stream\n";
+        return;
+    }
+
+    AVCodecParameters* pCodecParameters = pFormatCtx->streams[videoStream]->codecpar;
+    AVCodec* pCodec = avcodec_find_decoder(pCodecParameters->codec_id);
+
+    if (pCodec == NULL) {
+        std::cerr << "Unsupported codec\n";
+        return;
+    }
+
+    AVCodecContext* pCodecCtx = avcodec_alloc_context3(pCodec);
+    pCodecCtx->pix_fmt = AV_PIX_FMT_GRAY8;
+    avcodec_parameters_to_context(pCodecCtx, pCodecParameters);
+
+    if (avcodec_open2(pCodecCtx, pCodec, NULL) < 0) {
+        std::cerr << "Could not open codec\n";
+        return;
+    }
+
+    AVFrame* pFrame = av_frame_alloc();
+    AVFrame* pFrameRGB = av_frame_alloc();
+    
+    int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, pCodecCtx->width, pCodecCtx->height, 1);
+    uint8_t* buffer = (uint8_t*)av_malloc(numBytes * sizeof(uint8_t));
+    av_image_fill_arrays(pFrameRGB->data, pFrameRGB->linesize, buffer, AV_PIX_FMT_BGR24, pCodecCtx->width, pCodecCtx->height, 1);
+
+    SwsContext* sws_ctx = sws_getContext(
+        pCodecCtx->width,
+        pCodecCtx->height,
+        pCodecCtx->pix_fmt,
+        pCodecCtx->width,
+        pCodecCtx->height,
+        AV_PIX_FMT_BGR24,
+        SWS_BILINEAR,
+        NULL,
+        NULL,
+        NULL);
+
+    SDL_Renderer* renderer = SDL_CreateRenderer(window, -1, 0);
+    SDL_Texture* texture = SDL_CreateTexture(renderer, SDL_PIXELFORMAT_BGR24, SDL_TEXTUREACCESS_STATIC, pCodecCtx->width, pCodecCtx->height);
+
+    while (!stop) {
+        AVPacket packet;
+        if(av_read_frame(pFormatCtx, &packet) < 0) {
+            std::cerr << "Error while reading a frame\n";
+            stop = true;
+            break;
+        }
+        
+        if (packet.stream_index == videoStream) {
+            int response = avcodec_send_packet(pCodecCtx, &packet);
+            if (response < 0) {
+                std::cerr << "Error while sending a packet to the decoder: " << response << '\n';
+                stop = true;
+                break;
+            }
+
+            while (response >= 0) {
+                response = avcodec_receive_frame(pCodecCtx, pFrame);
+                if (response == AVERROR(EAGAIN) || response == AVERROR_EOF) {
+                    break;
+                } else if (response < 0) {
+                    std::cerr << "Error while receiving a frame from the decoder: " << response << '\n';
+                    stop = true;
+                    break;
+                }
+
+                if (response >= 0) {
+                    sws_scale(sws_ctx, (uint8_t const* const*)pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
+
+                    std::unique_lock<std::mutex> lock(mtx);
+
+                    img = cv::Mat(pCodecCtx->height, pCodecCtx->width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]);
+                    for (unsigned i = 0; i < found.size(); i++) {
+                        rectangle(img, found[i], cv::Scalar(255 - weights[i] * 255, 0, weights[i] * 255), 3);
+                    }
+
+
+                    SDL_UpdateTexture(texture, NULL, img.data, img.cols * 3);
+                    SDL_RenderDrawLine(renderer, 1280 / 2, 720 / 2, target_x, target_y);
+
+                    SDL_RenderClear(renderer);
+                    SDL_RenderCopy(renderer, texture, NULL, NULL);
+                    SDL_RenderPresent(renderer);
+
+                    lock.unlock();
+                }
+            }
+        }
+        av_packet_unref(&packet);
+
+    }
+
+    av_free(buffer);
+    av_frame_free(&pFrameRGB);
+    av_frame_free(&pFrame);
+    avcodec_close(pCodecCtx);
+    avformat_close_input(&pFormatCtx);
+
+    SDL_DestroyTexture(texture);
+    SDL_DestroyRenderer(renderer);
+}
+
+int main(int argc, char* argv[]) {
+    if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) < 0) {
+        fprintf(stderr, "%s", SDL_GetError());
+        return 1;
+    }
+    printf("Detected %d joysticks\n", SDL_NumJoysticks());
+    SDL_Joystick* joystick = NULL;
+    if(SDL_NumJoysticks() > 0) {
+        joystick = SDL_JoystickOpen(0);
+    }
+    SDL_Window* win = SDL_CreateWindow(
+		    "Robomaster",
+		    SDL_WINDOWPOS_UNDEFINED,
+		    SDL_WINDOWPOS_UNDEFINED,
+		    1280, 720,
+		    SDL_WINDOW_RESIZABLE );
+    if(!win) {
+        fprintf(stderr, "%s", SDL_GetError());
+        return 1;
+    }
+
+    Robot robot = robot_new();
+    robot_init(robot);
+    robot_stream(robot, true);
+
+    SDL_AddTimer(75, drive_timer_handler, robot);
+    SDL_AddTimer(1000, heartbeat_timer_handler, robot);
+
+    int streamcount = 0;
+    std::thread *captureThread = nullptr;
+    std::thread *processThread = nullptr;
+
+    target_x = 720 / 2;
+    target_y = 1280 / 2;
+
+    while(robot_work(robot)) {
+
+        int h = 720, w = 1280;
+
+        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 GIMBAL_ACTION_PUSH_CMD:
+                            if(fragment.message.push.gimbalaction.state != 1)
+                                break;
+                            yaw_cur = fragment.message.push.gimbalaction.yaw;
+                            pitch_cur = fragment.message.push.gimbalaction.pitch;
+                            printf("Gimbal action %d\n", fragment.message.push.gimbalaction.id);
+                            printf("\tProgress %d\n", fragment.message.push.gimbalaction.progress);
+                            printf("\tYaw %d, Pitch %d\n", yaw_cur, pitch_cur);
+                            break;
+                        case GIMBAL_ROTATE_CMD:
+                            if(fragment.message.resp.gimbalrot.retcode) {
+                                printf("ERROR: Gimbal rotate message failure\n");
+                                break;
+                            }
+                            break;
+                        case STREAM_CTRL_CMD:
+                            if(fragment.message.resp.stream.retcode) {
+                                printf("ERROR: Stream not enabled\n");
+                                break;
+                            }
+                            printf("Stream enabled\n");
+                            if(++streamcount >= 2) {
+                                captureThread = new std::thread(captureFrameThread, win, "tcp://192.168.2.1:40921");
+                                processThread = new std::thread(processFrameThread, robot);
+                            }
+                            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) {
+                case SDL_KEYUP:
+                case SDL_KEYDOWN:
+                    switch(event.key.keysym.scancode) {
+                        case SDL_SCANCODE_Q:
+                            z = event.type == SDL_KEYUP ? 0 : 0.25;
+                            break;
+                        case SDL_SCANCODE_E:
+                            z = event.type == SDL_KEYUP ? 0 : -0.25;
+                            break;
+                        case SDL_SCANCODE_LEFT:
+                        case SDL_SCANCODE_A:
+                            x = event.type == SDL_KEYUP ? 0 : 0.25;
+                            break;
+                        case SDL_SCANCODE_RIGHT:
+                        case SDL_SCANCODE_D:
+                            x = event.type == SDL_KEYUP ? 0 : -0.25;
+                            break;
+                        case SDL_SCANCODE_UP:
+                        case SDL_SCANCODE_W:
+                            y = event.type == SDL_KEYUP ? 0 : 0.25;
+                            break;
+                        case SDL_SCANCODE_DOWN:
+                        case SDL_SCANCODE_S:
+                            y = event.type == SDL_KEYUP ? 0 : -0.25;
+                            break;
+                        case SDL_SCANCODE_SPACE:
+                            robot_led(robot, colors[color].r, colors[color].g, colors[color].b);
+                            color = (color + 1) % 3;
+                            break;
+                        case SDL_SCANCODE_RETURN:
+                            robot_blast(robot);
+                            break;
+                        case SDL_SCANCODE_T:
+                            track = true;
+                            break;
+                        default: break;
+		            }
+                    break;
+                case SDL_MOUSEBUTTONDOWN:
+                {
+                    // Get window coordinates
+                    SDL_GetMouseState(&target_x, &target_y);
+
+                    float yaw = target_x;
+                    float pitch = target_y;
+
+                    // Normalize the coordinates
+                    yaw = 2 * (yaw - w / 2) / w;
+                    pitch = 2 * (pitch - h / 2) / h;
+
+                    robot_target(robot, pitch, yaw);
+
+                    break;
+                }
+                case SDL_WINDOWEVENT:
+                    if(event.window.event != SDL_WINDOWEVENT_CLOSE) break;
+                case SDL_QUIT:
+                    robot_stop(robot);
+                    stop = true;
+                default: break;
+	        }
+	    }
+    }
+
+    if(captureThread != nullptr) {
+        captureThread->join();
+        delete captureThread;
+    }
+    if(processThread != nullptr) {
+        processThread->join();
+        delete processThread;
+    }
+    SDL_JoystickClose(joystick);
+    SDL_Quit();
+    return 0;
+}
+

src/stream.cpp

@@ -0,0 +1,233 @@
+#include <iostream>
+#include <thread>
+#include <mutex>
+#include <cmath>
+#include <opencv2/opencv.hpp>
+#include <opencv2/dnn/dnn.hpp>
+#include <SDL2/SDL.h>
+
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libswscale/swscale.h>
+#include <libavutil/imgutils.h>
+}
+
+std::mutex mtx;
+cv::Mat img;
+std::vector<cv::Rect> found;
+std::vector<double> weights;
+unsigned target;
+
+bool stop = false;
+
+static void processFrameThread() {
+    cv::dnn::Net net = cv::dnn::readNet("yolov3-tiny.weights", "yolov3-tiny.cfg");
+
+    while(!stop) {
+        std::unique_lock<std::mutex> lock(mtx);
+
+        if(!img.empty()) {
+            found.clear();
+            weights.clear();
+            cv::Mat blob = cv::dnn::blobFromImage(img, 1/255.0, cv::Size(416, 416), cv::Scalar(0, 0, 0), true, false);
+            net.setInput(blob);
+            std::vector<cv::Mat> outs;
+            net.forward(outs, net.getUnconnectedOutLayersNames());
+            float *data = (float*)outs[0].data;
+            for(int i = 0; i < outs[0].rows; ++i) {
+                float final_score = data[4] * data[5];
+                if(final_score >= 0.0075)
+                {
+                    weights.push_back(final_score);
+                    int cx = data[0] * img.cols;
+                    int cy = data[1] * img.rows;
+                    int width = data[2] * img.cols;
+                    int height = data[3] * img.rows;
+                    int left = cx - width / 2;
+                    int top = cy - height / 2;
+                    found.push_back(cv::Rect(left, top, width, height));
+                }
+                data += 85;
+            }
+        }
+
+        lock.unlock();
+
+        SDL_Delay(250);
+    }
+
+}
+
+static void captureFrameThread(SDL_Window* window, const char* fname) {
+
+    av_register_all();
+    avcodec_register_all();
+    AVFormatContext* pFormatCtx = avformat_alloc_context();
+    
+    if (avformat_open_input(&pFormatCtx, fname, NULL, NULL) != 0) {
+        std::cerr << "Couldn't open stream\n";
+        return;
+    }
+    
+    if (avformat_find_stream_info(pFormatCtx, NULL) < 0) {
+        std::cerr << "Couldn't find stream information\n";
+        return;
+    }
+
+    int videoStream = -1;
+    for (unsigned int i = 0; i < pFormatCtx->nb_streams; i++) {
+        if (pFormatCtx->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_VIDEO) {
+            videoStream = i;
+            break;
+        }
+    }
+
+    if (videoStream == -1) {
+        std::cerr << "Didn't find a video stream\n";
+        return;
+    }
+
+    AVCodecParameters* pCodecParameters = pFormatCtx->streams[videoStream]->codecpar;
+    AVCodec* pCodec = avcodec_find_decoder(pCodecParameters->codec_id);
+
+    if (pCodec == NULL) {
+        std::cerr << "Unsupported codec\n";
+        return;
+    }
+
+    AVCodecContext* pCodecCtx = avcodec_alloc_context3(pCodec);
+    pCodecCtx->pix_fmt = AV_PIX_FMT_GRAY8;
+    avcodec_parameters_to_context(pCodecCtx, pCodecParameters);
+
+    if (avcodec_open2(pCodecCtx, pCodec, NULL) < 0) {
+        std::cerr << "Could not open codec\n";
+        return;
+    }
+
+    AVFrame* pFrame = av_frame_alloc();
+    AVFrame* pFrameRGB = av_frame_alloc();
+    
+    int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, pCodecCtx->width, pCodecCtx->height, 1);
+    uint8_t* buffer = (uint8_t*)av_malloc(numBytes * sizeof(uint8_t));
+    av_image_fill_arrays(pFrameRGB->data, pFrameRGB->linesize, buffer, AV_PIX_FMT_BGR24, pCodecCtx->width, pCodecCtx->height, 1);
+
+    SwsContext* sws_ctx = sws_getContext(
+        pCodecCtx->width,
+        pCodecCtx->height,
+        pCodecCtx->pix_fmt,
+        pCodecCtx->width,
+        pCodecCtx->height,
+        AV_PIX_FMT_BGR24,
+        SWS_BILINEAR,
+        NULL,
+        NULL,
+        NULL);
+
+    //AVRational time_base = pFormatCtx->streams[videoStream]->time_base;
+    //AVRational frame_rate = av_guess_frame_rate(pFormatCtx, pFormatCtx->streams[videoStream], NULL);
+    //uint32_t delay = (av_rescale_q(1, av_inv_q(frame_rate), time_base) / AV_TIME_BASE) * 1000;
+    //printf("delay: %u\n", delay);
+
+    SDL_SetWindowSize(window, pCodecCtx->width, pCodecCtx->height);
+    SDL_Renderer* renderer = SDL_CreateRenderer(window, -1, 0);
+    SDL_Texture* texture = SDL_CreateTexture(renderer, SDL_PIXELFORMAT_BGR24, SDL_TEXTUREACCESS_STATIC, pCodecCtx->width, pCodecCtx->height);
+
+    while (!stop) {
+        AVPacket packet;
+        if(av_read_frame(pFormatCtx, &packet) < 0) {
+            stop = true;
+            break;
+        }
+        
+        if (packet.stream_index == videoStream) {
+            int response = avcodec_send_packet(pCodecCtx, &packet);
+            if (response < 0) {
+                std::cerr << "Error while sending a packet to the decoder: " << response << '\n';
+                return;
+            }
+
+            while (response >= 0) {
+                response = avcodec_receive_frame(pCodecCtx, pFrame);
+                if (response == AVERROR(EAGAIN) || response == AVERROR_EOF) {
+                    break;
+                } else if (response < 0) {
+                    std::cerr << "Error while receiving a frame from the decoder: " << response << '\n';
+                    return;
+                }
+
+                if (response >= 0) {
+                    sws_scale(sws_ctx, (uint8_t const* const*)pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
+
+                    std::unique_lock<std::mutex> lock(mtx);
+
+                    img = cv::Mat(pCodecCtx->height, pCodecCtx->width, CV_8UC3, pFrameRGB->data[0], pFrameRGB->linesize[0]);
+
+                    for (unsigned i = 0; i < found.size(); i++) {
+                        rectangle(img, found[i], cv::Scalar(255 - weights[i] * 255, 0, weights[i] * 255), 3);
+                    }
+
+                    lock.unlock();
+
+                    {
+                        SDL_UpdateTexture(texture, NULL, img.data, img.cols * 3);
+                        SDL_RenderClear(renderer);
+                        SDL_RenderCopy(renderer, texture, NULL, NULL);
+                        SDL_RenderPresent(renderer);
+                    }
+                }
+            }
+        }
+        av_packet_unref(&packet);
+
+        SDL_Event event;
+        while(SDL_PollEvent(&event)) {
+            switch(event.type) {
+                case SDL_QUIT:
+                    stop = true;
+                default: break;
+            }
+        }
+
+        SDL_Delay(33);
+
+    }
+
+    av_free(buffer);
+    av_frame_free(&pFrameRGB);
+    av_frame_free(&pFrame);
+    avcodec_close(pCodecCtx);
+    avformat_close_input(&pFormatCtx);
+
+    SDL_DestroyTexture(texture);
+    SDL_DestroyRenderer(renderer);
+}
+
+int main(int argc, char* argv[]) {
+    if(SDL_Init(SDL_INIT_VIDEO) < 0) {
+        fprintf(stderr, "%s", SDL_GetError());
+        return 1;
+    }
+
+    SDL_Window* win = SDL_CreateWindow(
+		    "Robomaster",
+		    SDL_WINDOWPOS_UNDEFINED,
+		    SDL_WINDOWPOS_UNDEFINED,
+		    800, 300,
+		    SDL_WINDOW_RESIZABLE );
+    if(!win) {
+        fprintf(stderr, "%s", SDL_GetError());
+        return 1;
+    }
+
+    std::thread captureThread(captureFrameThread, win, argv[1]);
+    std::thread processThread(processFrameThread);
+
+    captureThread.join();
+    processThread.join();
+
+    SDL_Quit();
+
+    return 0;
+}
+