diff --git a/CMakeLists.txt b/CMakeLists.txt index 0f4debf..4186383 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,10 +13,8 @@ add_compile_options(-Wall) add_library(robomaster src/message.c - src/messages/sdk_connection.c - src/messages/led.c - src/messages/sdk_mode.c - src/messages/set_wheel_speed.c + src/modules/sdk.c + src/modules/chassis.c src/connection.c src/robomaster.c ) diff --git a/include/chassis.h b/include/chassis.h new file mode 100644 index 0000000..ca790d8 --- /dev/null +++ b/include/chassis.h @@ -0,0 +1,28 @@ +#pragma once + +#include "message.h" + +#include +#include + +static const uint8_t CHASSIS_HOST = 3; +static const uint8_t CHASSIS_INDEX = 6; + +static const uint8_t SET_WHEEL_SPEED_CMDID = 0x20; + +void +set_wheel_speed ( + Client session, + int16_t w1, + int16_t w2, + int16_t w3, + int16_t w4 ); + +static const uint8_t CHASSIS_SPEED_MODE_CMDID = 0x21; + +void +chassis_speed_mode ( + Client session, + float x, + float y, + float z ); diff --git a/include/led.h b/include/led.h deleted file mode 100644 index 2c13e5f..0000000 --- a/include/led.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include "message.h" - -#include -#include - -static const uint8_t SET_SYSTEM_LED_CMDID = 0x33; - -enum LEDCOMP { - LEDCOMP_BOTTOM_BACK = 0x1, - LEDCOMP_BOTTOM_FRONT = 0x2, - LEDCOMP_BOTTOM_LEFT = 0x4, - LEDCOMP_BOTTOM_RIGHT = 0x8, - LEDCOMP_BOTTOM_ALL = 0xf, - LEDCOMP_TOP_LEFT = 0x10, - LEDCOMP_TOP_RIGHT = 0x20, - LEDCOMP_TOP_ALL = 0x30, - LEDCOMP_ALL = 0x3f -}; - -enum LEDEFFECT { - LEDEFFECT_OFF = 0, - LEDEFFECT_ON = 1, - LEDEFFECT_BREATH = 2, - LEDEFFECT_FLASH = 3, - LEDEFFECT_SCROLLING = 4 -}; - -void set_system_led ( - Client session, - uint8_t red, - uint8_t green, - uint8_t blue, - enum LEDCOMP comp, - uint16_t led_mask, - enum LEDEFFECT effect, - uint16_t t1, - uint16_t t2 ); diff --git a/include/robomaster.h b/include/robomaster.h index 503ecea..a89703f 100644 --- a/include/robomaster.h +++ b/include/robomaster.h @@ -5,10 +5,8 @@ struct Client; typedef struct Client* Client; #include "message.h" -#include "led.h" -#include "sdk_connection.h" -#include "sdk_mode.h" -#include "wheel.h" +#include "chassis.h" +#include "sdk.h" // SIMPLE // Create a client @@ -20,3 +18,12 @@ Client client_new(); void client_connect(Client client); void poll_message(Client client, union Message* message); +static inline uint8_t host2byte(uint8_t host, uint8_t index) { + return index * 32 + host; +} + +static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) { + *host = (b & 0x1F); + *index = b >> 5; +} + diff --git a/include/sdk.h b/include/sdk.h new file mode 100644 index 0000000..4991037 --- /dev/null +++ b/include/sdk.h @@ -0,0 +1,96 @@ +#pragma once + +#include "message.h" + +#include +#include + +static const uint8_t SDK_HOST = 9; +static const uint8_t SDK_INDEX = 0; + +static const uint8_t SET_SDK_CONNECTION_CMDID = 0xD4; + +enum CONNECTION { + CONNECTION_WIFI_AP = 0, + CONNECTION_WIFI_STA = 1, + CONNECTION_USB_RNDIS = 2 +}; + +void +set_sdk_connection( + Client session, + enum CONNECTION connection_type, + uint32_t ip_address, + uint16_t port ); + +static const uint8_t SDK_HEARTBEAT_CMDID = 0xD5; + +void +sdk_heartbeat( + Client session ); + +static const uint8_t SET_SDK_MODE_CMDID = 0xd1; + +void +set_sdk_mode(Client session, bool enable); + +static const uint8_t SET_SYSTEM_LED_CMDID = 0x33; + +enum LEDCOMP { + LEDCOMP_BOTTOM_BACK = 0x1, + LEDCOMP_BOTTOM_FRONT = 0x2, + LEDCOMP_BOTTOM_LEFT = 0x4, + LEDCOMP_BOTTOM_RIGHT = 0x8, + LEDCOMP_BOTTOM_ALL = 0xf, + LEDCOMP_TOP_LEFT = 0x10, + LEDCOMP_TOP_RIGHT = 0x20, + LEDCOMP_TOP_ALL = 0x30, + LEDCOMP_ALL = 0x3f +}; + +enum LEDEFFECT { + LEDEFFECT_OFF = 0, + LEDEFFECT_ON = 1, + LEDEFFECT_BREATH = 2, + LEDEFFECT_FLASH = 3, + LEDEFFECT_SCROLLING = 4 +}; + +void set_system_led ( + Client session, + uint8_t red, + uint8_t green, + uint8_t blue, + enum LEDCOMP comp, + uint16_t led_mask, + enum LEDEFFECT effect, + uint16_t t1, + uint16_t t2 ); + +static const uint8_t SET_ROBOT_MODE_CMDID = 0x46; + +enum MOVEMENTMODE { + MOVEMENTMODE_FREE, + MOVEMENTMODE_GIMBAL_LEAD, + MOVEMENTMODE_CHASSIS_LEAD +}; + +void +set_robot_mode ( + Client session, + enum MOVEMENTMODE mode ); + +// cmdset 0x48 +static const uint8_t SUBNODE_RESET_CMDID = 0x02; + +void +subnode_reset ( + Client session ); + +// cmdset 0x48 +static const uint8_t SUBSCRIBE_ADD_NODE_CMDID = 0x01; + +void +subscribe_add_node ( + Client session ); + diff --git a/include/sdk_connection.h b/include/sdk_connection.h deleted file mode 100644 index 2bf95e7..0000000 --- a/include/sdk_connection.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include "message.h" - -#include -#include - -static const uint8_t SET_SDK_CONNECTION_CMDID = 0xD4; - -enum CONNECTION { - CONNECTION_WIFI_AP = 0, - CONNECTION_WIFI_STA = 1, - CONNECTION_USB_RNDIS = 2 -}; - -void -set_sdk_connection( - Client session, - enum CONNECTION connection_type, - uint32_t ip_address, - uint16_t port ); - -static const uint8_t SDK_HEARTBEAT_CMDID = 0xD5; - -void -sdk_heartbeat( - Client session ); diff --git a/include/sdk_mode.h b/include/sdk_mode.h deleted file mode 100644 index 58fd44c..0000000 --- a/include/sdk_mode.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include "message.h" - -#include -#include - -static const uint8_t SET_SDK_MODE_CMDID = 0xd1; - -void -set_sdk_mode(Client session, bool enable); diff --git a/include/wheel.h b/include/wheel.h deleted file mode 100644 index 086ca01..0000000 --- a/include/wheel.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include "message.h" - -#include -#include - -static const uint8_t SET_WHEEL_SPEED_CMDID = 0x20; - -void -set_wheel_speed ( - Client session, - int16_t w1, - int16_t w2, - int16_t w3, - int16_t w4 ); - -static const uint8_t SET_CHASSIS_WHEEL_SPEED_CMDID = 0x26; - -void -set_chassis_wheel_speed ( - Client session, - int8_t w1, - int8_t w2, - int8_t w3, - int8_t w4 ); - -static const uint8_t CHASSIS_SPEED_MODE_CMDID = 0x21; - -void -chassis_speed_mode ( - Client session, - float x, - float y, - float z ); - -static const uint8_t SET_ROBOT_MODE_CMDID = 0x46; - -enum MOVEMENTMODE { - MOVEMENTMODE_FREE, - MOVEMENTMODE_GIMBAL_LEAD, - MOVEMENTMODE_CHASSIS_LEAD -}; - -void -set_robot_mode ( - Client session, - enum MOVEMENTMODE mode ); - -// cmdset 0x48 -static const uint8_t SUBNODE_RESET_CMDID = 0x02; - -void -subnode_reset ( - Client session ); - -// cmdset 0x48 -static const uint8_t SUBSCRIBE_ADD_NODE_CMDID = 0x01; - -void -subscribe_add_node ( - Client session ); diff --git a/src/client.h b/src/client.h index 0a01c3b..b57ac9b 100644 --- a/src/client.h +++ b/src/client.h @@ -18,17 +18,6 @@ struct Client { }; -// Not sure what these are, but they are used for the hostbyte -static const uint8_t DEFAULT_CLIENT_HOST = 9; -static const uint8_t DEFAULT_CLIENT_INDEX = 6; -static const uint8_t DEFAULT_ROBOT_INDEX = 0; - -static inline uint8_t host2byte(uint8_t host, uint8_t index) { - return index * 32 + host; -} - -static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) { - *host = (b & 0x1F); - *index = b >> 5; -} +static const uint8_t CLIENT_HOST = 9; +static const uint8_t CLIENT_INDEX = 6; diff --git a/src/messages/led.c b/src/messages/led.c deleted file mode 100644 index 9d7ae38..0000000 --- a/src/messages/led.c +++ /dev/null @@ -1,33 +0,0 @@ -#include "message.h" -#include "connection.h" -#include "robomaster.h" - -void -set_system_led ( - Client session, - uint8_t red, - uint8_t green, - uint8_t blue, - enum LEDCOMP comp, - uint16_t led_mask, - enum LEDEFFECT effect, - uint16_t t1, - uint16_t t2 ) { - - union Request req = {0}; - - req.led.comp_mask = comp; - req.led.led_mask = led_mask; - req.led.effect_mode = effect; - req.led.control_mode = 7; - req.led.red = red; - req.led.green = green; - req.led.blue = blue; - req.led.loop = 0; - req.led.t1 = t1; - req.led.t2 = t2; - - req_finalize(session, 0x3F, SET_SYSTEM_LED_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SetSystemLedReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetSystemLedReq)); - -} diff --git a/src/messages/sdk_connection.c b/src/messages/sdk_connection.c deleted file mode 100644 index 4586126..0000000 --- a/src/messages/sdk_connection.c +++ /dev/null @@ -1,21 +0,0 @@ -#include "message.h" -#include "connection.h" -#include "robomaster.h" - -void -set_sdk_connection( - Client session, - enum CONNECTION connection_type, - uint32_t ip_address, - uint16_t port ) { - - union Request req = {0}; - req.sdkconn.control = 0; - req.sdkconn.host = session->hostbyte; - req.sdkconn.connection = connection_type; - req.sdkconn.protocol = 0; - req.sdkconn.ip_address = ip_address; - req.sdkconn.port = port; - req_finalize(session, 0x3F, SET_SDK_CONNECTION_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SetSdkConnectionReq), &req); - req_send(session->sdk_conn, &req, sizeof(struct SetSdkConnectionReq)); -} diff --git a/src/messages/sdk_mode.c b/src/messages/sdk_mode.c deleted file mode 100644 index 673fc30..0000000 --- a/src/messages/sdk_mode.c +++ /dev/null @@ -1,21 +0,0 @@ -#include "message.h" -#include "connection.h" -#include "robomaster.h" - -void -set_sdk_mode( - Client session, - bool enable ) { - union Request req = {0}; - req.sdkmode.enable = enable; - req_finalize(session, 0x3F, SET_SDK_MODE_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SetSdkModeReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetSdkModeReq)); -} - -void -sdk_heartbeat( - Client session ) { - union Request req = {0}; - req_finalize(session, 0x3F, SDK_HEARTBEAT_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SdkHeartbeatReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SdkHeartbeatReq)); -} diff --git a/src/messages/set_wheel_speed.c b/src/messages/set_wheel_speed.c deleted file mode 100644 index 0103c18..0000000 --- a/src/messages/set_wheel_speed.c +++ /dev/null @@ -1,78 +0,0 @@ -#include "message.h" -#include "connection.h" -#include "robomaster.h" - -void -set_wheel_speed ( - Client session, - int16_t w1, - int16_t w2, - int16_t w3, - int16_t w4 ) { - union Request req = {0}; - req.wheel.wheel_speed[0] = w1; - req.wheel.wheel_speed[1] = w2; - req.wheel.wheel_speed[2] = w3; - req.wheel.wheel_speed[3] = w4; - req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SetWheelSpeedReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetWheelSpeedReq)); -} - -void -set_chassis_wheel_speed ( - Client session, - int8_t w1, - int8_t w2, - int8_t w3, - int8_t w4 ) { - union Request req = {0}; - req.chswheel.wheel_speed[0] = w1; - req.chswheel.wheel_speed[1] = w2; - req.chswheel.wheel_speed[2] = w3; - req.chswheel.wheel_speed[3] = w4; - req_finalize(session, 0x3F, SET_CHASSIS_WHEEL_SPEED_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SetChassisWheelSpeedReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetChassisWheelSpeedReq)); -} - -void -chassis_speed_mode ( - Client session, - float x, - float y, - float z ) { - union Request req = {0}; - req.chsspeed.speed[0] = x; - req.chsspeed.speed[1] = y; - req.chsspeed.speed[2] = z; - req_finalize(session, 0x3F, CHASSIS_SPEED_MODE_CMDID, host2byte(3, 6), false, sizeof(struct ChassisSpeedModeReq), &req); - req_send(session->dev_conn, &req, sizeof(struct ChassisSpeedModeReq)); -} - -void -set_robot_mode ( - Client session, - enum MOVEMENTMODE mode ) { - union Request req = {0}; - req.mvmode.mode = mode; - req_finalize(session, 0x3F, SET_ROBOT_MODE_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SetRobotModeReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetRobotModeReq)); -} - -void -subnode_reset ( - Client session ) { - union Request req = {0}; - req.subnodereset.hostbyte = session->hostbyte; - req_finalize(session, 0x48, SUBNODE_RESET_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SubNodeResetReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SubNodeResetReq)); -} - -void -subscribe_add_node ( - Client session ) { - union Request req = {0}; - req.subnodeadd.hostbyte = session->hostbyte; - req.subnodeadd.sub_vision = 0x03000000; - req_finalize(session, 0x48, SUBSCRIBE_ADD_NODE_CMDID, host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX), true, sizeof(struct SubscribeAddNodeReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SubscribeAddNodeReq)); -} diff --git a/src/modules/chassis.c b/src/modules/chassis.c new file mode 100644 index 0000000..44b2faa --- /dev/null +++ b/src/modules/chassis.c @@ -0,0 +1,33 @@ +#include "message.h" +#include "connection.h" +#include "robomaster.h" + +void +set_wheel_speed ( + Client session, + int16_t w1, + int16_t w2, + int16_t w3, + int16_t w4 ) { + union Request req = {0}; + req.wheel.wheel_speed[0] = w1; + req.wheel.wheel_speed[1] = w2; + req.wheel.wheel_speed[2] = w3; + req.wheel.wheel_speed[3] = w4; + req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, host2byte(CHASSIS_HOST, CHASSIS_INDEX), true, sizeof(struct SetWheelSpeedReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SetWheelSpeedReq)); +} + +void +chassis_speed_mode ( + Client session, + float x, + float y, + float z ) { + union Request req = {0}; + req.chsspeed.speed[0] = x; + req.chsspeed.speed[1] = y; + req.chsspeed.speed[2] = z; + req_finalize(session, 0x3F, CHASSIS_SPEED_MODE_CMDID, host2byte(CHASSIS_HOST, CHASSIS_INDEX), false, sizeof(struct ChassisSpeedModeReq), &req); + req_send(session->dev_conn, &req, sizeof(struct ChassisSpeedModeReq)); +} diff --git a/src/modules/sdk.c b/src/modules/sdk.c new file mode 100644 index 0000000..6fe2532 --- /dev/null +++ b/src/modules/sdk.c @@ -0,0 +1,98 @@ +#include "message.h" +#include "connection.h" +#include "robomaster.h" + +void +set_sdk_connection( + Client session, + enum CONNECTION connection_type, + uint32_t ip_address, + uint16_t port ) { + + union Request req = {0}; + req.sdkconn.control = 0; + req.sdkconn.host = session->hostbyte; + req.sdkconn.connection = connection_type; + req.sdkconn.protocol = 0; + req.sdkconn.ip_address = ip_address; + req.sdkconn.port = port; + req_finalize(session, 0x3F, SET_SDK_CONNECTION_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSdkConnectionReq), &req); + req_send(session->sdk_conn, &req, sizeof(struct SetSdkConnectionReq)); +} + +void +set_sdk_mode( + Client session, + bool enable ) { + union Request req = {0}; + req.sdkmode.enable = enable; + req_finalize(session, 0x3F, SET_SDK_MODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSdkModeReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SetSdkModeReq)); +} + +void +sdk_heartbeat( + Client session ) { + union Request req = {0}; + req_finalize(session, 0x3F, SDK_HEARTBEAT_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SdkHeartbeatReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SdkHeartbeatReq)); +} + +void +set_robot_mode ( + Client session, + enum MOVEMENTMODE mode ) { + union Request req = {0}; + req.mvmode.mode = mode; + req_finalize(session, 0x3F, SET_ROBOT_MODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetRobotModeReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SetRobotModeReq)); +} + +void +subnode_reset ( + Client session ) { + union Request req = {0}; + req.subnodereset.hostbyte = session->hostbyte; + req_finalize(session, 0x48, SUBNODE_RESET_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SubNodeResetReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SubNodeResetReq)); +} + +void +subscribe_add_node ( + Client session ) { + union Request req = {0}; + req.subnodeadd.hostbyte = session->hostbyte; + req.subnodeadd.sub_vision = 0x03000000; + req_finalize(session, 0x48, SUBSCRIBE_ADD_NODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SubscribeAddNodeReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SubscribeAddNodeReq)); +} + +void +set_system_led ( + Client session, + uint8_t red, + uint8_t green, + uint8_t blue, + enum LEDCOMP comp, + uint16_t led_mask, + enum LEDEFFECT effect, + uint16_t t1, + uint16_t t2 ) { + + union Request req = {0}; + + req.led.comp_mask = comp; + req.led.led_mask = led_mask; + req.led.effect_mode = effect; + req.led.control_mode = 7; + req.led.red = red; + req.led.green = green; + req.led.blue = blue; + req.led.loop = 0; + req.led.t1 = t1; + req.led.t2 = t2; + + req_finalize(session, 0x3F, SET_SYSTEM_LED_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSystemLedReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SetSystemLedReq)); + +} diff --git a/src/robomaster.c b/src/robomaster.c index 6fb46f5..382544a 100644 --- a/src/robomaster.c +++ b/src/robomaster.c @@ -11,7 +11,7 @@ Client client_new() { struct Client* client = malloc(sizeof(struct Client)); memset(client, 0, sizeof(struct Client)); // TODO: Make this configurable - client->hostbyte = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_CLIENT_INDEX); + client->hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX); return client; }