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

Compare commits

base: pgsocks:c0f2b11db525fe7d7781b0cd11ea1bb4ef743ba0
Branches Tags
pgsocks:gimbal
pgsocks:windowz
pgsocks:addsubpushperiod
pgsocks:master
pgsocks:opencv
pgsocks:template
pgsocks:refactor
..
compare: pgsocks:9de914dfa8781dcaf1f80193898793827c39d1cf
Branches Tags
pgsocks:gimbal
pgsocks:windowz
pgsocks:addsubpushperiod
pgsocks:master
pgsocks:opencv
pgsocks:template
pgsocks:refactor

No commits in common. 'c0f2b11db525fe7d7781b0cd11ea1bb4ef743ba0' and '9de914dfa8781dcaf1f80193898793827c39d1cf' have entirely different histories.
c0f2b11db5 ... 9de914dfa8

6 changed files with 23 additions and 194 deletions
Show Stats

49
include/message.h
Unescape Escape View File

@ -280,52 +280,6 @@ struct PACKED StreamCtrlResp {
struct Footer footer; struct Footer footer;
}; };
struct PACKED AddSubMsgReq {
struct Header header;
uint8_t node_id;
uint8_t msg_id;
uint8_t reserved;
uint8_t sub_mode;
uint8_t subject_count;
uint64_t subject_uuid;
uint16_t freq;
struct Footer footer;
};
struct PACKED AddSubMsgResp {
struct Header header;
uint8_t retcode;
uint8_t pub_node_id;
uint8_t ack_sub_mode;
uint8_t ack_msg_id;
uint16_t ack_err_uid_data;
struct Footer footer;
};
struct PACKED PushPeriodMsg {
struct Header header;
uint8_t sub_mode;
uint8_t msg_id;
union {
struct {
float x;
float y;
float z;
} position;
struct {
uint16_t yaw_ground;
uint16_t pitch_ground;
uint16_t yaw;
uint16_t pitch;
struct {
uint8_t option_mode : 2;
uint8_t return_center : 1;
};
} gimbal;
};
struct Footer footer;
};
union Request { union Request {
struct Header header; struct Header header;
struct SetSdkConnectionReq sdkconn; struct SetSdkConnectionReq sdkconn;
@ -341,7 +295,6 @@ union Request {
struct GimbalCtrlSpeedReq gimbspeed; struct GimbalCtrlSpeedReq gimbspeed;
struct BlasterFireReq blaster; struct BlasterFireReq blaster;
struct StreamCtrlReq stream; struct StreamCtrlReq stream;
struct AddSubMsgReq sub;
}; };
union Response { union Response {
struct Header header; struct Header header;
@ -358,8 +311,6 @@ union Response {
struct GimbalCtrlSpeedResp gimbspeed; struct GimbalCtrlSpeedResp gimbspeed;
struct BlasterFireResp blaster; struct BlasterFireResp blaster;
struct StreamCtrlResp stream; struct StreamCtrlResp stream;
struct AddSubMsgResp sub;
struct PushPeriodMsg push;
}; };
union Message { union Message {
struct Header header; struct Header header;

31
include/sdk.h
Unescape Escape View File

@ -108,34 +108,3 @@ subscribe_add_node (
uint16_t seq, uint16_t seq,
bool ack ); bool ack );
#define ADD_SUB_MSG_CMD 0x0348
#define PUSH_PERIOD_MSG_CMD 0x0848
#define DDS_BATTERY 0x000200096862229f
#define DDS_GIMBAL_BASE 0x00020009f5882874
#define DDS_VELOCITY 0x0002000949a4009c
#define DDS_ESC 0x00020009c14cb7c5
#define DDS_ATTITUDE 0x000200096b986306
#define DDS_IMU 0x00020009a7985b8d
#define DDS_POSITION 0x00020009eeb7cece
#define DDS_SA_STATUS 0x000200094a2c6d55
#define DDS_CHASSIS_MODE 0x000200094fcb1146
#define DDS_SBUS 0x0002000988223568
#define DDS_SERVO 0x000200095f0059e7
#define DDS_ARM 0x0002000926abd64d
#define DDS_GRIPPER 0x00020009124d156a
#define DDS_GIMBAL_POS 0x00020009f79b3c97
#define DDS_STICK 0x0002000955e9a0fa
#define DDS_MOVE_MODE 0x00020009784c7bfd
#define DDS_TOF 0x0002000986e4c05a
#define DDS_PINBOARD 0x00020009eebb9ffc
void
add_sub_msg (
union Request* req,
uint16_t seq,
bool ack,
uint8_t msg_id,
uint8_t freq,
uint64_t subject_uuid );

47
src/connection.c
Unescape Escape View File

@ -7,6 +7,7 @@
#include <sys/socket.h> #include <sys/socket.h>
#include <sys/select.h> #include <sys/select.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h> #include <string.h>
#include <fcntl.h> #include <fcntl.h>
#include <stdio.h> #include <stdio.h>
@ -40,8 +41,6 @@ message_length(int cmd) {
return sizeof(struct BlasterFireReq); return sizeof(struct BlasterFireReq);
case STREAM_CTRL_CMD: case STREAM_CTRL_CMD:
return sizeof(struct StreamCtrlReq); return sizeof(struct StreamCtrlReq);
case ADD_SUB_MSG_CMD:
return sizeof(struct AddSubMsgReq);
default: default:
return 0; return 0;
} }
@ -59,7 +58,6 @@ message_module(int cmd) {
case SET_ROBOT_MODE_CMD: case SET_ROBOT_MODE_CMD:
case SUBNODE_RESET_CMD: case SUBNODE_RESET_CMD:
case SUBSCRIBE_ADD_NODE_CMD: case SUBSCRIBE_ADD_NODE_CMD:
case ADD_SUB_MSG_CMD:
return host2byte(SDK_HOST, SDK_INDEX); return host2byte(SDK_HOST, SDK_INDEX);
case SET_WHEEL_SPEED_CMD: case SET_WHEEL_SPEED_CMD:
case CHASSIS_SPEED_MODE_CMD: case CHASSIS_SPEED_MODE_CMD:
@ -81,49 +79,44 @@ int max_fd = -1;
// TODO: Close the socket and return NULL on error // TODO: Close the socket and return NULL on error
struct Connection* struct Connection*
connection_new(uint16_t source_port, uint32_t source_ip, uint16_t dest_port, uint32_t dest_ip) connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip)
{ {
struct Connection* conn = malloc(sizeof(struct Connection)); struct Connection* conn = malloc(sizeof(struct Connection));
memset(conn, 0, sizeof(struct Connection)); memset(conn, 0, sizeof(struct Connection));
// Request a UDP socket
conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
// Set the source address and port if they are provided // Set the source address and port if they are provided
if(source_port && source_ip) { if(source_port && source_ip) {
conn->local_addr.sin_family = AF_INET; struct sockaddr_in loc_addr;
conn->local_addr.sin_port = htons(source_port); loc_addr.sin_family = AF_INET;
conn->local_addr.sin_addr.s_addr = source_ip; loc_addr.sin_port = htons(source_port);
loc_addr.sin_addr.s_addr = inet_addr(source_ip);
if(bind(conn->sockfd, (struct sockaddr*)&loc_addr, sizeof(loc_addr)) < 0)
{
perror("unable to bind local port");
exit(EXIT_FAILURE);
}
} }
// Make the socket non-blocking
int flags = fcntl(conn->sockfd, F_GETFL);
fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK);
// Set the address of the drone // Set the address of the drone
memset(&conn->remote_addr, 0, sizeof(conn->remote_addr)); memset(&conn->remote_addr, 0, sizeof(conn->remote_addr));
conn->addrlen = sizeof(conn->remote_addr); conn->addrlen = sizeof(conn->remote_addr);
conn->remote_addr.sin_family = AF_INET; conn->remote_addr.sin_family = AF_INET;
conn->remote_addr.sin_port = htons(dest_port); conn->remote_addr.sin_port = htons(dest_port);
conn->remote_addr.sin_addr.s_addr = dest_ip; conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip);
return conn;
}
enum connection_error
connection_connect(struct Connection* conn) {
// Request a UDP socket
conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if(conn->local_addr.sin_addr.s_addr
&& bind(conn->sockfd, (struct sockaddr*)&conn->local_addr, sizeof(conn->local_addr)) < 0)
return CONNECTION_LOCAL_BIND;
// Make the socket non-blocking
int flags = fcntl(conn->sockfd, F_GETFL);
fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK);
// File descriptors are numbers that count up sequentially, // File descriptors are numbers that count up sequentially,
// so save the last one as the greatest file descriptor. // so save the last one as the greatest file descriptor.
// This is needed for polling the sockets later. // This is needed for polling the sockets later.
max_fd = conn->sockfd; max_fd = conn->sockfd;
return CONNECTION_NO_ERROR; return conn;
} }
struct Connection* struct Connection*

