Add message fragment queue to easy robo API for vision info

include/camera.h

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

include/message.h

@@ -316,6 +316,19 @@ struct PACKED VisionDetectInfo {
     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;
@@ -331,6 +344,7 @@ union Request {
     struct GimbalCtrlSpeedReq gimbspeed;
     struct BlasterFireReq blaster;
     struct StreamCtrlReq stream;
+    struct VisionDetectEnableReq enablevision;
 };
 union Response {
     struct Header header;
@@ -347,6 +361,7 @@ union Response {
     struct GimbalCtrlSpeedResp gimbspeed;
     struct BlasterFireResp blaster;
     struct StreamCtrlResp stream;
+    struct VisionDetectEnableResp enablevision;
 };
 union Push {
     struct VisionDetectInfo vision;

include/roboeasy.h

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

src/connection.c

@@ -41,6 +41,8 @@ message_length(int 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;
     }
@@ -68,6 +70,8 @@ message_module(int cmd) {
             return host2byte(BLASTER_HOST, BLASTER_INDEX);
         case STREAM_CTRL_CMD:
             return host2byte(STREAM_HOST, STREAM_INDEX);
+        case VISION_DETECT_ENABLE_CMD:
+            return host2byte(VISION_HOST, VISION_INDEX);
         default:
             return 0;
     }

src/message.c

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

src/modules/camera.c

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

src/robo.c

@@ -7,8 +7,18 @@
 
 #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;
 
@@ -33,6 +43,7 @@ struct RobotImp {
         SETTING_SDK_CONNECTION,
         CONNECTING_TO_DEVICE_PORT,
         SETTING_SDK_MODE,
+        STREAMING_ENABLED,
         RESETTING_SUBNODE,
         SUSBCRIBING_SUBNODE,
         SETTING_MOVEMENT_MODE,
@@ -98,6 +109,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) {
 
@@ -170,6 +236,16 @@ robot_work(Robot robot) {
             break;
         }
 
+        case STREAMING_ENABLED:
+        {
+            vision_enable (
+                &req, robot->seq++, true
+            );
+            req_send(robot->client->dev_conn, &req);
+            robot->state = READY;
+            break;
+        }
+
         case READY:
         {
             if(robot->dirty_wheels) {
@@ -241,9 +317,42 @@ 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 STREAM_CTRL_CMD:
+            if(!resp.resp.sdkconn.retcode)
+                robot->state = STREAMING_ENABLED;
+            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"
 
@@ -60,6 +61,64 @@ int main(int argc, char* argv[]) {
     SDL_GetWindowSize(win, &w, &h);
 
     while(robot_work(robot)) {
+
+        struct Fragment fragment;
+        while(robot_poll(robot, &fragment)) {
+            switch(fragment.type) {
+                case FRAGMENT_RECTANGLE:
+                case FRAGMENT_LINE:
+                case FRAGMENT_MARKER:
+                    printf("Rect(%f,%f,%f,%f)\n",
+                            fragment.rect.rect.x,
+                            fragment.rect.rect.y,
+                            fragment.rect.rect.w,
+                            fragment.rect.rect.h );
+                    break;
+                case FRAGMENT_MESSAGE:
+                    switch(fragment.message.header.cmd) {
+                        case VISION_DETECT_INFO_CMD:
+                            if(fragment.message.push.vision.errcode) {
+                                printf("Vision error %d\n", fragment.message.push.vision.errcode);
+                                break;
+                            }
+                            printf("Received %d rectangles\n", fragment.message.push.vision.count);
+                            printf("\tVision type %d\n", fragment.message.push.vision.type);
+                            printf("\tVision status %d\n", fragment.message.push.vision.status);
+                            break;
+                        case SDK_HEARTBEAT_CMD:
+                            if(fragment.message.resp.heartbeat.retcode) {
+                                printf("ERROR: Heartbeat error\n");
+                                break;
+                            }
+                            printf("Heartbeat\n");
+                            break;
+                        case STREAM_CTRL_CMD:
+                            if(fragment.message.resp.stream.retcode) {
+                                printf("ERROR: Stream not enabled\n");
+                                break;
+                            }
+                            printf("Stream enabled\n");
+                            break;
+                        case VISION_DETECT_ENABLE_CMD:
+                            if(fragment.message.resp.enablevision.retcode) {
+                                printf("ERROR: Vision not enabled\n");
+                                break;
+                            }
+                            if(fragment.message.resp.enablevision.error) {
+                                printf("ERROR: Vision not enabled %d\n",
+                                    fragment.message.resp.enablevision.error);
+                                break;
+                            }
+                            printf("Vision enabled\n");
+                            break;
+                    }
+                    break;
+                default:
+                    printf("Unhandled fragment type\n");
+                    break;
+            }
+        }
+
         SDL_Event event;
         while(SDL_PollEvent(&event)) {
             switch(event.type) {