Increment major version

The low level API needed to change to accomodate the hight level API,
which is a major revision.

CMakeLists.txt

@@ -15,12 +15,8 @@ add_library(robomaster
     src/message.c
     src/modules/sdk.c
     src/modules/chassis.c
-    src/modules/gimbal.c
-    src/modules/blaster.c
-    src/modules/camera.c
     src/connection.c
     src/robomaster.c
-    src/robo.c
 )
 
 target_include_directories(robomaster
@@ -30,19 +26,14 @@ target_include_directories(robomaster
     include
 )
 
-include(FetchContent)
+find_package(SDL2)
-FetchContent_Declare (
-    SDL2
-    GIT_REPOSITORY https://github.com/libsdl-org/SDL
-    GIT_TAG release-2.0.20
-    GIT_SHALLOW TRUE
-)
-FetchContent_MakeAvailable(SDL2)
 
-add_executable(robomasterapp
+if(SDL2_FOUND)
-    src/sdl.c
+    add_executable(robomasterapp
-)
+        src/sdl.c
-target_link_libraries(robomasterapp
+    )
-    robomaster
+    target_link_libraries(robomasterapp
-    SDL2-static
+        robomaster
-)
+	SDL2::SDL2-static
+    )
+endif()

include/blaster.h

@@ -1,33 +0,0 @@
-#pragma once
-
-#include "message.h"
-
-#include <stdbool.h>
-#include <stdint.h>
-
-static const uint8_t BLASTER_HOST = 23;
-static const uint8_t BLASTER_INDEX = 0;
-
-#define BLASTER_FIRE_CMD 0x513F
-
-enum FIRETYPE {
-    FIRETYPE_WATER = 0,
-    FIRETYPE_INFRARED = 1
-};
-
-enum FIRETIMES {
-    ONE = 1,
-    TWO = 2,
-    THREE = 3,
-    FOUR = 4,
-    FIVE = 5
-};
-
-void
-blaster_fire (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		enum FIRETYPE type,
-		enum FIRETIMES times );
-

include/camera.h

@@ -1,40 +0,0 @@
-#pragma once
-
-#include "message.h"
-
-#include <stdbool.h>
-#include <stdint.h>
-
-static const uint8_t STREAM_HOST = 1;
-static const uint8_t STREAM_INDEX = 0;
-
-#define STREAM_CTRL_CMD 0xD23F
-
-#define VIDEO_STREAM_PORT 40921
-
-enum STREAMSTATE {
-    STREAMSTATE_OFF = 0,
-    STREAMSTATE_ON = 1
-};
-
-enum STREAMRESOLUTION {
-    RES_720P = 0,
-    RES_360P = 1,
-    RES_540P = 2
-};
-
-enum STREAMCTRL {
-    STREAMCTRL_SDK = 1,
-    STREAMCTRL_VIDEO = 2,
-    STREAMCTRL_AUDIO = 3
-};
-
-void
-stream_ctrl (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		enum STREAMSTATE state,
-		enum STREAMRESOLUTION res,
-		enum STREAMCTRL ctrl );
-

include/gimbal.h

@@ -1,38 +0,0 @@
-#pragma once
-
-#include "message.h"
-
-#include <stdbool.h>
-#include <stdint.h>
-
-static const uint8_t GIMBAL_HOST = 4;
-static const uint8_t GIMBAL_INDEX = 0;
-
-#define GIMBAL_CTRL_SPEED_CMD 0x0C04
-
-void
-gimbal_ctrl_speed (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		int16_t p,
-		int16_t y,
-		int16_t r );
-
-#define GIMBAL_ROTATE_CMD 0xb03f
-void
-gimbal_rotate (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		int16_t p,
-		int16_t y );
-void
-gimbal_rotate_to (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		int16_t p,
-		int16_t y );
-
-#define GIMBAL_ACTION_PUSH_CMD 0xb13f

include/message.h

@@ -168,97 +168,6 @@ struct PACKED SetChassisWheelSpeedResp
     struct Footer footer;
 };
 