13
src/connection.h
Unescape Escape View File

@ -6,25 +6,14 @@
#include <stddef.h> #include <stddef.h>
#include <netinet/in.h> #include <netinet/in.h>
#define ROBOT_DEFAULT_WIFI_ADDR 0X102A8C0
struct Connection { struct Connection {
int sockfd; int sockfd;
socklen_t addrlen; socklen_t addrlen;
struct sockaddr_in remote_addr; struct sockaddr_in remote_addr;
struct sockaddr_in local_addr;
}; };
struct Connection* struct Connection*
connection_new(uint16_t source_port, uint32_t source_ip, uint16_t dest_port, uint32_t dest_ip); connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip);
enum connection_error {
CONNECTION_NO_ERROR,
CONNECTION_LOCAL_BIND
};
enum connection_error
connection_connect();
struct Connection* struct Connection*
connection_poll_ready(struct Client* client); connection_poll_ready(struct Client* client);

21
src/modules/sdk.c
Unescape Escape View File

@ -96,24 +96,3 @@ set_system_led (
req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req); req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req);
} }
void
add_sub_msg (
union Request* req,
uint16_t seq,
bool ack,
uint8_t msg_id,
uint8_t freq,
uint64_t subject_uuid ) {
req->sub.node_id = host2byte(CLIENT_HOST, CLIENT_INDEX);
req->sub.msg_id = msg_id;
req->sub.reserved = 0; // NOTE: Always 0
req->sub.sub_mode = 0; // NOTE: Always 0
req->sub.subject_count = 1; // TODO: Add support for multiple
req->sub.subject_uuid = subject_uuid; // TODO: Add support for multiple
req->sub.freq = freq;
req_finalize(seq, ADD_SUB_MSG_CMD, ack, req);
}

56
src/robo.c
Unescape Escape View File

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

Write Preview
Loading…
Cancel
Save