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

Add robot gimbal position push topic

Browse Source
This commit is contained in:
pgsocks
2024-04-25 10:30:04 -05:00
parent 23a07f99d9
commit 11c3ac43fb
2 changed files with 36 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/robo.c
+26 -4
View File
@@ -30,6 +30,11 @@ struct RobotImp {
float x, y, z;
uint16_t yaw_ground;
uint16_t pitch_ground;
uint16_t yaw;
uint16_t pitch;
enum {
CONNECTING_TO_PROXY_PORT,
SETTING_SDK_CONNECTION,
@@ -39,6 +44,7 @@ struct RobotImp {
SUSBCRIBING_SUBNODE,
SETTING_MOVEMENT_MODE,
SUBSCRIBING_POSITION_STATUS,
SUBSCRIBING_GIMBAL_STATUS,
SENDING_HEARTBEAT,
WAITING,
READY,
@@ -177,6 +183,14 @@ robot_work(Robot robot) {
{
add_sub_msg ( &req, robot->seq++, true, 20, 1, DDS_POSITION );
req_send(robot->client->dev_conn, &req);
robot->state = SUBSCRIBING_GIMBAL_STATUS;
break;
}
case SUBSCRIBING_GIMBAL_STATUS:
{
add_sub_msg ( &req, robot->seq++, true, 21, 1, DDS_GIMBAL_POS );
req_send(robot->client->dev_conn, &req);
robot->state = WAITING;
break;
}
@@ -309,10 +323,18 @@ 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;
switch(resp.resp.push.msg_id) {
case 20:
robot->x = resp.resp.push.position.x;
robot->y = resp.resp.push.position.y;
robot->z = resp.resp.push.position.z;
break;
case 21:
robot->yaw_ground = resp.resp.push.gimbal.yaw_ground;
robot->pitch_ground = resp.resp.push.gimbal.pitch_ground;
robot->yaw = resp.resp.push.gimbal.yaw;
robot->pitch = resp.resp.push.gimbal.pitch;
}
break;
}