Add video stream support

CMakeLists.txt

@@ -17,6 +17,7 @@ add_library(robomaster
     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

include/camera.h

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

include/message.h

@@ -263,6 +263,23 @@ struct PACKED BlasterFireResp {
     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;
+};
+
 union Request {
     struct Header header;
     struct SetSdkConnectionReq sdkconn;
@@ -277,6 +294,7 @@ union Request {
     struct SubscribeAddNodeReq subnodeadd;
     struct GimbalCtrlSpeedReq gimbspeed;
     struct BlasterFireReq blaster;
+    struct StreamCtrlReq stream;
 };
 union Response {
     struct Header header;
@@ -292,6 +310,7 @@ union Response {
     struct SubscribeAddNodeResp subnodeadd;
     struct GimbalCtrlSpeedResp gimbspeed;
     struct BlasterFireResp blaster;
+    struct StreamCtrlResp stream;
 };
 union Message {
     struct Header header;

include/roboeasy.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <stdbool.h>
+
 // Handle for the high level robot interface
 struct RobotImp;
 typedef struct RobotImp* Robot;
@@ -67,6 +69,8 @@ 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

@@ -9,6 +9,7 @@ typedef struct Client* Client;
 #include "sdk.h"
 #include "gimbal.h"
 #include "blaster.h"
+#include "camera.h"
 
 Client client_new();
 void client_connect(Client client);

src/connection.c

@@ -39,6 +39,8 @@ message_length(int cmd) {
             return sizeof(struct GimbalCtrlSpeedReq);
         case BLASTER_FIRE_CMD:
             return sizeof(struct BlasterFireReq);
+        case STREAM_CTRL_CMD:
+            return sizeof(struct StreamCtrlReq);
         default:
             return 0;
     }
@@ -64,6 +66,8 @@ message_module(int 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);
         default:
             return 0;
     }

src/modules/camera.c

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

src/robo.c

@@ -23,6 +23,9 @@ struct RobotImp {
 
     bool dirty_blaster;
 
+    bool stream_state;
+    bool dirty_stream;
+
     bool sdk_mode;
 
     enum {
@@ -41,6 +44,13 @@ 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;
@@ -204,6 +214,23 @@ robot_work(Robot robot) {
                 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;
         }
 

src/sdl.c

@@ -51,6 +51,8 @@ int main(int argc, char* argv[]) {
 
     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);