diff --git a/CMakeLists.txt b/CMakeLists.txt index debe51c..f33f2db 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ add_library(robomaster src/message.c src/modules/sdk.c src/modules/chassis.c + src/modules/gimbal.c src/connection.c src/robomaster.c src/robo.c diff --git a/include/gimbal.h b/include/gimbal.h new file mode 100644 index 0000000..73e9220 --- /dev/null +++ b/include/gimbal.h @@ -0,0 +1,20 @@ +#pragma once + +#include "message.h" + +#include +#include + +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 ); diff --git a/include/message.h b/include/message.h index 76eab84..bcd0b76 100644 --- a/include/message.h +++ b/include/message.h @@ -168,6 +168,30 @@ 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 GimbalCtrlSpeedResp +{ + struct Header header; + int8_t retcode; + struct Footer footer; +}; + struct PACKED SetWheelSpeedReq { struct Header header; @@ -236,6 +260,7 @@ union Request { struct SetRobotModeReq mvmode; struct SubNodeResetReq subnodereset; struct SubscribeAddNodeReq subnodeadd; + struct GimbalCtrlSpeedReq gimbspeed; }; union Response { struct Header header; @@ -249,6 +274,7 @@ union Response { struct SetRobotModeResp mvmode; struct SubNodeResetResp subnodereset; struct SubscribeAddNodeResp subnodeadd; + struct GimbalCtrlSpeedResp gimbspeed; }; union Message { struct Header header; diff --git a/include/robomaster.h b/include/robomaster.h index 7bf2b61..6bf7bf9 100644 --- a/include/robomaster.h +++ b/include/robomaster.h @@ -7,6 +7,7 @@ typedef struct Client* Client; #include "message.h" #include "chassis.h" #include "sdk.h" +#include "gimbal.h" Client client_new(); void client_connect(Client client); diff --git a/src/connection.c b/src/connection.c index c9ad568..6d9d2f2 100644 --- a/src/connection.c +++ b/src/connection.c @@ -35,6 +35,8 @@ 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); default: return 0; } @@ -56,6 +58,8 @@ message_module(int cmd) { case SET_WHEEL_SPEED_CMD: case CHASSIS_SPEED_MODE_CMD: return host2byte(CHASSIS_HOST, CHASSIS_INDEX); + case GIMBAL_CTRL_SPEED_CMD: + return host2byte(GIMBAL_HOST, GIMBAL_INDEX); default: return 0; } diff --git a/src/modules/gimbal.c b/src/modules/gimbal.c new file mode 100644 index 0000000..e949080 --- /dev/null +++ b/src/modules/gimbal.c @@ -0,0 +1,18 @@ +#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); +}