pgsocks/robomaster-sdk
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add message fragment queue to easy robo API for vision info

Browse Source
This commit is contained in:
pgsocks
2023-09-05 13:43:15 -05:00
parent 103086b1a4
commit 94363b56f5
8 changed files with 242 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/camera.h
+25
View File
@@ -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
+15
View File
@@ -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
+17
View File
@@ -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
+4
View File
@@ -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
+5
View File
@@ -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
+8
View File
@@ -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
+109
View File
@@ -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
+59
View File
@@ -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) {