Increment major version

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

CMakeLists.txt

@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.15)
 project (
     RoboMasterSDK
     VERSION 2.0.0
-    LANGUAGES C CXX
+    LANGUAGES C
 )
 
 set_property(GLOBAL PROPERTY C_STANDARD 17)
@@ -15,12 +15,8 @@ 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
@@ -30,27 +26,14 @@ target_include_directories(robomaster
     include
 )
 
-find_package(SDL2 REQUIRED)
-find_package(OpenCV REQUIRED)
-find_package(PkgConfig REQUIRED)
-
-pkg_check_modules(FFMPEG REQUIRED IMPORTED_TARGET libavcodec libavformat libavutil libswscale)
+find_package(SDL2)
 
+if(SDL2_FOUND)
     add_executable(robomasterapp
-    src/sdl.cpp
+        src/sdl.c
     )
     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
+	SDL2::SDL2-static
     )
+endif()

include/blaster.h

@@ -1,33 +0,0 @@
-#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

@@ -1,65 +0,0 @@
-#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/gimbal.h

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

@@ -168,30 +168,6 @@ 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;
@@ -248,154 +224,6 @@ 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;
@@ -408,11 +236,6 @@ 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;
@@ -426,30 +249,11 @@ 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 {

include/roboeasy.h

@@ -1,27 +1,8 @@
 #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);
+struct Robot;
+typedef struct Robot* Robot;
 
 /*
  * Return a handle to the high level robot interface, or NULL on error.
@@ -48,19 +29,6 @@ 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.
@@ -86,10 +54,6 @@ 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,15 +1,12 @@
 #pragma once
 
 // Public stuff
-struct ClientImp;
-typedef struct ClientImp* Client;
+struct Client;
+typedef struct Client* 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);

src/client.h

@@ -2,7 +2,7 @@
 
 #include <stdint.h>
 
-struct ClientImp {
+struct Client {
 
     uint16_t seq;
 

src/connection.c

@@ -35,16 +35,6 @@ message_length(int 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;
     }
@@ -66,15 +56,6 @@ message_module(int cmd) {
         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;
     }
@@ -127,7 +108,7 @@ connection_new(unsigned int source_port, const char* source_ip, unsigned int des
 }
 
 struct Connection*
-connection_poll_ready(struct ClientImp* client) {
+connection_poll_ready(struct Client* client) {
 
     // Return a null connection if no sockets have been opened
     if(max_fd < 0)

src/connection.h

@@ -16,7 +16,7 @@ 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 ClientImp* client);
+connection_poll_ready(struct Client* client);
 
 void
 connection_read(struct Connection* connection, union Message* resp);

src/message.c

@@ -1,14 +1,9 @@
 #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/blaster.c

@@ -1,16 +0,0 @@
-#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

@@ -1,28 +0,0 @@
-#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

@@ -15,7 +15,7 @@ set_wheel_speed (
     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);
+    req_finalize(seq, SET_WHEEL_SPEED_CMD, false, req);
 }
 
 void
@@ -29,5 +29,5 @@ chassis_speed_mode (
     req->chsspeed.speed[0] = x;
     req->chsspeed.speed[1] = y;
     req->chsspeed.speed[2] = z;
-    req_finalize(seq, CHASSIS_SPEED_MODE_CMD, ack, req);
+    req_finalize(seq, CHASSIS_SPEED_MODE_CMD, false, req);
 }

src/modules/gimbal.c

@@ -1,50 +0,0 @@
-#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

@@ -17,7 +17,7 @@ set_sdk_connection(
     req->sdkconn.protocol = 0;
     req->sdkconn.ip_address = ip_address;
     req->sdkconn.port = port;
-    req_finalize(seq, SET_SDK_CONNECTION_CMD, ack, req);
+    req_finalize(seq, SET_SDK_CONNECTION_CMD, true, req);
 }
 
 void
@@ -27,7 +27,7 @@ set_sdk_mode(
         bool ack,
 		bool enable ) {
     req->sdkmode.enable = enable;
-    req_finalize(seq, SET_SDK_MODE_CMD, ack, req);
+    req_finalize(seq, SET_SDK_MODE_CMD, true, req);
 }
 
 void
@@ -35,7 +35,7 @@ sdk_heartbeat(
 		union Request* req,
         uint16_t seq,
         bool ack ) {
-    req_finalize(seq, SDK_HEARTBEAT_CMD, ack, req);
+    req_finalize(seq, SDK_HEARTBEAT_CMD, false, req);
 }
 
 void
@@ -45,7 +45,7 @@ set_robot_mode (
         bool ack,
 		enum MOVEMENTMODE mode ) {
     req->mvmode.mode = mode;
-    req_finalize(seq, SET_ROBOT_MODE_CMD, ack, req);
+    req_finalize(seq, SET_ROBOT_MODE_CMD, true, req);
 }
 
 void
@@ -54,7 +54,7 @@ subnode_reset (
         uint16_t seq,
         bool ack ) {
     req->subnodereset.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
-    req_finalize(seq, SUBNODE_RESET_CMD, ack, req);
+    req_finalize(seq, SUBNODE_RESET_CMD, true, req);
 }
 
 void
@@ -64,7 +64,7 @@ subscribe_add_node (
         bool ack ) {
     req->subnodeadd.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
     req->subnodeadd.sub_vision = 0x03000000;
-    req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, ack, req);
+    req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, true, req);
 }
 
 void
@@ -93,6 +93,6 @@ set_system_led (
     req->led.t1 = t1;
     req->led.t2 = t2;
 
-    req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req);
+    req_finalize(seq, SET_SYSTEM_LED_CMD, true, req);
 
 }

src/robo.c

@@ -4,23 +4,10 @@
 
 #include <string.h>
 #include <stdlib.h>
-#include <math.h>
 
-#include <stdio.h>
+struct Robot {
 
-struct FragmentLink {
-    struct Fragment fragment;
-    struct FragmentLink *next;
-};
-
-struct RobotImp {
-
-    struct {
-        struct FragmentLink *head;
-        struct FragmentLink *free;
-    } frag_queue;
-
-    struct ClientImp* client;
+    struct Client* client;
     uint16_t seq;
 
     int16_t wheels[4];
@@ -29,18 +16,6 @@ struct RobotImp {
     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 {
@@ -59,19 +34,6 @@ struct RobotImp {
 
 };
 
-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
@@ -84,33 +46,6 @@ robot_drive(Robot robot, float x, float y, float r) {
     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;
@@ -132,61 +67,6 @@ 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) {
 
@@ -245,7 +125,7 @@ robot_work(Robot robot) {
 
         case SETTING_MOVEMENT_MODE:
         {
-            set_robot_mode(&req, robot->seq++, true, MOVEMENTMODE_GIMBAL_LEAD);
+            set_robot_mode(&req, robot->seq++, true, MOVEMENTMODE_FREE);
             req_send(robot->client->dev_conn, &req);
             robot->state = WAITING;
             break;
@@ -271,23 +151,6 @@ robot_work(Robot robot) {
                 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,
@@ -302,32 +165,6 @@ robot_work(Robot robot) {
                 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;
         }
 
@@ -338,38 +175,9 @@ 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;
-        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) {
@@ -421,8 +229,8 @@ robot_work(Robot robot) {
 }
 
 Robot robot_new() {
-    struct RobotImp* robot = malloc(sizeof(struct RobotImp));
-    memset(robot, 0, sizeof(struct RobotImp));
+    struct Robot* robot = malloc(sizeof(struct Robot));
+    memset(robot, 0, sizeof(struct Robot));
     return robot;
 }
 

src/robomaster.c

@@ -8,8 +8,8 @@
 #include <string.h>
 
 Client client_new() {
-    struct ClientImp* client = malloc(sizeof(struct ClientImp));
-    memset(client, 0, sizeof(struct ClientImp));
+    struct Client* client = malloc(sizeof(struct Client));
+    memset(client, 0, sizeof(struct Client));
     // TODO: Make this configurable
     return client;
 }

src/sdl.c

@@ -0,0 +1,96 @@
+#include "roboeasy.h"
+#include "SDL2/SDL.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+
+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;
+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;
+}
+
+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;
+    }
+
+    Robot robot = robot_new();
+    robot_init(robot);
+    SDL_AddTimer(75, drive_timer_handler, robot);
+    SDL_AddTimer(1000, heartbeat_timer_handler, robot);
+
+    while(robot_work(robot)) {
+        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;
+                        default: break;
+		            }
+                case SDL_WINDOWEVENT:
+                    if(event.window.event != SDL_WINDOWEVENT_CLOSE) break;
+                case SDL_QUIT:
+                    robot_stop(robot);
+                default: break;
+	        }
+	    }
+    }
+
+    SDL_Quit();
+    return 0;
+}
+

src/sdl.cpp

@@ -1,417 +0,0 @@
-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

@@ -1,233 +0,0 @@
-#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;
-}
-