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

Add robot position push topic

Browse Source
This commit is contained in:
pgsocks
2024-04-25 09:52:58 -05:00
parent 19861ca234
commit 23a07f99d9
3 changed files with 32 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/robo.c
+15 -9
View File
@@ -28,6 +28,8 @@ struct RobotImp {
bool sdk_mode;
float x, y, z;
enum {
CONNECTING_TO_PROXY_PORT,
SETTING_SDK_CONNECTION,
@@ -36,7 +38,7 @@ struct RobotImp {
RESETTING_SUBNODE,
SUSBCRIBING_SUBNODE,
SETTING_MOVEMENT_MODE,
SUBSCRIBING_GIMBAL_STATUS,
SUBSCRIBING_POSITION_STATUS,
SENDING_HEARTBEAT,
WAITING,
READY,
@@ -171,10 +173,10 @@ robot_work(Robot robot) {
break;
}
case SUBSCRIBING_GIMBAL_STATUS:
case SUBSCRIBING_POSITION_STATUS:
{
add_sub_msg ( &req, robot->seq++, true, 21, 1, DDS_GIMBAL_BASE );
req_send(robot->client->sdk_conn, &req);
add_sub_msg ( &req, robot->seq++, true, 20, 1, DDS_POSITION );
req_send(robot->client->dev_conn, &req);
robot->state = WAITING;
break;
}
@@ -253,6 +255,8 @@ robot_work(Robot robot) {
switch(resp.header.cmd) {
case 0:
break;
default:
break;
case SET_SDK_CONNECTION_CMD:
// TODO: Do more with this
if(resp.resp.sdkconn.retcode) {
@@ -288,17 +292,13 @@ robot_work(Robot robot) {
robot->state = STOPPED;
break;
}
robot->state = SUBSCRIBING_GIMBAL_STATUS;
robot->state = SUBSCRIBING_POSITION_STATUS;
break;
case ADD_SUB_MSG_CMD:
if(resp.resp.sub.retcode) {
robot->state = STOPPED;
break;
}
if(resp.resp.sub.ack_node_id != host2byte(CLIENT_HOST, CLIENT_INDEX)) {
robot->state = STOPPED;
break;
}
robot->state = SENDING_HEARTBEAT;
break;
case SDK_HEARTBEAT_CMD:
@@ -308,6 +308,12 @@ robot_work(Robot robot) {
}
robot->state = READY;
break;
case PUSH_PERIOD_MSG_CMD:
// assume it is a position status for now
robot->x = resp.resp.push.position.x;
robot->y = resp.resp.push.position.y;
robot->z = resp.resp.push.position.z;
break;
}
return 1;