Remove pause for enabling image recognition

.github/issue_template

@@ -1,59 +0,0 @@
-name: Bug Report
-about: File a bug report
-title: "[Bug]: "
-body:
-  - type: markdown
-    attributes:
-      value: |
-        Thanks for taking the time to fill out this bug report!        
-  - type: input
-    id: contact
-    attributes:
-      label: Contact Details
-      description: How can we get in touch with you if we need more info?
-      placeholder: ex. email@example.com
-    validations:
-      required: false
-  - type: textarea
-    id: what-happened
-    attributes:
-      label: What happened?
-      description: Also tell us, what did you expect to happen?
-      placeholder: Tell us what you see!
-      value: "A bug happened!"
-    validations:
-      required: true
-  - type: dropdown
-    id: version
-    attributes:
-      label: Version
-      description: What version of our software are you running?
-      options:
-        - 1.0.2 (Default)
-        - 1.0.3 (Edge)
-    validations:
-      required: true
-  - type: dropdown
-    id: browsers
-    attributes:
-      label: What browsers are you seeing the problem on?
-      multiple: true
-      options:
-        - Firefox
-        - Chrome
-        - Safari
-        - Microsoft Edge
-  - type: textarea
-    id: logs
-    attributes:
-      label: Relevant log output
-      description: Please copy and paste any relevant log output. This will be automatically formatted into code, so no need for backticks.
-      render: shell
-  - type: checkboxes
-    id: terms
-    attributes:
-      label: Code of Conduct
-      description: By submitting this issue, you agree to follow our [Code of Conduct](https://example.com)
-      options:
-        - label: I agree to follow this project's Code of Conduct
-          required: true

CMakeLists.txt

@@ -16,6 +16,8 @@ add_library(robomaster
     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
@@ -28,14 +30,19 @@ target_include_directories(robomaster
     include
 )
 
-find_package(SDL2)
+include(FetchContent)
+FetchContent_Declare (
+    SDL2
+    GIT_REPOSITORY https://github.com/libsdl-org/SDL
+    GIT_TAG release-2.0.20
+    GIT_SHALLOW TRUE
+)
+FetchContent_MakeAvailable(SDL2)
 
-if(SDL2_FOUND)
-    add_executable(robomasterapp
-        src/sdl.c
-    )
-    target_link_libraries(robomasterapp
-        robomaster
-	SDL2::SDL2-static
-    )
-endif()
+add_executable(robomasterapp
+    src/sdl.c
+)
+target_link_libraries(robomasterapp
+    robomaster
+    SDL2-static
+)

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/message.h

@@ -248,6 +248,87 @@ 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 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;
@@ -261,6 +342,9 @@ union Request {
     struct SubNodeResetReq subnodereset;
     struct SubscribeAddNodeReq subnodeadd;
     struct GimbalCtrlSpeedReq gimbspeed;
+    struct BlasterFireReq blaster;
+    struct StreamCtrlReq stream;
+    struct VisionDetectEnableReq enablevision;
 };
 union Response {
     struct Header header;
@@ -275,11 +359,18 @@ union Response {
     struct SubNodeResetResp subnodereset;
     struct SubscribeAddNodeResp subnodeadd;
     struct GimbalCtrlSpeedResp gimbspeed;
+    struct BlasterFireResp blaster;
+    struct StreamCtrlResp stream;
+    struct VisionDetectEnableResp enablevision;
+};
+union Push {
+    struct VisionDetectInfo vision;
 };
 union Message {
     struct Header header;
     union Request req;
     union Response resp;
+    union Push push;
 };
 
 enum MESSAGEERR {

include/roboeasy.h

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

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

src/connection.c

@@ -37,6 +37,12 @@ message_length(int 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);
         default:
             return 0;
     }
@@ -60,6 +66,12 @@ message_module(int cmd) {
             return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
         case GIMBAL_CTRL_SPEED_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;
     }

src/message.c

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

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

@@ -5,8 +5,20 @@
 #include <string.h>
 #include <stdlib.h>
 
+#include <stdio.h>
+
+struct FragmentLink {
+    struct Fragment fragment;
+    struct FragmentLink *next;
+};
+
 struct RobotImp {
 
+    struct {
+        struct FragmentLink *head;
+        struct FragmentLink *free;
+    } frag_queue;
+
     struct Client* client;
     uint16_t seq;
 
@@ -19,6 +31,11 @@ struct RobotImp {
     int16_t gimbal[2];
     bool dirty_gimbal;
 
+    bool dirty_blaster;
+
+    bool stream_state;
+    bool dirty_stream;
+
     bool sdk_mode;
 
     enum {
@@ -37,6 +54,19 @@ 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
@@ -78,6 +108,61 @@ robot_heartbeat(Robot robot) {
 
 }
 
+inline
+static
+void
+enqueue_fragment (
+        Robot robot,
+        const struct Fragment fragment ) {
+
+    struct FragmentLink *link;
+    if(robot->frag_queue.free) {
+        link = robot->frag_queue.free;
+        robot->frag_queue.free = link->next;
+    } else {
+        link = malloc(sizeof(struct FragmentLink));
+        link->next = NULL;
+    }
+
+    link->fragment = fragment;
+
+    if(robot->frag_queue.head) {
+        robot->frag_queue.head->next = link;
+    } else {
+        robot->frag_queue.head = link;
+    }
+
+}
+
+inline
+static
+void
+dequeue_fragment (
+        Robot robot,
+        struct Fragment *fragment ) {
+
+    fragment->type = 0;
+    if(!robot->frag_queue.head)
+        return;
+
+    struct FragmentLink *link = robot->frag_queue.head;
+    *fragment = link->fragment;
+
+    robot->frag_queue.head = link->next;
+    link->next = NULL;
+    if(robot->frag_queue.free) {
+        robot->frag_queue.free->next = link;
+    } else {
+        robot->frag_queue.free = link;
+    }
+
+}
+
+int robot_poll(Robot robot, struct Fragment* fragment) {
+    dequeue_fragment(robot, fragment);
+    return fragment->type;
+}
+
 int
 robot_work(Robot robot) {
 
@@ -185,6 +270,32 @@ 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;
         }
 
@@ -195,9 +306,38 @@ 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) {

src/sdl.c

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