-struct PACKED GimbalCtrlSpeedReq
-{
-    struct Header header;
-    // Values between -360 and 360
-    union {
-        int16_t yrp[3];
-        struct {
-            int16_t yaw;
-            int16_t roll;
-            int16_t pitch;
-        };
-    };
-    // Always 0xDC
-    uint8_t ctrl;
-    struct Footer footer;
-};
-
-struct PACKED GimbalActionPush {
-    struct Header header;
-    uint8_t id;
-    uint8_t progress;
-    uint8_t state;
-    int16_t yaw;
-    int16_t roll;
-    int16_t pitch;
-};
-
-struct PACKED GimbalRotateReq {
-    struct Header header;
-    uint8_t id;
-
-    //union {
-        uint8_t control_bitfield;
-    //    struct {
-    //        // always 0 for start
-    //        uint8_t control : 1;
-    //        uint8_t frequency : 2;
-    //    };
-    //};
-
-    //union {
-        uint8_t coord_bitfield;
-    //    struct {
-    //        uint8_t coordinate : 3;
-    //        // always true
-    //        uint8_t pitch_valid : 1;
-    //        // always false
-    //        uint8_t roll_valid : 1;
-    //        // always true
-    //        uint8_t yaw_valid : 1;
-    //    };
-    //};
-
-    union {
-        int16_t yaw;
-        struct {
-            int8_t l_yaw;
-            int8_t h_yaw;
-        };
-    };
-    int16_t roll;
-    union {
-        int16_t pitch;
-        struct {
-            int8_t l_pitch;
-            int8_t h_pitch;
-        };
-    };
-
-    uint16_t error;
-    uint16_t yaw_speed;
-    uint16_t roll_speed;
-    uint16_t pitch_speed;
-
-    struct Footer footer;
-};
-
-struct PACKED GimbalRotateResp {
-    struct Header header;
-    uint8_t retcode;
-    uint8_t accept;
-    struct Footer footer;
-};
-
-struct PACKED GimbalCtrlSpeedResp
-{
-    struct Header header;
-    int8_t retcode;
-    struct Footer footer;
-};
-
 struct PACKED SetWheelSpeedReq
 {
     struct Header header;
@@ -315,84 +224,6 @@ struct PACKED SetSdkConnectionResp {
     struct Footer footer;
 };
 
-struct PACKED BlasterFireReq {
-    struct Header header;
-    struct {
-        uint8_t type : 4;
-        uint8_t times : 4;
-    };
-    struct Footer footer;
-};
-
-struct PACKED BlasterFireResp {
-    struct Header header;
-    uint8_t retcode;
-    struct Footer footer;
-};
-
-struct PACKED StreamCtrlReq {
-    struct Header header;
-    uint8_t ctrl;
-    struct {
-        uint8_t state : 4;
-        uint8_t conn_type : 4;
-    };
-    uint8_t resolution;
-    struct Footer footer;
-};
-
-struct PACKED StreamCtrlResp {
-    struct Header header;
-    uint8_t retcode;
-    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 {
     struct Header header;
     struct SetSdkConnectionReq sdkconn;
@@ -405,11 +236,6 @@ union Request {
     struct SetRobotModeReq mvmode;
     struct SubNodeResetReq subnodereset;
     struct SubscribeAddNodeReq subnodeadd;
-    struct GimbalCtrlSpeedReq gimbspeed;
-    struct GimbalRotateReq gimbalrot;
-    struct BlasterFireReq blaster;
-    struct StreamCtrlReq stream;
-    struct AddSubMsgReq sub;
 };
 union Response {
     struct Header header;
@@ -423,13 +249,6 @@ union Response {
     struct SetRobotModeResp mvmode;
     struct SubNodeResetResp subnodereset;
     struct SubscribeAddNodeResp subnodeadd;
-    struct GimbalCtrlSpeedResp gimbspeed;
-    struct GimbalRotateResp gimbalrot;
-    struct BlasterFireResp blaster;
-    struct StreamCtrlResp stream;
-    struct AddSubMsgResp sub;
-    struct PushPeriodMsg push;
-    struct GimbalActionPush gimbalaction;
 };
 union Message {
     struct Header header;

include/roboeasy.h

@@ -1,10 +1,8 @@
 #pragma once
 
-#include <stdbool.h>
-
 // Handle for the high level robot interface
-struct RobotImp;
+struct Robot;
-typedef struct RobotImp* Robot;
+typedef struct Robot* Robot;
 
 /*
  * Return a handle to the high level robot interface, or NULL on error.
@@ -31,20 +29,6 @@ int robot_init(Robot robot);
  */
 int robot_stop(Robot robot);
 
-/*
- * Set the gimble speed. A packet will be sent to the robot on
- * the next ready tick of the work function.
- *
- * robot: The robot to set the velocity of
- * p: pitch speed
- * y: yaw speed
- * returns: 0 on success, non-zero on failure
- */
-int robot_aim(Robot, float p, float y);
-
-int robot_target(Robot robot, float pitch, float yaw);
-int robot_target_to(Robot robot, float pitch, float yaw);
-
 /*
  * Set the velocity of the robot chassis. A packet will be sent to the robot on
  * the next ready tick of the work function.
@@ -70,10 +54,6 @@ int robot_drive(Robot, float x, float y, float r);
  */
 int robot_led(Robot, unsigned char r, unsigned char g, unsigned char b);
 
-int robot_blast(Robot);
-
-int robot_stream(Robot, bool);
-
 /*
  * If the robot is in ready mode, this makes the robot send a heartbeat in
  * the next tick of the work function. The work function will not do anything

include/robomaster.h

@@ -7,9 +7,6 @@ typedef struct Client* Client;
 #include "message.h"
 #include "chassis.h"
 #include "sdk.h"
-#include "gimbal.h"
-#include "blaster.h"
-#include "camera.h"
 
 Client client_new();
 void client_connect(Client client);

include/sdk.h

@@ -108,34 +108,3 @@ subscribe_add_node (
         uint16_t seq,
         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 );

src/connection.c

@@ -7,6 +7,7 @@
 #include <sys/socket.h>
 #include <sys/select.h>
 #include <netinet/in.h>
+#include <arpa/inet.h>
 #include <string.h>
 #include <fcntl.h>
 #include <stdio.h>
@@ -34,16 +35,6 @@ message_length(int cmd) {
             return sizeof(struct SetWheelSpeedReq);
         case CHASSIS_SPEED_MODE_CMD:
             return sizeof(struct ChassisSpeedModeReq);
-        case GIMBAL_CTRL_SPEED_CMD:
-            return sizeof(struct GimbalCtrlSpeedReq);
-        case GIMBAL_ROTATE_CMD:
-            return sizeof(struct GimbalRotateReq);
-        case BLASTER_FIRE_CMD:
-            return sizeof(struct BlasterFireReq);
-        case STREAM_CTRL_CMD:
-            return sizeof(struct StreamCtrlReq);
-        case ADD_SUB_MSG_CMD:
-            return sizeof(struct AddSubMsgReq);
         default:
             return 0;
     }
@@ -61,18 +52,10 @@ message_module(int cmd) {
         case SET_ROBOT_MODE_CMD:
         case SUBNODE_RESET_CMD:
         case SUBSCRIBE_ADD_NODE_CMD:
-        case ADD_SUB_MSG_CMD:
             return host2byte(SDK_HOST, SDK_INDEX);
         case SET_WHEEL_SPEED_CMD:
         case CHASSIS_SPEED_MODE_CMD:
             return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
-        case GIMBAL_CTRL_SPEED_CMD:
-        case GIMBAL_ROTATE_CMD:
-            return host2byte(GIMBAL_HOST, GIMBAL_INDEX);
-        case BLASTER_FIRE_CMD:
-            return host2byte(BLASTER_HOST, BLASTER_INDEX);
-        case STREAM_CTRL_CMD:
-            return host2byte(STREAM_HOST, STREAM_INDEX);
         default:
             return 0;
     }
@@ -84,49 +67,44 @@ int max_fd = -1;
 
 // TODO: Close the socket and return NULL on error
 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));
     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
     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
     memset(&conn->remote_addr, 0, sizeof(conn->remote_addr));
     conn->addrlen = sizeof(conn->remote_addr);
     conn->remote_addr.sin_family = AF_INET;
     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,
     // so save the last one as the greatest file descriptor.
     // This is needed for polling the sockets later.
     max_fd = conn->sockfd;
 
-    return CONNECTION_NO_ERROR;
+    return conn;
-
 }
 
 struct Connection*

src/connection.h

@@ -6,25 +6,14 @@
 #include <stddef.h>
 #include <netinet/in.h>
 
-#define ROBOT_DEFAULT_WIFI_ADDR 0X102A8C0
-
 struct Connection {
     int sockfd;
     socklen_t addrlen;
     struct sockaddr_in remote_addr;
-    struct sockaddr_in local_addr;
 };
 
 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*
 connection_poll_ready(struct Client* client);

src/modules/blaster.c

@@ -1,16 +0,0 @@
-#include "message.h"
-#include "connection.h"
-#include "robomaster.h"
-
-void
-blaster_fire (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		enum FIRETYPE type,
-		enum FIRETIMES times ) {
-    req->blaster.type = type;
-    req->blaster.times = times;
-    req_finalize(seq, BLASTER_FIRE_CMD, ack, req);
-}
-

src/modules/camera.c

@@ -1,20 +0,0 @@
-#include "message.h"
-#include "connection.h"
-#include "robomaster.h"
-
-void
-stream_ctrl (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		enum STREAMSTATE state,
-		enum STREAMRESOLUTION res,
-		enum STREAMCTRL ctrl ) {
-    req->stream.state = state;
-    req->stream.resolution = res;
-    req->stream.ctrl = ctrl;
-    req->stream.conn_type = 0; // Hardcode to WiFi
-    req_finalize(seq, STREAM_CTRL_CMD, ack, req);
-}
-
-

src/modules/chassis.c

@@ -15,7 +15,7 @@ set_wheel_speed (
     req->wheel.wheel_speed[1] = w2;
     req->wheel.wheel_speed[2] = w3;
     req->wheel.wheel_speed[3] = w4;
-    req_finalize(seq, SET_WHEEL_SPEED_CMD, ack, req);
+    req_finalize(seq, SET_WHEEL_SPEED_CMD, false, req);
 }
 
 void
@@ -29,5 +29,5 @@ chassis_speed_mode (
     req->chsspeed.speed[0] = x;
     req->chsspeed.speed[1] = y;
     req->chsspeed.speed[2] = z;
-    req_finalize(seq, CHASSIS_SPEED_MODE_CMD, ack, req);
+    req_finalize(seq, CHASSIS_SPEED_MODE_CMD, false, req);
 }

src/modules/gimbal.c

@@ -1,82 +0,0 @@
-#include "message.h"
-#include "connection.h"
-#include "robomaster.h"
-
-void
-gimbal_ctrl_speed (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		int16_t p,
-		int16_t y,
-		int16_t r ) {
-    req->gimbspeed.yaw = y;
-    req->gimbspeed.roll = r;
-    req->gimbspeed.pitch = p;
-    req->gimbspeed.ctrl = 0xDC;
-    req_finalize(seq, GIMBAL_CTRL_SPEED_CMD, ack, req);
-}
-
-void
-gimbal_rotate (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		int16_t p,
-		int16_t y ) {
-    //req->gimbalrot.coordinate = 1;
-    //req->gimbalrot.yaw_valid = 1;
-    //req->gimbalrot.pitch_valid = 1;
-    //req->gimbalrot.roll_valid = 0;
-    req->gimbalrot.coord_bitfield = 0;
-    req->gimbalrot.coord_bitfield = 1 | 0 << 1 | 1 << 2 | 1 << 3;
-
-    //req->gimbalrot.frequency = 2;
-    //req->gimbalrot.control = 0;
-    req->gimbalrot.control_bitfield = 0;
-    req->gimbalrot.control_bitfield = 0 | 1 << 2;
-
-    req->gimbalrot.id = 33;
-    req->gimbalrot.yaw = 0;
-    req->gimbalrot.yaw = y;// >> 8;//(y >> 8 | y << 8);
-    req->gimbalrot.roll = 0;
-    req->gimbalrot.pitch = 0;
-    req->gimbalrot.pitch = p;// >> 8;//(p >> 8 | y << 8);
-    req->gimbalrot.error = 0;
-    req->gimbalrot.yaw_speed = 30;
-    req->gimbalrot.roll_speed = 0;
-    req->gimbalrot.pitch_speed = 30;
-    req_finalize(seq, GIMBAL_ROTATE_CMD, ack, req);
-}
-
-void
-gimbal_rotate_to (
-		union Request* req,
-        uint16_t seq,
-        bool ack,
-		int16_t p,
-		int16_t y ) {
-    //req->gimbalrot.coordinate = 1;
-    //req->gimbalrot.yaw_valid = 1;
-    //req->gimbalrot.pitch_valid = 1;
-    //req->gimbalrot.roll_valid = 0;
-    req->gimbalrot.coord_bitfield = 0;
-    req->gimbalrot.coord_bitfield = 1 | 0 << 1 | 1 << 2 | 4 << 3;
-
-    //req->gimbalrot.frequency = 2;
-    //req->gimbalrot.control = 0;
-    req->gimbalrot.control_bitfield = 0;
-    req->gimbalrot.control_bitfield = 0 | 1 << 2;
-
-    req->gimbalrot.id = 33;
-    req->gimbalrot.yaw = 0;
-    req->gimbalrot.yaw = y;// >> 8;//(y >> 8 | y << 8);
-    req->gimbalrot.roll = 0;
-    req->gimbalrot.pitch = 0;
-    req->gimbalrot.pitch = p;// >> 8;//(p >> 8 | y << 8);
-    req->gimbalrot.error = 0;
-    req->gimbalrot.yaw_speed = 30;
-    req->gimbalrot.roll_speed = 0;
-    req->gimbalrot.pitch_speed = 30;
-    req_finalize(seq, GIMBAL_ROTATE_CMD, ack, req);
-}

src/modules/sdk.c

@@ -17,7 +17,7 @@ set_sdk_connection(
     req->sdkconn.protocol = 0;
     req->sdkconn.ip_address = ip_address;
     req->sdkconn.port = port;
-    req_finalize(seq, SET_SDK_CONNECTION_CMD, ack, req);
+    req_finalize(seq, SET_SDK_CONNECTION_CMD, true, req);
 }
 
 void
@@ -27,7 +27,7 @@ set_sdk_mode(
         bool ack,
 		bool enable ) {
     req->sdkmode.enable = enable;
-    req_finalize(seq, SET_SDK_MODE_CMD, ack, req);
+    req_finalize(seq, SET_SDK_MODE_CMD, true, req);
 }
 
 void
@@ -35,7 +35,7 @@ sdk_heartbeat(
 		union Request* req,
         uint16_t seq,
         bool ack ) {
-    req_finalize(seq, SDK_HEARTBEAT_CMD, ack, req);
+    req_finalize(seq, SDK_HEARTBEAT_CMD, false, req);
 }
 
 void
@@ -45,7 +45,7 @@ set_robot_mode (
         bool ack,
 		enum MOVEMENTMODE mode ) {
     req->mvmode.mode = mode;
-    req_finalize(seq, SET_ROBOT_MODE_CMD, ack, req);
+    req_finalize(seq, SET_ROBOT_MODE_CMD, true, req);
 }
 
 void
@@ -54,7 +54,7 @@ subnode_reset (
         uint16_t seq,
         bool ack ) {
     req->subnodereset.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
-    req_finalize(seq, SUBNODE_RESET_CMD, ack, req);
+    req_finalize(seq, SUBNODE_RESET_CMD, true, req);
 }
 
 void
@@ -64,7 +64,7 @@ subscribe_add_node (
         bool ack ) {
     req->subnodeadd.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
     req->subnodeadd.sub_vision = 0x03000000;
-    req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, ack, req);
+    req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, true, req);
 }
 
 void
@@ -93,27 +93,6 @@ set_system_led (
     req->led.t1 = t1;
     req->led.t2 = t2;
 
-    req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req);
+    req_finalize(seq, SET_SYSTEM_LED_CMD, true, 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);
 
 }

src/robo.c

@@ -4,12 +4,8 @@
 
 #include <string.h>
 #include <stdlib.h>
-#include <arpa/inet.h>
-#include <math.h>
 
-#include <stdio.h>
+struct Robot {
-
-struct RobotImp {
 
     struct Client* client;
     uint16_t seq;
@@ -20,28 +16,8 @@ struct RobotImp {
     uint8_t colors[3];
     bool dirty_colors;
 
-    int16_t gimbal[2];
-    bool dirty_gimbal;
-
-    bool dirty_blaster;
-
-    bool stream_state;
-    bool dirty_stream;
-
     bool sdk_mode;
 
-    float x, y, z;
-
-    uint16_t yaw_ground;
-    uint16_t pitch_ground;
-    uint16_t yaw;
-    uint16_t pitch;
-    enum {
-        TARGET_NONE,
-        TARGET_REL,
-        TARGET_ABS
-    } dirty_target;
-
     enum {
         CONNECTING_TO_PROXY_PORT,
         SETTING_SDK_CONNECTION,
@@ -50,8 +26,6 @@ struct RobotImp {
         RESETTING_SUBNODE,
         SUSBCRIBING_SUBNODE,
         SETTING_MOVEMENT_MODE,
-        SUBSCRIBING_POSITION_STATUS,
-        SUBSCRIBING_GIMBAL_STATUS,
         SENDING_HEARTBEAT,
         WAITING,
         READY,
@@ -60,19 +34,6 @@ struct RobotImp {
 
 };
 
-int
-robot_stream(Robot robot, bool state) {
-    robot->stream_state = state;
-    robot->dirty_stream = true;
-    return 1;
-}
-
-int
-robot_blast(Robot robot) {
-    robot->dirty_blaster = true;
-    return 1;
-}
-
 int
 robot_drive(Robot robot, float x, float y, float r) {
     // TODO: move individual wheel speed calculation to work function
@@ -85,52 +46,6 @@ robot_drive(Robot robot, float x, float y, float r) {
     return 1;
 }
 
-int
-robot_aim(Robot robot, float p, float y) {
-    robot->gimbal[0] = p * 360;
-    robot->gimbal[1] = y * 360;
-    robot->dirty_gimbal = true;
-    return 1;
-}
-
-int
-robot_target(Robot robot, float pitch, float yaw) {
-    // Get the FOV angle of the point in radians
-    float FOV = 120 * (M_PI / 180);
-    yaw = yaw * (FOV / 2);
-    pitch = pitch * (FOV / 2);
-
-    // Convert to degrees
-    yaw = yaw * (180 / M_PI);
-    pitch = pitch * (180 / M_PI);
-
-    // Convert for Robomaster
-    robot->yaw = yaw * 10;
-    robot->pitch = -pitch * 10;
-
-    robot->dirty_target = TARGET_REL;
-    return 1;
-};
-
-int
-robot_target_to(Robot robot, float pitch, float yaw) {
-    // Get the FOV angle of the point in radians
-    float FOV = 120 * (M_PI / 180);
-    yaw = yaw * (FOV / 2);
-    pitch = pitch * (FOV / 2);
-
-    // Convert to degrees
-    yaw = yaw * (180 / M_PI);
-    pitch = pitch * (180 / M_PI);
-
-    // Convert for Robomaster
-    robot->yaw = yaw * 10;
-    robot->pitch = -pitch * 10;
-
-    robot->dirty_target = TARGET_ABS;
-    return 1;
-};
-
 int
 robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
     robot->colors[0] = r;
@@ -164,8 +79,7 @@ robot_work(Robot robot) {
 
         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;
             break;
         }
@@ -180,8 +94,7 @@ robot_work(Robot robot) {
 
         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;
             break;
         }
@@ -226,22 +139,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) {
@@ -254,29 +151,6 @@ robot_work(Robot robot) {
                 req_send(robot->client->dev_conn, &req);
                 robot->dirty_wheels = false;
             }
-            if(robot->dirty_gimbal) {
-                gimbal_ctrl_speed (
-                        &req, robot->seq++, false,
-                        robot->gimbal[0],
-                        robot->gimbal[1],
-                        0 );
-                req_send(robot->client->dev_conn, &req);
-                robot->dirty_gimbal = false;
-            }
-            if(robot->dirty_target) {
-                if(robot->dirty_target == TARGET_REL)
-                    gimbal_rotate (
-                            &req, robot->seq++, true,
-                            robot->pitch,
-                            robot->yaw );
-                else if(robot->dirty_target == TARGET_ABS)
-                    gimbal_rotate_to (
-                            &req, robot->seq++, true,
-                            robot->pitch,
-                            robot->yaw );
-                req_send(robot->client->dev_conn, &req);
-                robot->dirty_target = TARGET_NONE;
-            }
             if(robot->dirty_colors) {
                 set_system_led (
                         &req, robot->seq++, false,
@@ -291,32 +165,6 @@ robot_work(Robot robot) {
                 req_send(robot->client->dev_conn, &req);
                 robot->dirty_colors = false;
             }
-            if(robot->dirty_blaster) {
-                blaster_fire (
-                    &req, robot->seq++, false,
-                    FIRETYPE_INFRARED,
-                    ONE
-                );
-                req_send(robot->client->dev_conn, &req);
-                robot->dirty_blaster = false;
-            }
-            if(robot->dirty_stream) {
-                stream_ctrl (
-                    &req, robot->seq++, true,
-                    robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
-                    RES_720P,
-                    STREAMCTRL_SDK
-                );
-                req_send(robot->client->dev_conn, &req);
-                stream_ctrl (
-                    &req, robot->seq++, true,
-                    robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
-                    RES_720P,
-                    STREAMCTRL_VIDEO
-                );
-                req_send(robot->client->dev_conn, &req);
-                robot->dirty_stream = false;
-            }
             break;
         }
 
@@ -330,15 +178,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:
@@ -368,13 +213,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:
@@ -384,20 +222,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;
@@ -405,8 +229,8 @@ robot_work(Robot robot) {
 }
 
 Robot robot_new() {
-    struct RobotImp* robot = malloc(sizeof(struct RobotImp));
+    struct Robot* robot = malloc(sizeof(struct Robot));
-    memset(robot, 0, sizeof(struct RobotImp));
+    memset(robot, 0, sizeof(struct Robot));
     return robot;
 }
 

src/sdl.c

@@ -16,11 +16,9 @@ struct {
 };
 int color = 0;
 
-float pitch = 0, yaw = 0;
 float x = 0, y = 0, z = 0;
 static Uint32 drive_timer_handler(Uint32 interval, void* param) {
     robot_drive((Robot)param, x, y, z);
-    robot_aim((Robot)param, pitch, yaw);
     return 75;
 }
 static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) {
@@ -29,15 +27,10 @@ static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) {
 }
 
 int main(int argc, char* argv[]) {
-    if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) < 0) {
+    if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_GAMECONTROLLER) < 0) {
         fprintf(stderr, "%s", SDL_GetError());
         return 1;
     }
-    printf("Detected %d joysticks\n", SDL_NumJoysticks());
-    SDL_Joystick* joystick = NULL;
-    if(SDL_NumJoysticks() > 0) {
-        joystick = SDL_JoystickOpen(0);
-    }
     SDL_Window* win = SDL_CreateWindow(
 		    "Robomaster",
 		    SDL_WINDOWPOS_UNDEFINED,
@@ -51,14 +44,9 @@ int main(int argc, char* argv[]) {
 
     Robot robot = robot_new();
     robot_init(robot);
-    robot_stream(robot, true);
-
     SDL_AddTimer(75, drive_timer_handler, robot);
     SDL_AddTimer(1000, heartbeat_timer_handler, robot);
 
-    int h, w;
-    SDL_GetWindowSize(win, &w, &h);
-
     while(robot_work(robot)) {
         SDL_Event event;
         while(SDL_PollEvent(&event)) {
@@ -91,39 +79,8 @@ int main(int argc, char* argv[]) {
                         case SDL_SCANCODE_SPACE:
                             robot_led(robot, colors[color].r, colors[color].g, colors[color].b);
                             color = (color + 1) % 3;
-                            break;
-                        case SDL_SCANCODE_RETURN:
-                            robot_blast(robot);
-                            break;
                         default: break;
 		            }
-                    break;
-                case SDL_MOUSEMOTION:
-                    yaw = (float)event.motion.xrel;
-                    pitch = (float)event.motion.yrel;
-                    break;
-                case SDL_JOYAXISMOTION:
-                    switch(event.jaxis.axis) {
-                        case 0:
-                            x = (float)event.jaxis.value / 32767;
-                            break;
-                        case 1:
-                            y = (float)event.jaxis.value / 32767;
-                            break;
-                        case 4:
-                            z = (float)event.jaxis.value / 32767 / 2;
-                            break;
-                        case 2:
-                            yaw = (float)event.jaxis.value / 32767;
-                            break;
-                        case 3:
-                            pitch = (float)event.jaxis.value / 32767;
-                            break;
-                        default:
-                            printf("axis: %d\n", event.jaxis.axis);
-                            break;
-                    }
-                    break;
                 case SDL_WINDOWEVENT:
                     if(event.window.event != SDL_WINDOWEVENT_CLOSE) break;
                 case SDL_QUIT:
@@ -133,7 +90,6 @@ int main(int argc, char* argv[]) {
 	    }
     }
 
-    SDL_JoystickClose(joystick);
     SDL_Quit();
     return 0;
 }