|
|
|
@ -4,7 +4,6 @@
|
|
|
|
|
|
|
|
|
|
#include <string.h>
|
|
|
|
|
#include <stdlib.h>
|
|
|
|
|
#include <arpa/inet.h>
|
|
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
|
|
|
|
|
@ -29,13 +28,6 @@ struct RobotImp {
|
|
|
|
|
|
|
|
|
|
bool sdk_mode;
|
|
|
|
|
|
|
|
|
|
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,
|
|
|
|
@ -44,8 +36,6 @@ struct RobotImp {
|
|
|
|
|
RESETTING_SUBNODE,
|
|
|
|
|
SUSBCRIBING_SUBNODE,
|
|
|
|
|
SETTING_MOVEMENT_MODE,
|
|
|
|
|
SUBSCRIBING_POSITION_STATUS,
|
|
|
|
|
SUBSCRIBING_GIMBAL_STATUS,
|
|
|
|
|
SENDING_HEARTBEAT,
|
|
|
|
|
WAITING,
|
|
|
|
|
READY,
|
|
|
|
@ -120,8 +110,7 @@ robot_work(Robot robot) {
|
|
|
|
|
|
|
|
|
|
case CONNECTING_TO_PROXY_PORT:
|
|
|
|
|
{
|
|
|
|
|
robot->client->sdk_conn = connection_new(0, 0, 30030, ROBOT_DEFAULT_WIFI_ADDR);
|
|
|
|
|
connection_connect(robot->client->sdk_conn);
|
|
|
|
|
robot->client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
|
|
|
|
|
robot->state = SETTING_SDK_CONNECTION;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
@ -136,8 +125,7 @@ robot_work(Robot robot) {
|
|
|
|
|
|
|
|
|
|
case CONNECTING_TO_DEVICE_PORT:
|
|
|
|
|
{
|
|
|
|
|
if(connection_connect(robot->client->dev_conn))
|
|
|
|
|
robot->state = STOPPED;
|
|
|
|
|
robot->client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
|
|
|
|
|
robot->state = SETTING_SDK_MODE;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
@ -182,22 +170,6 @@ robot_work(Robot robot) {
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
case SUBSCRIBING_POSITION_STATUS:
|
|
|
|
|
{
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
case READY:
|
|
|
|
|
{
|
|
|
|
|
if(robot->dirty_wheels) {
|
|
|
|
@ -272,15 +244,12 @@ 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) {
|
|
|
|
|
robot->state = STOPPED;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
robot->client->dev_conn = connection_new(10010, resp.resp.sdkconn.config_ip, 20020, ROBOT_DEFAULT_WIFI_ADDR);
|
|
|
|
|
robot->state = CONNECTING_TO_DEVICE_PORT;
|
|
|
|
|
break;
|
|
|
|
|
case SET_SDK_MODE_CMD:
|
|
|
|
@ -310,13 +279,6 @@ robot_work(Robot robot) {
|
|
|
|
|
robot->state = STOPPED;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
robot->state = SUBSCRIBING_POSITION_STATUS;
|
|
|
|
|
break;
|
|
|
|
|
case ADD_SUB_MSG_CMD:
|
|
|
|
|
if(resp.resp.sub.retcode) {
|
|
|
|
|
robot->state = STOPPED;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
robot->state = SENDING_HEARTBEAT;
|
|
|
|
|
break;
|
|
|
|
|
case SDK_HEARTBEAT_CMD:
|
|
|
|
@ -326,20 +288,6 @@ robot_work(Robot robot) {
|
|
|
|
|
}
|
|
|
|
|
robot->state = READY;
|
|
|
|
|
break;
|
|
|
|
|
case PUSH_PERIOD_MSG_CMD:
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return 1;
|
|
|
|
|