floraachy
/
SpireCV
forked from floratest1/SpireCV
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

!22 云台接口优化 

Merge pull request !22 from AiYangSky/changeIgimbal
This commit is contained in:
jario-jin 2023-10-14 08:19:40 +00:00 committed by Gitee
parent 2eca3052d2 7e757d5f23
commit a9f9cf60e6
31 changed files with 1792 additions and 1218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -67,7 +67,7 @@ include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/IOs/serial/include
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/FIFO
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G1
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G2
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/AT10
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/Q10f
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src
${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl
@ -131,11 +131,11 @@ list(APPEND serial_SRCS gimbal_ctrl/IOs/serial/src/impl/unix.cc)
list(APPEND serial_SRCS gimbal_ctrl/IOs/serial/src/impl/list_ports/list_ports_linux.cc)
set(driver_SRCS
gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc
gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp
)
file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G1/*.cpp)
list(APPEND driver_SRCS ${DRV_LIB_FILES})
file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G2/*.cpp)
file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/AT10/*.cpp)
list(APPEND driver_SRCS ${DRV_LIB_FILES})
file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/Q10f/*.cpp)
list(APPEND driver_SRCS ${DRV_LIB_FILES})

38
gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h Executable file
View File

@ -0,0 +1,38 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov
* @LastEditTime: 2023-08-16 21:53:43
* @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_crc32.h
*/
#ifndef AT10_GIMBAL_CRC32_H
#define AT10_GIMBAL_CRC32_H
namespace AT10
{
static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght)
{
unsigned short temp = 0;
unsigned short i = 0;
for (i = 0; i < Lenght; i++)
{
temp += pData[i];
}
return temp & 0XFF;
}
static inline unsigned char checkXOR(unsigned char *pData, unsigned char Lenght)
{
unsigned char temp = Lenght;
unsigned char i;
for (i = 1; i < Lenght - 1; i++)
{
temp ^= pData[i];
}
return temp;
}
} // namespace name
#endif

425
gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp Executable file
View File

@ -0,0 +1,425 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov
* @LastEditTime: 2023-08-25 19:39:56
* @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_driver.cpp
*/
#include "AT10_gimbal_driver.h"
#include "AT10_gimbal_crc32.h"
#include "string.h"
/**
* The function creates a new instance of the g1GimbalDriver class, which is a subclass of the
* IamovGimbalBase class
*
* @param _IO The IOStreamBase object that will be used to communicate with the gimbal.
*/
AT10GimbalDriver::AT10GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO)
{
rxQueue = new fifoRing(sizeof(AT10::GIMBAL_EXTEND_FRAME_T), MAX_QUEUE_SIZE);
txQueue = new fifoRing(sizeof(AT10::GIMBAL_EXTEND_FRAME_T), MAX_QUEUE_SIZE);
stdRxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE);
stdTxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE);
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
// Initialize and enable attitude data return (50Hz)
// uint8_t cmd = 0XFF;
// pack(AT10::GIMBAL_CMD_SET_FEEDBACK_H, &cmd, 1);
// pack(AT10::GIMBAL_CMD_SET_FEEDBACK_L, &cmd, 1);
// cmd = 0X00;
// pack(AT10::GIMBAL_CMD_OPEN_FEEDBACK, &cmd, 1);
}
/**
* The function takes a command, a pointer to a payload, and the size of the payload. It then copies
* the payload into the tx buffer, calculates the checksum, and then calculates the CRC32 of the
* payload. It then copies the CRC32 into the tx buffer, and then copies the tx buffer into the txQueue
*
* @param uint32_t 4 bytes
* @param pPayload pointer to the data to be sent
* @param payloadSize the size of the payload
*
* @return The size of the data to be sent.
*/
uint32_t AT10GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize)
{
uint32_t ret = 0;
if (cmd > 0XFF)
{
AT10::GIMBAL_EXTEND_FRAME_T txTemp;
txTemp.head = cmd;
memcpy(txTemp.data, pPayload, payloadSize);
payloadSize--;
txTemp.len = payloadSize;
if (txQueue->inCell(&txTemp))
{
ret = payloadSize + sizeof(uint32_t) + sizeof(uint8_t);
}
}
else
{
AT10::GIMBAL_STD_FRAME_T txTemp;
txTemp.head = AT10::GIMBAL_CMD_STD;
txTemp.len = payloadSize + 3;
txTemp.cmd = cmd;
memcpy(txTemp.data, pPayload, payloadSize);
txTemp.data[payloadSize] = AT10::checkXOR((uint8_t *)&txTemp.len, txTemp.len);
if (stdTxQueue->inCell(&txTemp))
{
ret = payloadSize + 6;
}
}
return ret;
}
void AT10GimbalDriver::convert(void *buf)
{
AT10::GIMBAL_EXTEND_FRAME_T *temp;
temp = reinterpret_cast<AT10::GIMBAL_EXTEND_FRAME_T *>(buf);
switch (temp->head)
{
case AT10::GIMBAL_CMD_RCV_STATE:
std::cout << "Undefined old frame from AT10";
// AT10::GIMBAL_RCV_POS_MSG_T *tempPos;
// tempPos = reinterpret_cast<AT10::GIMBAL_RCV_POS_MSG_T *>(((uint8_t *)buf) + AT10_EXT_PAYLOAD_OFFSET);
// mState.lock();
// state.abs.yaw = tempPos->yawIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE;
// state.abs.roll = tempPos->rollIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE;
// state.abs.pitch = tempPos->pitchIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE;
// state.rel.yaw = tempPos->yawStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED;
// state.rel.roll = tempPos->rollStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED;
// state.rel.pitch = tempPos->pitchStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED;
// updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
// state.abs.roll, state.abs.pitch, state.abs.yaw,
// state.fov.x, state.fov.y);
// mState.unlock();
break;
case AT10::GIMBAL_CMD_STD:
AT10::GIMBAL_STD_FRAME_T *stdTemp;
stdTemp = reinterpret_cast<AT10::GIMBAL_STD_FRAME_T *>(buf);
switch (stdTemp->cmd)
{
case AT10::GIMBAL_CMD_STD_RCV_STATE:
AT10::GIMBAL_RCV_STD_STATE_MSG_T *tempRcv;
tempRcv = reinterpret_cast<AT10::GIMBAL_RCV_STD_STATE_MSG_T *>(((uint8_t *)buf) + AT10_STD_PAYLOAD_OFFSET);
mState.lock();
state.abs.roll = (amovGimbalTools::conversionBigLittle((uint16_t)(tempRcv->B1.roll & 0XFF0F)) * 0.043956043956044f) - 90.0f;
state.abs.yaw = (int16_t)amovGimbalTools::conversionBigLittle((uint16_t)tempRcv->B1.yaw) * 0.0054931640625f;
state.abs.pitch = (int16_t)amovGimbalTools::conversionBigLittle((uint16_t)tempRcv->B1.pitch) * 0.0054931640625f;
state.rel.yaw = state.abs.yaw;
state.rel.roll = state.abs.roll;
state.rel.pitch = state.abs.pitch;
state.fov.x = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovX) * 0.1;
state.fov.y = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovY) * 0.1;
if ((amovGimbalTools::conversionBigLittle(tempRcv->D1.camera) & 0X0003) == 0X01)
{
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE;
}
else
{
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
}
updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
state.abs.roll, state.abs.pitch, state.abs.yaw,
state.fov.x, state.fov.y);
mState.unlock();
break;
case AT10::GIMBAL_CMD_STD_NOP:
break;
default:
std::cout << "Undefined std frame from AT10";
std::cout << std::endl;
break;
}
break;
default:
printf("\r\nUndefined frame from AT10,head:%08X", temp->head);
break;
}
}
/**
* It's a state machine that parses a serial stream of bytes into a struct
*
* @param uint8_t unsigned char
*
* @return A boolean value.
*/
bool AT10GimbalDriver::parser(IN uint8_t byte)
{
bool state = false;
static uint8_t payloadLenghte = 0;
static uint8_t *pRx = nullptr;
uint8_t suncheck;
switch (parserState)
{
case AT10::GIMBAL_SERIAL_STATE_IDLE:
if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X000000FF) >> 0))
{
parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD1;
}
else if (byte == ((AT10::GIMBAL_CMD_STD & 0X0000FF00) >> 8))
{
parserState = AT10::GIMBAL_SERIAL_STATE_STD_HAED1;
}
break;
// STD msg
case AT10::GIMBAL_SERIAL_STATE_STD_HAED1:
if (byte == ((AT10::GIMBAL_CMD_STD & 0X00FF0000) >> 16))
{
parserState = AT10::GIMBAL_SERIAL_STATE_STD_HAED2;
}
else
{
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
}
break;
case AT10::GIMBAL_SERIAL_STATE_STD_HAED2:
if (byte == ((AT10::GIMBAL_CMD_STD & 0XFF000000) >> 24))
{
parserState = AT10::GIMBAL_SERIAL_STATE_STD_LEN;
}
else
{
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
}
break;
case AT10::GIMBAL_SERIAL_STATE_STD_LEN:
stdRx.len = byte;
payloadLenghte = (byte & 0X3F) - 3;
pRx = stdRx.data;
parserState = AT10::GIMBAL_SERIAL_STATE_STD_CMD;
break;
case AT10::GIMBAL_SERIAL_STATE_STD_CMD:
stdRx.cmd = byte;
parserState = AT10::GIMBAL_SERIAL_STATE_STD_DATE;
break;
case AT10::GIMBAL_SERIAL_STATE_STD_DATE:
*pRx = byte;
pRx++;
payloadLenghte--;
if (payloadLenghte == 0)
{
parserState = AT10::GIMBAL_SERIAL_STATE_STD_CHECK;
}
break;
case AT10::GIMBAL_SERIAL_STATE_STD_CHECK:
stdRx.checkXOR = byte;
if (AT10::checkXOR((uint8_t *)&stdRx.len, (stdRx.len & 0X3F)) == byte)
{
state = true;
stdRxQueue->inCell(&stdRx);
}
else
{
memset(&stdRx, 0, sizeof(AT10::GIMBAL_STD_FRAME_T));
}
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
payloadLenghte = 0;
pRx = nullptr;
break;
// EXT msg
case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD1:
if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X0000FF00) >> 8))
{
parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD2;
}
else
{
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
}
break;
case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD2:
if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X00FF0000) >> 16))
{
parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD3;
}
else
{
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
}
break;
case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD3:
if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0XFF000000) >> 24))
{
parserState = AT10::GIMBAL_SERIAL_STATE_EXT_DATE;
payloadLenghte = sizeof(AT10::GIMBAL_RCV_POS_MSG_T);
pRx = extendRx.data;
extendRx.head = AT10::GIMBAL_CMD_RCV_STATE;
}
else
{
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
}
break;
case AT10::GIMBAL_SERIAL_STATE_EXT_DATE:
*pRx = byte;
payloadLenghte--;
pRx++;
if (payloadLenghte == 0)
{
parserState = AT10::GIMBAL_SERIAL_STATE_EXT_CHECK;
}
break;
case AT10::GIMBAL_SERIAL_STATE_EXT_CHECK:
suncheck = AT10::CheckSum(extendRx.data, sizeof(AT10::GIMBAL_RCV_POS_MSG_T));
if (byte == suncheck)
{
state = true;
rxQueue->inCell(&extendRx);
}
else
{
memset(&extendRx, 0, sizeof(AT10::GIMBAL_EXTEND_FRAME_T));
}
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
payloadLenghte = 0;
pRx = nullptr;
break;
default:
parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
memset(&extendRx, 0, sizeof(AT10::GIMBAL_EXTEND_FRAME_T));
memset(&stdRx, 0, sizeof(AT10::GIMBAL_STD_FRAME_T));
payloadLenghte = 0;
pRx = nullptr;
break;
}
return state;
}
void AT10GimbalDriver::sendHeart(void)
{
// uint8_t tempBuffer[72];
uint8_t temp = 0X00;
while (1)
{
// if (!IO->isBusy() && IO->isOpen())
// {
// bool state = false;
// state = txQueue->outCell(&tempBuffer);
// if (state)
// {
// IO->outPutBytes((uint8_t *)&tempBuffer,
// reinterpret_cast<AT10::GIMBAL_EXTEND_FRAME_T *>(tempBuffer)->len + AT10_EXT_PAYLOAD_OFFSET + sizeof(uint8_t));
// }
// }
std::this_thread::sleep_for(std::chrono::milliseconds(100));
pack(AT10::GIMBAL_CMD_STD_HEART, &temp, sizeof(temp));
}
}
void AT10GimbalDriver::sendStd(void)
{
uint8_t tempBuffer[72];
while (1)
{
if (!IO->isBusy() && IO->isOpen())
{
bool state = false;
state = stdTxQueue->outCell(&tempBuffer);
if (state)
{
IO->outPutBytes((uint8_t *)&tempBuffer + 1,
reinterpret_cast<AT10::GIMBAL_STD_FRAME_T *>(tempBuffer)->len + 3);
}
}
}
}
void AT10GimbalDriver::stackStart(void)
{
if (!this->IO->isOpen())
{
this->IO->open();
}
std::thread sendHeartLoop(&AT10GimbalDriver::sendHeart, this);
std::thread sendStdLoop(&AT10GimbalDriver::sendStd, this);
sendHeartLoop.detach();
sendStdLoop.detach();
}
void AT10GimbalDriver::parserLoop(void)
{
uint8_t temp;
while (1)
{
if (IO->inPutByte(&temp))
{
parser(temp);
}
}
}
void AT10GimbalDriver::getStdRxPack(void)
{
uint8_t tempBuffer[280];
while (1)
{
if (stdRxQueue->outCell(tempBuffer))
{
msgCustomCallback(tempBuffer);
convert(tempBuffer);
}
}
}
void AT10GimbalDriver::getExtRxPack(void)
{
uint8_t tempBuffer[280];
while (1)
{
if (rxQueue->outCell(tempBuffer))
{
msgCustomCallback(tempBuffer);
convert(tempBuffer);
}
}
}
void AT10GimbalDriver::parserStart(amovGimbal::pStateInvoke callback)
{
this->updateGimbalStateCallback = callback;
std::thread parser(&AT10GimbalDriver::parserLoop, this);
std::thread getStdRxPackLoop(&AT10GimbalDriver::getStdRxPack, this);
std::thread getExtRxPackLooP(&AT10GimbalDriver::getExtRxPack, this);
parser.detach();
getStdRxPackLoop.detach();
getExtRxPackLooP.detach();
}
uint32_t AT10GimbalDriver::calPackLen(void *pack)
{
return 0;
}

82
gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h Executable file
View File

@ -0,0 +1,82 @@
/*
* @Description: Q10f吊舱的驱动文件
* @Author: L LC @amov
* @Date: 2022-10-28 12:24:21
* @LastEditors: L LC @amov
* @LastEditTime: 2023-08-25 19:28:55
* @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_driver.h
*/
#include "../amov_gimbal.h"
#include "../amov_gimbal_private.h"
#include "AT10_gimbal_struct.h"
#include <mutex>
#include <malloc.h>
#include <iostream>
#ifndef __AT10_DRIVER_H
#define __AT10_DRIVER_H
class AT10GimbalDriver : protected amovGimbal::amovGimbalBase
{
private:
AT10::GIMBAL_SERIAL_STATE_T parserState;
AT10::GIMBAL_EXTEND_FRAME_T extendRx;
AT10::GIMBAL_STD_FRAME_T stdRx;
fifoRing *stdRxQueue;
fifoRing *stdTxQueue;
// void send(void);
void stackStart(void);
void sendHeart(void);
void sendStd(void);
void parserStart(amovGimbal::pStateInvoke callback);
void parserLoop(void);
void getExtRxPack(void);
void getStdRxPack(void);
// bool getRxPack(OUT void *pack);
bool parser(IN uint8_t byte);
void convert(void *buf);
uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
uint32_t calPackLen(void *pack);
public:
// funtions
uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed);
uint32_t setGimabalHome(void);
uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t takePic(void);
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
uint32_t extensionFuntions(void* cmd);
// builds
static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
{
return new AT10GimbalDriver(_IO);
}
AT10GimbalDriver(amovGimbal::IOStreamBase *_IO);
~AT10GimbalDriver()
{
if (stdRxQueue != nullptr)
{
delete stdRxQueue;
}
if (stdTxQueue != nullptr)
{
delete stdTxQueue;
}
}
};
#endif

208
gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp Executable file
View File

@ -0,0 +1,208 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-03-02 10:00:52
* @LastEditors: L LC @amov
* @LastEditTime: 2023-09-07 10:56:15
* @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_funtion.cpp
*/
#include "AT10_gimbal_driver.h"
#include "AT10_gimbal_crc32.h"
#include "string.h"
#include "math.h"
/**
* It sets the gimbal position.
*
* @param pos the position of the gimbal
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t AT10GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
{
int16_t yaw, pitch;
AT10::GIMBAL_CMD_A1_MSG_T temp;
yaw = (int16_t)(pos.yaw / (360.0f / 65536.0f));
printf("\r\n %04X\r\n", yaw);
yaw = amovGimbalTools::conversionBigLittle((uint16_t)yaw);
pitch = (int16_t)(pos.pitch / (360.0f / 65536.0f));
pitch = amovGimbalTools::conversionBigLittle((uint16_t)pitch);
temp.cmd = 0x0B;
temp.param[0] = yaw;
temp.param[1] = pitch;
temp.param[2] = 0;
temp.param[3] = 0;
return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T));
}
/**
* It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type
* G1::GIMBAL_SET_POS_MSG_T
*
* @param speed the speed of the gimbal
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t AT10GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
{
int16_t speedYaw, speedPitch;
AT10::GIMBAL_CMD_A1_MSG_T temp;
speedYaw = (int16_t)(speed.yaw * 100);
printf("\r\n %04X\r\n", speedYaw);
speedYaw = amovGimbalTools::conversionBigLittle((uint16_t)speedYaw);
speedPitch = (int16_t)(speed.pitch * 100);
speedPitch = amovGimbalTools::conversionBigLittle((uint16_t)speedPitch);
temp.cmd = 0x01;
temp.param[0] = speedYaw;
temp.param[1] = speedPitch;
temp.param[2] = 0;
temp.param[3] = 0;
return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T));
}
/**
* This function sets the gimbal's follow speed
*
* @param followSpeed the speed of the gimbal
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t AT10GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
{
state.maxFollow.pitch = fabs(followSpeed.pitch * 100);
state.maxFollow.yaw = fabs(followSpeed.yaw * 100);
state.maxFollow.roll = fabs(followSpeed.roll * 100);
return 0;
}
/**
* This function sets the gimbal to its home position
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t AT10GimbalDriver::setGimabalHome(void)
{
AT10::GIMBAL_CMD_A1_MSG_T temp;
temp.cmd = 0x04;
temp.param[0] = 0;
temp.param[1] = 0;
temp.param[2] = 0;
temp.param[3] = 0;
return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T));
}
/**
* It takes a picture.
*
* @return The return value is the number of bytes written to the serial port.
*/
uint32_t AT10GimbalDriver::takePic(void)
{
uint16_t temp = 0x13 << 3;
uint16_t data = amovGimbalTools::conversionBigLittle(temp);
return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t));
}
/**
* The function sets the video state of the gimbal
*
* @param newState The new state of the video.
*
* @return The return value is the number of bytes written to the serial port.
*/
uint32_t AT10GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
{
uint16_t temp;
if (newState == amovGimbal::AMOV_GIMBAL_VIDEO_T::AMOV_GIMBAL_VIDEO_TAKE)
{
temp = 0x14 << 3;
}
else
{
temp = 0x15 << 3;
}
uint16_t data = amovGimbalTools::conversionBigLittle(temp);
return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t));
}
uint32_t AT10GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
if (targetRate == 0.0f)
{
uint16_t temp = 0;
switch (zoom)
{
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
temp = 0X08 << 3;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
temp = 0X09 << 3;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
temp = 0X01 << 3;
break;
default:
break;
}
uint16_t data = amovGimbalTools::conversionBigLittle(temp);
return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t));
}
else
{
AT10::GIMBAL_CMD_C2_MSG_T temp;
temp.cmd = 0x53;
temp.param = targetRate * 10;
temp.param = amovGimbalTools::conversionBigLittle(temp.param);
return pack(AT10::GIMBAL_CMD_STD_CAMERA2, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_C2_MSG_T));
}
}
uint32_t AT10GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
uint16_t temp = 0;
switch (zoom)
{
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
temp = 0X0B << 3;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
temp = 0X0A << 3;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
temp = 0X01 << 3;
break;
default:
break;
}
uint16_t data = amovGimbalTools::conversionBigLittle(temp);
return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t));
}
//
/**
* The function `extensionFuntions` in the `AT10GimbalDriver` class takes a command as input, casts it
* to a specific type, and then packs the command and its parameters into a byte array.
*
* @param cmd The "cmd" parameter is a void pointer, which means it can point to any type of data. In
* this case, it is being cast to a pointer of type AT10::AT10_EXT_CMD_T using the reinterpret_cast
* operator.
*
* @return the result of the `pack` function, which is of type `uint32_t`.
*/
uint32_t AT10GimbalDriver::extensionFuntions(void *cmd)
{
AT10::AT10_EXT_CMD_T *tempCMD;
tempCMD = reinterpret_cast<AT10::AT10_EXT_CMD_T *>(cmd);
return pack(tempCMD->cmd, (uint8_t *)tempCMD->param, tempCMD->paramLen);
}
// AT10_EXT_CMD_T infraredOpen ;
// infraredOpen.cmd = AT10::GIMBAL_CMD_STD_CAMERA;
// infraredOpen.param[0] = 0X02;
// infraredOpen.param[1] = 0;
// infraredOpen.paramLen = 2;

174
gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h Executable file
View File

@ -0,0 +1,174 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:07
* @LastEditors: L LC @amov
* @LastEditTime: 2023-08-25 19:32:59
* @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_struct.h
*/
#ifndef AT10_GIMBAL_STRUCT_H
#define AT10_GIMBAL_STRUCT_H
#include <stdint.h>
namespace AT10
{
#define AT10_MAX_GIMBAL_PAYLOAD 64
#define AT10_EXT_PAYLOAD_OFFSET 4
#define AT10_STD_PAYLOAD_OFFSET 6
#define AT10_EXT_SCALE_FACTOR_ANGLE 0.02197f
#define AT10_EXT_SCALE_FACTOR_SPEED 0.06103f
typedef enum
{
GIMBAL_CMD_STD_NOP = 0X00,
GIMBAL_CMD_STD_HEART = 0X10,
GIMBAL_CMD_STD_RCV_STATE = 0X40,
GIMBAL_CMD_STD_MOTOR = 0X1A,
GIMBAL_CMD_STD_CAMERA = 0X1C,
GIMBAL_CMD_STD_CAMERA2 = 0X2C,
GIMBAL_CMD_STD_MOTOR2 = 0X32,
GIMBAL_CMD_STD = 0XDCAA5500,
GIMBAL_CMD_RCV_STATE = 0X721A583E,
GIMBAL_CMD_SET_FEEDBACK_L = 0X143055AA,
GIMBAL_CMD_SET_FEEDBACK_H = 0X003155AA,
GIMBAL_CMD_OPEN_FEEDBACK = 0X3E003E3E,
GIMBAL_CMD_CLOSE_FEEDBACK = 0X3D003D3E,
} GIMBAL_CMD_T;
typedef enum
{
GIMBAL_SERIAL_STATE_IDLE,
GIMBAL_SERIAL_STATE_EXT_HEAD1,
GIMBAL_SERIAL_STATE_EXT_HEAD2,
GIMBAL_SERIAL_STATE_EXT_HEAD3,
GIMBAL_SERIAL_STATE_EXT_DATE,
GIMBAL_SERIAL_STATE_EXT_CHECK,
GIMBAL_SERIAL_STATE_STD_HAED1,
GIMBAL_SERIAL_STATE_STD_HAED2,
GIMBAL_SERIAL_STATE_STD_LEN,
GIMBAL_SERIAL_STATE_STD_CMD,
GIMBAL_SERIAL_STATE_STD_DATE,
GIMBAL_SERIAL_STATE_STD_CHECK,
} GIMBAL_SERIAL_STATE_T;
#pragma pack(1)
typedef struct
{
uint8_t cmd;
uint8_t param[AT10_MAX_GIMBAL_PAYLOAD];
uint8_t paramLen;
} AT10_EXT_CMD_T;
typedef struct
{
uint32_t head;
uint8_t len;
uint8_t cmd;
uint8_t data[AT10_MAX_GIMBAL_PAYLOAD];
uint8_t checkXOR;
} GIMBAL_STD_FRAME_T;
typedef struct
{
uint32_t head;
uint8_t data[AT10_MAX_GIMBAL_PAYLOAD];
uint8_t checkSum;
uint8_t len;
} GIMBAL_EXTEND_FRAME_T;
typedef struct
{
uint16_t timeStamp;
int16_t rollIMUAngle;
int16_t pitchIMUAngle;
int16_t yawIMUAngle;
int16_t rollTAGAngle;
int16_t pitchTAGAngle;
int16_t yawTAGAngle;
int16_t rollTAGSpeed;
int16_t pitchTAGSpeed;
int16_t yawTAGSpeed;
int16_t rollStatorRotorAngle;
int16_t pitchStatorRotorAngle;
int16_t yawStatorRotorAngle;
} GIMBAL_RCV_POS_MSG_T;
typedef struct
{
uint8_t hight;
uint8_t reserve;
uint32_t lat;
uint32_t log;
int16_t alt;
uint32_t latTar;
uint32_t logTar;
int16_t altTar;
} GIMBAL_RCV_GPS_STATE_MSG_T;
typedef struct
{
int16_t roll;
int16_t yaw;
int16_t pitch;
} GIMBAL_RCV_MOTOR_STATE_MSG_T;
typedef struct
{
uint8_t mode;
uint8_t reserve;
uint16_t camera;
uint16_t distance;
uint16_t fovY;
uint16_t fovX;
uint16_t rate;
} GIMBAL_RCV_CAMERA_STATE_MSG_T;
typedef struct
{
GIMBAL_RCV_GPS_STATE_MSG_T T1;
uint8_t F1;
GIMBAL_RCV_MOTOR_STATE_MSG_T B1;
GIMBAL_RCV_CAMERA_STATE_MSG_T D1;
} GIMBAL_RCV_STD_STATE_MSG_T;
typedef struct
{
uint16_t param;
} GIMBAL_CMD_C1_MSG_T;
typedef struct
{
uint8_t cmd;
uint16_t param;
} GIMBAL_CMD_C2_MSG_T;
typedef struct
{
uint8_t cmd;
uint16_t param[4];
} GIMBAL_CMD_A1_MSG_T;
typedef struct
{
uint8_t cmd;
uint8_t reserve;
uint32_t param[3];
} GIMBAL_CMD_S1_MSG_T;
typedef struct
{
uint8_t param[3];
} GIMBAL_CMD_E1_MSG_T;
typedef struct
{
GIMBAL_CMD_A1_MSG_T a1;
GIMBAL_CMD_C1_MSG_T c1;
GIMBAL_CMD_E1_MSG_T e1;
GIMBAL_CMD_S1_MSG_T s1;
} GIMBAL_CMD_A1C1E1S1_MSG_T;
#pragma pack()
}
#endif

36
gimbal_ctrl/driver/src/CMakeLists.txt Executable file
View File

@ -0,0 +1,36 @@
add_library(AMOV_Gimbal ${LIB_FLAG})
SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb -fPIC")
SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall -fPIC")
add_definitions(
-DMAX_QUEUE_SIZE=100
)
add_subdirectory(FIFO)
########## add types of gimbal ##############
add_subdirectory(G1)
add_subdirectory(G2)
add_subdirectory(Q10f)
add_subdirectory(AT10)
file(GLOB LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp)
target_sources(AMOV_Gimbal
PRIVATE
${LIB_FILES}
)
target_link_libraries(AMOV_Gimbal
PRIVATE
Gimabl_G1
Gimabl_G2
Gimabl_Q10f
Gimabl_AT10
)
target_include_directories(AMOV_Gimbal
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)

13
gimbal_ctrl/driver/src/FIFO/CMakeLists.txt Executable file
View File

@ -0,0 +1,13 @@
add_library(FIFO)
file(GLOB LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp)
target_sources(FIFO
PRIVATE
${LIB_FILES}
)
target_include_directories(FIFO
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)

212
gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc
View File

@ -1,212 +0,0 @@
/*
* @Description :
* @Author : Aiyangsky
* @Date : 2022-08-26 21:42:10
* @LastEditors : Aiyangsky
* @LastEditTime : 2022-08-27 03:43:49
* @FilePath : \mavlink\src\route\Ring_Fifo.c
*/
#include <string.h>
#include "Ring_Fifo.h"
/**
* @description:
* @param {RING_FIFO_CB_T} *fifo fifo struct pointer
* @param {unsigned short} cell_size sizeof(cell)
* @param {unsigned char} *buffer fifo buffer address
* @param {unsigned int} buffer_lenght sizeof(buffer)
* @return {*}
* @note :
*/
void Ring_Fifo_init(RING_FIFO_CB_T *fifo, unsigned short cell_size,
unsigned char *buffer, unsigned int buffer_lenght)
{
fifo->cell_size = cell_size;
fifo->start = buffer;
// Remainder is taken to avoid splicing in the output so as to improve the efficiency
fifo->end = buffer + buffer_lenght - (buffer_lenght % cell_size);
fifo->in = buffer;
fifo->out = buffer;
fifo->curr_number = 0;
fifo->max_number = buffer_lenght / cell_size;
}
/**
* @description: add a cell to fifo
* @param {RING_FIFO_CB_T} *fifo fifo struct pointer
* @param {void} *data cell data [in]
* @return {*} Success or fail
* @note : failed if without space
*/
bool Ring_Fifo_in_cell(RING_FIFO_CB_T *fifo, void *data)
{
unsigned char *next;
unsigned char *ptemp = fifo->in;
bool ret = false;
LOCK();
if (fifo->curr_number < fifo->max_number)
{
next = fifo->in + fifo->cell_size;
if (next >= fifo->end)
{
next = fifo->start;
}
fifo->in = next;
fifo->curr_number++;
memcpy(ptemp, data, fifo->cell_size);
ret = true;
}
UNLOCK();
return ret;
}
/**
* @description: add a series of cells to fifo
* @param {RING_FIFO_CB_T} *fifo
* @param {void} *data cells data [in]
* @param {unsigned short} number expect add number of cells
* @return {*} number of successful add
* @note :
*/
unsigned short Ring_Fifo_in_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number)
{
// Number of remaining storable cells is described to simplify the calculation in the copying process.
unsigned short diff = fifo->max_number - fifo->curr_number;
unsigned short count_temp, count_temp_r;
unsigned char *next;
unsigned char *ptemp = fifo->in;
unsigned short ret;
LOCK();
if (diff > number)
{
ret = number;
}
else if (diff > 0 && diff < number)
{
ret = diff;
}
else
{
ret = 0;
}
count_temp = fifo->cell_size * ret;
next = fifo->in + count_temp;
// Moving the write pointer and the number of stored cells before
// copying data reduces the likelihood of multithreaded write conflicts.
fifo->curr_number += ret;
if (next < fifo->end)
{
fifo->in = next;
memcpy(ptemp, data, count_temp);
}
else
{
count_temp_r = fifo->end - fifo->in;
next = fifo->start + count_temp - count_temp_r;
fifo->in = next;
memcpy(ptemp, data, count_temp_r);
memcpy(fifo->start, ((unsigned char *)data) + count_temp_r, count_temp - count_temp_r);
}
UNLOCK();
return ret;
}
/**
* @description: output a cell
* @param {RING_FIFO_CB_T} *fifo
* @param {void} *data cell data [out]
* @return {*} Success or fail
* @note : fail if without cell
*/
bool Ring_Fifo_out_cell(RING_FIFO_CB_T *fifo, void *data)
{
unsigned char *next;
unsigned char *ptemp = fifo->out;
bool ret = false;
LOCK();
if (fifo->curr_number > 0)
{
next = fifo->out + fifo->cell_size;
if (next >= fifo->end)
{
next = fifo->start;
}
fifo->out = next;
fifo->curr_number--;
memcpy(data, ptemp, fifo->cell_size);
ret = true;
}
UNLOCK();
return ret;
}
/**
* @description: output a series of cells in fifo
* @param {RING_FIFO_CB_T} *fifo
* @param {void} *data cells data [out]
* @param {unsigned short} number expect out number of cells
* @return {*} number of successful output
* @note :
*/
unsigned short Ring_Fifo_out_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number)
{
unsigned char *next;
unsigned char *ptemp = fifo->out;
unsigned short count_temp, count_temp_r;
unsigned short ret;
LOCK();
if (fifo->curr_number > number)
{
ret = number;
}
else if (fifo->curr_number < number && fifo->curr_number > 0)
{
ret = fifo->curr_number;
}
else
{
ret = 0;
}
count_temp = fifo->cell_size * ret;
next = fifo->out + count_temp;
fifo->curr_number -= ret;
if (next < fifo->end)
{
fifo->out = next;
memcpy(data, ptemp, count_temp);
}
else
{
count_temp_r = fifo->end - fifo->in;
next = fifo->start + count_temp - count_temp_r;
fifo->out = next;
memcpy(data, ptemp, count_temp_r);
memcpy(((unsigned char *)data) + count_temp_r, fifo->start, count_temp - count_temp_r);
}
UNLOCK();
return ret;
}

228
gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp Executable file
View File

@ -0,0 +1,228 @@
/*
* @Description :
* @Author : Aiyangsky
* @Date : 2022-08-26 21:42:10
* @LastEditors: L LC @amov
* @LastEditTime: 2023-07-21 16:10:16
* @FilePath: /gimbal-sdk-multi-platform/src/FIFO/Ring_Fifo.cpp
*/
#include <string.h>
#include "Ring_Fifo.h"
/**
* The `fifoRing` constructor initializes the `cellSize`, `maxNumber`, `currNumber`, `start`, `in`,
* `out`, and `end` variables, and allocates memory for the `start` pointer.
*
* @param _cellSize The `_cellSize` parameter represents the size of each cell in the FIFO ring buffer.
* It determines the amount of memory allocated for each element in the buffer.
* @param _cellNum The parameter `_cellNum` represents the number of cells in the FIFO ring.
*/
fifoRing::fifoRing(unsigned short _cellSize, unsigned int _cellNum)
{
cellSize = _cellSize;
maxNumber = _cellNum;
currNumber = 0;
start = nullptr;
start = (uint8_t *)malloc(_cellNum * _cellSize);
if (start == nullptr)
{
std::cout << "fifo malloc failed! size :" << (_cellNum * _cellSize) << std::endl;
exit(1);
}
memset(start, 0, _cellNum * _cellSize);
in = start;
out = start;
end = start + _cellNum * _cellSize;
}
/**
* The `inCell` function adds data to a FIFO ring buffer and returns true if successful.
*
* @param data A pointer to the data that needs to be stored in the FIFO ring.
*
* @return a boolean value.
*/
bool fifoRing::inCell(void *data)
{
std::lock_guard<std::mutex> locker(fifoMutex);
unsigned char *next;
unsigned char *ptemp = in;
bool ret = false;
if (currNumber < maxNumber)
{
next = in + cellSize;
if (next >= end)
{
next = start;
}
in = next;
currNumber++;
memcpy(ptemp, data, cellSize);
ret = true;
notEmpty.notify_all();
}
return ret;
}
/**
* The `inCells` function is used to store data in a FIFO ring buffer, returning the number of cells
* successfully stored.
*
* @param data A pointer to the data that needs to be stored in the FIFO ring.
* @param number The parameter "number" represents the number of cells that the function should attempt
* to store in the FIFO ring.
*
* @return the number of cells that were successfully stored in the FIFO ring buffer.
*/
unsigned short fifoRing::inCells(void *data, unsigned short number)
{
std::lock_guard<std::mutex> locker(fifoMutex);
// Number of remaining storable cells is described to simplify the calculation in the copying process.
unsigned short diff = maxNumber - currNumber;
unsigned short count_temp, count_temp_r;
unsigned char *next;
unsigned char *ptemp = in;
unsigned short ret;
if (diff > number)
{
ret = number;
}
else if (diff > 0 && diff < number)
{
ret = diff;
}
else
{
ret = 0;
}
count_temp = cellSize * ret;
next = in + count_temp;
// Moving the write pointer and the number of stored cells before
// copying data reduces the likelihood of multithreaded write conflicts.
currNumber += ret;
if (next < end)
{
in = next;
memcpy(ptemp, data, count_temp);
}
else
{
count_temp_r = end - in;
next = start + count_temp - count_temp_r;
in = next;
memcpy(ptemp, data, count_temp_r);
memcpy(start, ((unsigned char *)data) + count_temp_r, count_temp - count_temp_r);
}
if (ret > 0)
{
notEmpty.notify_all();
}
return ret;
}
/**
* The `outCell` function removes a cell from the FIFO ring buffer and copies its data to the provided
* memory location.
*
* @param data A pointer to the memory location where the data from the cell will be copied to.
*
* @return a boolean value. If a cell is successfully taken from the FIFO ring and the data is copied
* into the provided pointer, the function returns true. Otherwise, if the FIFO ring is empty and no
* cell can be taken, the function waits until a cell becomes available and then returns false.
*/
bool fifoRing::outCell(void *data)
{
std::lock_guard<std::mutex> locker(fifoMutex);
unsigned char *next;
unsigned char *ptemp = out;
bool ret = false;
if (currNumber > 0)
{
next = out + cellSize;
if (next >= end)
{
next = start;
}
out = next;
currNumber--;
memcpy(data, ptemp, cellSize);
ret = true;
}
else
{
notEmpty.wait(fifoMutex);
}
return ret;
}
/**
* The `outCells` function retrieves a specified number of cells from a FIFO ring buffer and copies the
* data into a provided buffer.
*
* @param data A pointer to the memory location where the extracted data will be stored.
* @param number The parameter "number" represents the number of cells that should be read from the
* FIFO ring.
*
* @return the number of cells that were successfully read from the FIFO ring buffer.
*/
unsigned short fifoRing::outCells(void *data, unsigned short number)
{
std::lock_guard<std::mutex> locker(fifoMutex);
unsigned char *next;
unsigned char *ptemp = out;
unsigned short count_temp, count_temp_r;
unsigned short ret;
if (currNumber > number)
{
ret = number;
}
else if (currNumber < number && currNumber > 0)
{
ret = currNumber;
}
else
{
ret = 0;
}
count_temp = cellSize * ret;
next = out + count_temp;
currNumber -= ret;
if (next < end)
{
out = next;
memcpy(data, ptemp, count_temp);
}
else
{
count_temp_r = end - in;
next = start + count_temp - count_temp_r;
out = next;
memcpy(data, ptemp, count_temp_r);
memcpy(((unsigned char *)data) + count_temp_r, start, count_temp - count_temp_r);
}
if (ret == 0)
{
notEmpty.wait(fifoMutex);
}
return ret;
}

52
gimbal_ctrl/driver/src/FIFO/Ring_Fifo.h
View File

@ -3,45 +3,47 @@
* @Author : Aiyangsky
* @Date : 2022-08-26 21:42:02
* @LastEditors: L LC @amov
* @LastEditTime: 2023-03-03 16:12:37
* @FilePath: \host\gimbal-sdk-multi-platform\src\FIFO\Ring_Fifo.h
* @LastEditTime: 2023-08-16 21:22:46
* @FilePath: /gimbal-sdk-multi-platform/src/FIFO/Ring_Fifo.h
*/
#ifndef RING_FIFO_H
#define RING_FIFO_H
#include "stdbool.h"
#include <thread>
#include <mutex>
#include <condition_variable>
#include <iostream>
#ifdef __cplusplus
extern "C"
class fifoRing
{
#endif
#define LOCK()
#define UNLOCK()
typedef struct
{
private:
unsigned char *start;
unsigned char *in;
unsigned char *out;
unsigned char *end;
unsigned short curr_number;
unsigned short max_number;
unsigned short cell_size;
} RING_FIFO_CB_T;
unsigned short currNumber;
unsigned short maxNumber;
unsigned short cellSize;
void Ring_Fifo_init(RING_FIFO_CB_T *fifo, unsigned short cell_size,
unsigned char *buffer, unsigned int buffer_lenght);
bool Ring_Fifo_in_cell(RING_FIFO_CB_T *fifo, void *data);
unsigned short Ring_Fifo_in_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number);
bool Ring_Fifo_out_cell(RING_FIFO_CB_T *fifo, void *data);
unsigned short Ring_Fifo_out_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number);
std::mutex fifoMutex;
std::condition_variable_any notEmpty;
public:
fifoRing(unsigned short _cellSize, unsigned int _cellNum);
~fifoRing()
{
if (start != nullptr)
{
free(start);
}
}
#ifdef __cplusplus
}
#endif
bool inCell(void *data);
unsigned short inCells(void *data, unsigned short number);
bool outCell(void *data);
unsigned short outCells(void *data, unsigned short number);
};
#endif

76
gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov
* @LastEditTime: 2023-04-18 10:12:46
* @LastEditTime: 2023-09-07 11:01:25
* @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_driver.cpp
*/
#include "g1_gimbal_driver.h"
@ -16,27 +16,10 @@
*
* @param _IO The IOStreamBase object that will be used to communicate with the gimbal.
*/
g1GimbalDriver::g1GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO)
g1GimbalDriver::g1GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO)
{
memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T));
memset(&txQueue, 0, sizeof(RING_FIFO_CB_T));
rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T));
if (rxBuffer == NULL)
{
std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
exit(1);
}
txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T));
if (txBuffer == NULL)
{
free(rxBuffer);
std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
exit(1);
}
Ring_Fifo_init(&rxQueue, sizeof(G1::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T));
Ring_Fifo_init(&txQueue, sizeof(G1::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T));
rxQueue = new fifoRing(sizeof(G1::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
txQueue = new fifoRing(sizeof(G1::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
parserState = G1::GIMBAL_SERIAL_STATE_IDLE;
}
@ -66,32 +49,14 @@ uint32_t g1GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloa
txTemp.crc.u32 = G1::CRC32Software(txTemp.payload, payloadSize);
memcpy(txTemp.payload + payloadSize, txTemp.crc.u8, sizeof(uint32_t));
txMutex.lock();
if (Ring_Fifo_in_cell(&txQueue, &txTemp))
if (txQueue->inCell(&txTemp))
{
ret = txTemp.lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t);
}
txMutex.unlock();
return ret;
}
/**
* > This function is used to get a packet from the receive queue
*
* @param void This is the type of data that will be stored in the queue.
*
* @return A boolean value.
*/
bool g1GimbalDriver::getRxPack(OUT void *pack)
{
bool state = false;
rxMutex.lock();
state = Ring_Fifo_out_cell(&rxQueue, pack);
rxMutex.unlock();
return state;
}
void g1GimbalDriver::convert(void *buf)
{
G1::GIMBAL_FRAME_T *temp;
@ -125,32 +90,9 @@ void g1GimbalDriver::convert(void *buf)
}
}
/**
* The function is called by the main thread to send a command to the gimbal.
*
* The function first checks to see if the serial port is busy and if it is open. If it is not busy and
* it is open, the function locks the txMutex and then checks to see if there is a command in the
* txQueue. If there is a command in the txQueue, the function copies the command to the tx buffer and
* then unlocks the txMutex. The function then sends the command to the gimbal.
*
* The txQueue is a ring buffer that holds commands that are waiting to be sent to the gimbal. The
* txQueue is a ring buffer because the gimbal can only process one command at a time. If the gimbal is
* busy processing a command, the command will be placed in the txQueue and sent to the gimbal when the
* gimbal is ready to receive the command.
*/
void g1GimbalDriver::send(void)
uint32_t g1GimbalDriver::calPackLen(void *pack)
{
if (!IO->isBusy() && IO->isOpen())
{
bool state = false;
txMutex.lock();
state = Ring_Fifo_out_cell(&txQueue, &tx);
txMutex.unlock();
if (state)
{
IO->outPutBytes((uint8_t *)&tx, tx.lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t));
}
}
return ((G1::GIMBAL_FRAME_T *)pack)->lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t);
}
/**
@ -225,9 +167,7 @@ bool g1GimbalDriver::parser(IN uint8_t byte)
if (*((uint32_t *)(pRx - sizeof(uint32_t))) == G1::CRC32Software(rx.payload, rx.lenght))
{
state = true;
rxMutex.lock();
Ring_Fifo_in_cell(&rxQueue, &rx);
rxMutex.unlock();
rxQueue->inCell(&rx);
}
else
{

46
gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h
View File

@ -3,10 +3,11 @@
* @Author: L LC @amov
* @Date: 2022-10-28 12:24:21
* @LastEditors: L LC @amov
* @LastEditTime: 2023-03-13 12:29:17
* @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_driver.h
* @LastEditTime: 2023-09-07 02:31:19
* @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_driver.h
*/
#include "../amov_gimbal.h"
#include "../amov_gimbal_private.h"
#include "g1_gimbal_struct.h"
#include <mutex>
#include <malloc.h>
@ -15,31 +16,16 @@
#ifndef __G1_DRIVER_H
#define __G1_DRIVER_H
extern "C"
{
#include "Ring_Fifo.h"
}
class g1GimbalDriver : protected amovGimbal::IamovGimbalBase
class g1GimbalDriver : protected amovGimbal::amovGimbalBase
{
private:
G1::GIMBAL_CMD_PARSER_STATE_T parserState;
G1::GIMBAL_FRAME_T rx;
G1::GIMBAL_FRAME_T tx;
std::mutex rxMutex;
uint8_t *rxBuffer;
RING_FIFO_CB_T rxQueue;
std::mutex txMutex;
uint8_t *txBuffer;
RING_FIFO_CB_T txQueue;
bool parser(IN uint8_t byte);
void send(void);
void convert(void *buf);
uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
bool getRxPack(OUT void *pack);
bool parser(IN uint8_t byte);
void convert(void *buf);
uint32_t calPackLen(void *pack);
public:
// funtions
@ -51,18 +37,24 @@ public:
uint32_t takePic(void);
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATERNION_T &quaterion,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &speed,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData);
uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData);
uint32_t extensionFuntions(void *cmd);
// builds
static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
{
return new g1GimbalDriver(_IO);
}
g1GimbalDriver(amovGimbal::IOStreamBase *_IO);
~g1GimbalDriver()
{
free(rxBuffer);
free(txBuffer);
}
};
#endif

111
gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp
View File

@ -3,12 +3,13 @@
* @Author: L LC @amov
* @Date: 2023-03-02 10:00:52
* @LastEditors: L LC @amov
* @LastEditTime: 2023-03-17 18:29:33
* @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_funtion.cpp
* @LastEditTime: 2023-09-07 10:50:30
* @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_funtion.cpp
*/
#include "g1_gimbal_driver.h"
#include "g1_gimbal_crc32.h"
#include "string.h"
#include <math.h>
/**
* It sets the gimbal position.
@ -104,7 +105,7 @@ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState
uint8_t temp = G1::GIMBAL_CMD_CAMERA_REC;
mState.lock();
if(state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
{
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
}
@ -116,3 +117,107 @@ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState
return pack(G1::GIMBAL_CMD_CAMERA, &temp, sizeof(uint8_t));
}
/**
* The function `attitudeCorrection` takes in quaternion, velocity, acceleration, and external data,
* and returns a packed message.
*
* @param quaterion The "quaterion" parameter is a structure of type "AMOV_GIMBAL_QUATERNION_T" which
* contains the following fields:
* @param speed The "speed" parameter is of type `amovGimbal::AMOV_GIMBAL_VELOCITY_T` and represents
* the velocity of the gimbal. It contains three components: `x`, `y`, and `z`, which represent the
* velocity in the respective axes.
* @param acc The "acc" parameter is of type "amovGimbal::AMOV_GIMBAL_VELOCITY_T" and represents the
* acceleration of the gimbal in three dimensions (x, y, z).
* @param extenData The extenData parameter is a void pointer that can be used to pass additional data
* to the attitudeCorrection function. In this case, it is being cast to a float pointer and then
* accessed as an array. The first element of the array is assigned to the temp.yawSetPoint variable,
* and
*
* @return a uint32_t value.
*/
uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATERNION_T &quaterion,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &speed,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData)
{
G1::GIMBAL_ATT_CORR_MSG_T temp;
temp.q[0] = quaterion.q0;
temp.q[1] = quaterion.q1;
temp.q[2] = quaterion.q2;
temp.q[3] = quaterion.q3;
temp.acc[0] = acc.x;
temp.acc[1] = acc.y;
temp.acc[2] = acc.z;
temp.yawSetPoint = ((float *)extenData)[0];
temp.yawSpeedSetPoint = ((float *)extenData)[1];
return pack(G1::GIMBAL_CMD_SET_STATE, reinterpret_cast<uint8_t *>(&temp), sizeof(G1::GIMBAL_ATT_CORR_MSG_T));
}
/**
* The function `attitudeCorrection` calculates the attitude correction for a gimbal based on the given
* position, velocity, and acceleration values.
*
* @param pos The "pos" parameter is of type amovGimbal::AMOV_GIMBAL_POS_T and represents the current
* position of the gimbal. It contains the pitch, roll, and yaw angles of the gimbal.
* @param seppd seppd stands for "Separate Pointing Device" and it represents the velocity of the
* gimbal in terms of pitch, roll, and yaw. It is of type `amovGimbal::AMOV_GIMBAL_VELOCITY_T` which
* likely contains three float values for pitch,
* @param acc The "acc" parameter is of type "amovGimbal::AMOV_GIMBAL_VELOCITY_T" and represents the
* acceleration of the gimbal in three dimensions (x, y, z).
* @param extenData The `extenData` parameter is a void pointer that can be used to pass additional
* data to the `attitudeCorrection` function. In this code snippet, it is assumed that `extenData` is a
* pointer to a float array with two elements.
*
* @return a uint32_t value.
*/
uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData)
{
G1::GIMBAL_ATT_CORR_MSG_T temp;
float pitch = pos.pitch * 0.5f;
float roll = pos.roll * 0.5f;
float yaw = pos.yaw * 0.5f;
temp.q[0] = cosf(pitch) * cosf(roll) * cosf(yaw) +
sinf(pitch) * sinf(roll) * sinf(yaw);
temp.q[1] = cosf(pitch) * sinf(roll) * cosf(yaw) -
sinf(pitch) * cosf(roll) * sinf(yaw);
temp.q[2] = sinf(pitch) * cosf(roll) * cosf(yaw) +
cosf(pitch) * sinf(roll) * sinf(yaw);
temp.q[3] = cosf(pitch) * sinf(roll) * sinf(yaw) -
sinf(pitch) * cosf(roll) * cosf(yaw);
temp.acc[0] = acc.x;
temp.acc[1] = acc.y;
temp.acc[2] = acc.z;
temp.yawSetPoint = ((float *)extenData)[0];
temp.yawSpeedSetPoint = ((float *)extenData)[1];
return pack(G1::GIMBAL_CMD_SET_STATE, reinterpret_cast<uint8_t *>(&temp), sizeof(G1::GIMBAL_ATT_CORR_MSG_T));
}
/**
* The function `extensionFuntions` in the `g1GimbalDriver` class takes a void pointer `cmd`, casts it
* to a `G1::GIMBAL_STD_MSG_T` pointer, and returns the result of calling the `pack` function with the
* `cmd`'s `cmd`, `data`, and `len` members as arguments.
*
* @param cmd The "cmd" parameter is a void pointer, which means it can point to any type of data. In
* this case, it is being cast to a G1::GIMBAL_STD_MSG_T pointer using reinterpret_cast.
*
* @return the result of the `pack` function, which is of type `uint32_t`.
*/
uint32_t g1GimbalDriver::extensionFuntions(void *cmd)
{
G1::GIMBAL_STD_MSG_T *tempCmd;
tempCmd = reinterpret_cast<G1::GIMBAL_STD_MSG_T *>(cmd);
return pack(tempCmd->cmd, tempCmd->data, tempCmd->len);
}

21
gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h
View File

@ -3,8 +3,8 @@
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:07
* @LastEditors: L LC @amov
* @LastEditTime: 2023-03-17 18:12:57
* @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_struct.h
* @LastEditTime: 2023-09-07 10:45:13
* @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_struct.h
*/
#ifndef G1_GIMBAL_STRUCT_H
#define G1_GIMBAL_STRUCT_H
@ -20,6 +20,7 @@ namespace G1
typedef enum
{
GIMBAL_CMD_SET_STATE = 0X01,
GIMBAL_CMD_SET_POS = 0X85,
GIMBAL_CMD_CAMERA = 0X86,
GIMBAL_CMD_RCV_POS = 0X87
@ -85,6 +86,22 @@ namespace G1
int16_t HALL_yaw;
} GIMBAL_RCV_POS_MSG_T;
typedef struct
{
float q[4];
float acc[3];
float yawSetPoint;
float yawSpeedSetPoint;
} GIMBAL_ATT_CORR_MSG_T;
typedef struct
{
uint8_t cmd;
uint8_t data[256];
uint8_t len;
}GIMBAL_STD_MSG_T;
#pragma pack()
}

166
gimbal_ctrl/driver/src/G2/g2_gimbal_crc.h
View File

@ -1,166 +0,0 @@
#ifndef __G2_GIMBAL_CHECK_H
#define __G2_GIMBAL_CHECK_H
namespace G2
{
#include "stdint.h"
const uint16_t crc16_tab[256] = {
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
/**
* "For each byte in the data, shift the CRC register left by 8 bits, XOR the CRC register with the CRC
* table value for the byte, and then shift the CRC register right by 8 bits."
*
* The CRC table is a 256-byte array of 16-bit values. The index into the table is the byte value.
* The value in the table is the CRC value for that byte. The CRC table is generated by the following
* function:
*
* @param data pointer to the data to be checked
* @param len the length of the data to be checked
*
* @return The CRC value.
* @note 16 bit CRC with polynomial x^16+x^12+x^5+1
*/
static inline uint16_t checkCrc16(uint8_t *pData, uint32_t len)
{
uint16_t crc = 0XFFFF;
uint32_t idx = 0;
for (idx = 0; idx < len; idx++)
{
crc = crc16_tab[((crc >> 8) ^ pData[idx]) & 0xFF] ^ (crc << 8);
}
return crc;
}
const unsigned int Crc32Table[256] = {
0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B,
0x1A864DB2, 0x1E475005, 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61,
0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD, 0x4C11DB70, 0x48D0C6C7,
0x4593E01E, 0x4152FDA9, 0x5F15ADAC, 0x5BD4B01B, 0x569796C2, 0x52568B75,
0x6A1936C8, 0x6ED82B7F, 0x639B0DA6, 0x675A1011, 0x791D4014, 0x7DDC5DA3,
0x709F7B7A, 0x745E66CD, 0x9823B6E0, 0x9CE2AB57, 0x91A18D8E, 0x95609039,
0x8B27C03C, 0x8FE6DD8B, 0x82A5FB52, 0x8664E6E5, 0xBE2B5B58, 0xBAEA46EF,
0xB7A96036, 0xB3687D81, 0xAD2F2D84, 0xA9EE3033, 0xA4AD16EA, 0xA06C0B5D,
0xD4326D90, 0xD0F37027, 0xDDB056FE, 0xD9714B49, 0xC7361B4C, 0xC3F706FB,
0xCEB42022, 0xCA753D95, 0xF23A8028, 0xF6FB9D9F, 0xFBB8BB46, 0xFF79A6F1,
0xE13EF6F4, 0xE5FFEB43, 0xE8BCCD9A, 0xEC7DD02D, 0x34867077, 0x30476DC0,
0x3D044B19, 0x39C556AE, 0x278206AB, 0x23431B1C, 0x2E003DC5, 0x2AC12072,
0x128E9DCF, 0x164F8078, 0x1B0CA6A1, 0x1FCDBB16, 0x018AEB13, 0x054BF6A4,
0x0808D07D, 0x0CC9CDCA, 0x7897AB07, 0x7C56B6B0, 0x71159069, 0x75D48DDE,
0x6B93DDDB, 0x6F52C06C, 0x6211E6B5, 0x66D0FB02, 0x5E9F46BF, 0x5A5E5B08,
0x571D7DD1, 0x53DC6066, 0x4D9B3063, 0x495A2DD4, 0x44190B0D, 0x40D816BA,
0xACA5C697, 0xA864DB20, 0xA527FDF9, 0xA1E6E04E, 0xBFA1B04B, 0xBB60ADFC,
0xB6238B25, 0xB2E29692, 0x8AAD2B2F, 0x8E6C3698, 0x832F1041, 0x87EE0DF6,
0x99A95DF3, 0x9D684044, 0x902B669D, 0x94EA7B2A, 0xE0B41DE7, 0xE4750050,
0xE9362689, 0xEDF73B3E, 0xF3B06B3B, 0xF771768C, 0xFA325055, 0xFEF34DE2,
0xC6BCF05F, 0xC27DEDE8, 0xCF3ECB31, 0xCBFFD686, 0xD5B88683, 0xD1799B34,
0xDC3ABDED, 0xD8FBA05A, 0x690CE0EE, 0x6DCDFD59, 0x608EDB80, 0x644FC637,
0x7A089632, 0x7EC98B85, 0x738AAD5C, 0x774BB0EB, 0x4F040D56, 0x4BC510E1,
0x46863638, 0x42472B8F, 0x5C007B8A, 0x58C1663D, 0x558240E4, 0x51435D53,
0x251D3B9E, 0x21DC2629, 0x2C9F00F0, 0x285E1D47, 0x36194D42, 0x32D850F5,
0x3F9B762C, 0x3B5A6B9B, 0x0315D626, 0x07D4CB91, 0x0A97ED48, 0x0E56F0FF,
0x1011A0FA, 0x14D0BD4D, 0x19939B94, 0x1D528623, 0xF12F560E, 0xF5EE4BB9,
0xF8AD6D60, 0xFC6C70D7, 0xE22B20D2, 0xE6EA3D65, 0xEBA91BBC, 0xEF68060B,
0xD727BBB6, 0xD3E6A601, 0xDEA580D8, 0xDA649D6F, 0xC423CD6A, 0xC0E2D0DD,
0xCDA1F604, 0xC960EBB3, 0xBD3E8D7E, 0xB9FF90C9, 0xB4BCB610, 0xB07DABA7,
0xAE3AFBA2, 0xAAFBE615, 0xA7B8C0CC, 0xA379DD7B, 0x9B3660C6, 0x9FF77D71,
0x92B45BA8, 0x9675461F, 0x8832161A, 0x8CF30BAD, 0x81B02D74, 0x857130C3,
0x5D8A9099, 0x594B8D2E, 0x5408ABF7, 0x50C9B640, 0x4E8EE645, 0x4A4FFBF2,
0x470CDD2B, 0x43CDC09C, 0x7B827D21, 0x7F436096, 0x7200464F, 0x76C15BF8,
0x68860BFD, 0x6C47164A, 0x61043093, 0x65C52D24, 0x119B4BE9, 0x155A565E,
0x18197087, 0x1CD86D30, 0x029F3D35, 0x065E2082, 0x0B1D065B, 0x0FDC1BEC,
0x3793A651, 0x3352BBE6, 0x3E119D3F, 0x3AD08088, 0x2497D08D, 0x2056CD3A,
0x2D15EBE3, 0x29D4F654, 0xC5A92679, 0xC1683BCE, 0xCC2B1D17, 0xC8EA00A0,
0xD6AD50A5, 0xD26C4D12, 0xDF2F6BCB, 0xDBEE767C, 0xE3A1CBC1, 0xE760D676,
0xEA23F0AF, 0xEEE2ED18, 0xF0A5BD1D, 0xF464A0AA, 0xF9278673, 0xFDE69BC4,
0x89B8FD09, 0x8D79E0BE, 0x803AC667, 0x84FBDBD0, 0x9ABC8BD5, 0x9E7D9662,
0x933EB0BB, 0x97FFAD0C, 0xAFB010B1, 0xAB710D06, 0xA6322BDF, 0xA2F33668,
0xBCB4666D, 0xB8757BDA, 0xB5365D03, 0xB1F740B4};
/**
* For each byte in the input data, XOR the current CRC value with the byte, then shift the CRC value
* left 8 bits, and XOR the CRC value with the CRC table value for the byte
*
* @param pData pointer to the data to be CRC'd
* @param Length The length of the data to be CRC'd.
*
* @return The CRC32 value of the data.
*/
static inline uint32_t checkCRC32(uint8_t *pData, uint32_t Length)
{
unsigned int nReg;
unsigned int nTemp = 0;
unsigned short i, n;
nReg = 0xFFFFFFFF;
for (n = 0; n < Length; n++)
{
nReg ^= (unsigned int)pData[n];
for (i = 0; i < 4; i++)
{
nTemp = Crc32Table[(unsigned char)((nReg >> 24) & 0xff)];
nReg <<= 8;
nReg ^= nTemp;
}
}
return nReg;
}
/**
* It takes a pointer to an array of bytes and the length of the array, and returns the sum of the
* bytes in the array
*
* @param pData The data to be calculated
* @param Lenght The length of the data to be sent.
*
* @return The sum of the bytes in the array.
*/
static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght)
{
unsigned short temp = 0;
unsigned short i = 0;
for (i = 0; i < Lenght; i++)
{
temp += pData[i];
}
return temp & 0XFF;
}
}
#endif

243
gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp
View File

@ -1,243 +0,0 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-03-01 10:12:58
* @LastEditors: L LC @amov
* @LastEditTime: 2023-04-11 17:33:42
* @FilePath: /gimbal-sdk-multi-platform/src/G2/g2_gimbal_driver.cpp
*/
#include "g2_gimbal_driver.h"
#include "g2_gimbal_crc.h"
#include "string.h"
/**
* The function creates a new instance of the g2GimbalDriver class, which is a subclass of the
* IamovGimbalBase class
*
* @param _IO The IOStreamBase class that is used to communicate with the gimbal.
*/
g2GimbalDriver::g2GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO)
{
memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T));
memset(&txQueue, 0, sizeof(RING_FIFO_CB_T));
rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T));
if (rxBuffer == NULL)
{
std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
exit(1);
}
txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T));
if (txBuffer == NULL)
{
free(rxBuffer);
std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
exit(1);
}
Ring_Fifo_init(&rxQueue, sizeof(G2::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T));
Ring_Fifo_init(&txQueue, sizeof(G2::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T));
parserState = G2::GIMBAL_SERIAL_STATE_IDEL;
}
/**
* It takes a command, a pointer to a payload, and the size of the payload, and then it puts the
* command, the payload, and the CRC into a ring buffer
*
* @param uint32_t 4 bytes
* @param pPayload pointer to the data to be sent
* @param payloadSize the size of the payload in bytes
*
* @return The number of bytes in the packet.
*/
uint32_t g2GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize)
{
uint32_t ret = 0;
G2::GIMBAL_FRAME_T txTemp;
txTemp.head = G2_SERIAL_HEAD;
txTemp.version = G2_SERIAL_VERSION;
txTemp.len = payloadSize;
txTemp.command = cmd;
txTemp.source = self;
txTemp.target = remote;
memcpy(txTemp.data, pPayload, payloadSize);
txTemp.crc.f16 = G2::checkCrc16((uint8_t *)&txTemp, txTemp.len + G2_PAYLOAD_OFFSET);
memcpy(txTemp.data + payloadSize, txTemp.crc.f8, sizeof(uint16_t));
txMutex.lock();
if (Ring_Fifo_in_cell(&txQueue, &txTemp))
{
ret = txTemp.len + G2_PAYLOAD_OFFSET + sizeof(uint16_t);
}
txMutex.unlock();
return ret;
}
/**
* > This function is used to get a packet from the receive queue
*
* @param void This is the type of data that will be stored in the queue.
*
* @return A boolean value.
*/
bool g2GimbalDriver::getRxPack(OUT void *pack)
{
bool state = false;
rxMutex.lock();
state = Ring_Fifo_out_cell(&rxQueue, pack);
rxMutex.unlock();
return state;
}
/**
* The function takes a pointer to a buffer, casts it to a pointer to a G2::GIMBAL_FRAME_T, and then
* checks the command field of the frame. If the command is G2::IAP_COMMAND_BLOCK_END, it locks the
* mutex, and then unlocks it. Otherwise, it prints out the contents of the buffer
*
* @param buf pointer to the data received from the gimbal
*/
void g2GimbalDriver::convert(void *buf)
{
G2::GIMBAL_FRAME_T *temp;
temp = reinterpret_cast<G2::GIMBAL_FRAME_T *>(buf);
switch (temp->command)
{
case G2::IAP_COMMAND_BLOCK_END:
mState.lock();
updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
state.abs.roll, state.abs.pitch, state.abs.yaw,
state.fov.x, state.fov.y);
mState.unlock();
break;
default:
std::cout << "Undefined frame from G2 : ";
for (uint16_t i = 0; i < temp->len + G2_PAYLOAD_OFFSET + sizeof(uint32_t); i++)
{
printf("%02X ", ((uint8_t *)buf)[i]);
}
std::cout << std::endl;
break;
}
}
/**
* If the serial port is not busy and is open, then lock the txMutex, get the next byte from the
* txQueue, unlock the txMutex, and send the byte
*/
void g2GimbalDriver::send(void)
{
if (!IO->isBusy() && IO->isOpen())
{
bool state = false;
txMutex.lock();
state = Ring_Fifo_out_cell(&txQueue, &tx);
txMutex.unlock();
if (state)
{
IO->outPutBytes((uint8_t *)&tx, tx.len + G2_PAYLOAD_OFFSET + sizeof(uint16_t));
}
}
}
/**
* The function is called every time a byte is received from the serial port. It parses the byte and
* stores it in a buffer. When the buffer is full, it checks the CRC and if it's correct, it stores the
* buffer in a queue
*
* @param uint8_t unsigned char
*
* @return The parser function is returning a boolean value.
*/
bool g2GimbalDriver::parser(IN uint8_t byte)
{
bool state = false;
static uint8_t payloadLenghte = 0;
static uint8_t *pRx = NULL;
switch (parserState)
{
case G2::GIMBAL_SERIAL_STATE_IDEL:
if (byte == G2_SERIAL_HEAD)
{
rx.head = byte;
parserState = G2::GIMBAL_SERIAL_STATE_HEAD_RCV;
}
break;
case G2::GIMBAL_SERIAL_STATE_HEAD_RCV:
if (byte == G2_SERIAL_VERSION)
{
rx.version = byte;
parserState = G2::GIMBAL_SERIAL_STATE_VERSION_RCV;
}
else
{
rx.head = 0;
parserState = G2::GIMBAL_SERIAL_STATE_IDEL;
}
break;
case G2::GIMBAL_SERIAL_STATE_VERSION_RCV:
rx.target = byte;
parserState = G2::GIMBAL_SERIAL_STATE_TARGET_RCV;
break;
case G2::GIMBAL_SERIAL_STATE_TARGET_RCV:
rx.source = byte;
parserState = G2::GIMBAL_SERIAL_STATE_SOURCE_RCV;
break;
case G2::GIMBAL_SERIAL_STATE_SOURCE_RCV:
rx.len = byte;
parserState = G2::GIMBAL_SERIAL_STATE_LENGHT_RCV;
pRx = rx.data;
payloadLenghte = byte;
break;
case G2::GIMBAL_SERIAL_STATE_LENGHT_RCV:
rx.command = byte;
parserState = G2::GIMBAL_SERIAL_STATE_DATA_RCV;
break;
case G2::GIMBAL_SERIAL_STATE_DATA_RCV:
*pRx = byte;
payloadLenghte--;
if (payloadLenghte == 0)
{
parserState = G2::GIMBAL_SERIAL_STATE_CRC_RCV1;
}
break;
case G2::GIMBAL_SERIAL_STATE_CRC_RCV1:
rx.crc.f8[1] = byte;
parserState = G2::GIMBAL_SERIAL_STATE_END;
break;
case G2::GIMBAL_SERIAL_STATE_END:
rx.crc.f8[0] = byte;
if (rx.crc.f16 == G2::checkCrc16((uint8_t *)&rx, G2_PAYLOAD_OFFSET + rx.len))
{
state = true;
rxMutex.lock();
Ring_Fifo_in_cell(&rxQueue, &rx);
rxMutex.unlock();
}
else
{
memset(&rx, 0, sizeof(G2::GIMBAL_FRAME_T));
}
parserState = G2::GIMBAL_SERIAL_STATE_IDEL;
break;
default:
parserState = G2::GIMBAL_SERIAL_STATE_IDEL;
break;
}
return state;
}

76
gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h
View File

@ -1,76 +0,0 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-03-01 10:02:24
* @LastEditors: L LC @amov
* @LastEditTime: 2023-03-13 12:29:33
* @FilePath: \gimbal-sdk-multi-platform\src\G2\g2_gimbal_driver.h
*/
#include "../amov_gimbal.h"
#include "g2_gimbal_struct.h"
#include <mutex>
#include <malloc.h>
#include <iostream>
#ifndef __G2_DRIVER_H
#define __G2_DRIVER_H
extern "C"
{
#include "Ring_Fifo.h"
}
class g2GimbalDriver : protected amovGimbal::IamovGimbalBase
{
private:
G2::GIMBAL_CMD_PARSER_STATE_T parserState;
G2::GIMBAL_FRAME_T rx;
G2::GIMBAL_FRAME_T tx;
std::mutex rxMutex;
uint8_t *rxBuffer;
RING_FIFO_CB_T rxQueue;
std::mutex txMutex;
uint8_t *txBuffer;
RING_FIFO_CB_T txQueue;
uint8_t self;
uint8_t remote;
bool parser(IN uint8_t byte);
void send(void);
void convert(void *buf);
uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
bool getRxPack(OUT void *pack);
public:
void nodeSet(SET uint32_t _self, SET uint32_t _remote)
{
self = _self;
remote = _remote;
}
// funtion
uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed);
uint32_t setGimabalHome(void);
uint32_t takePic(void);
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
{
return new g2GimbalDriver(_IO);
}
g2GimbalDriver(amovGimbal::IOStreamBase *_IO);
~g2GimbalDriver()
{
free(rxBuffer);
free(txBuffer);
}
};
#endif

81
gimbal_ctrl/driver/src/G2/g2_gimbal_funtion.cpp
View File

@ -1,81 +0,0 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-03-13 11:58:54
* @LastEditors: L LC @amov
* @LastEditTime: 2023-03-13 12:31:58
* @FilePath: \gimbal-sdk-multi-platform\src\G2\g2_gimbal_funtion.cpp
*/
#include "g2_gimbal_driver.h"
#include "g2_gimbal_crc.h"
#include "string.h"
/**
* It sets the gimbal position.
*
* @param pos the position of the gimbal
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t g2GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
{
return 0;
}
/**
* It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type
* G1::GIMBAL_SET_POS_MSG_T
*
* @param speed the speed of the gimbal
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t g2GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
{
return 0;
}
/**
* This function sets the gimbal's follow speed
*
* @param followSpeed the speed of the gimbal
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t g2GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
{
return 0;
}
/**
* This function sets the gimbal to its home position
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t g2GimbalDriver::setGimabalHome(void)
{
return 0;
}
/**
* It takes a picture.
*
* @return The return value is the number of bytes written to the serial port.
*/
uint32_t g2GimbalDriver::takePic(void)
{
return 0;
}
/**
* The function sets the video state of the gimbal
*
* @param newState The new state of the video.
*
* @return The return value is the number of bytes written to the serial port.
*/
uint32_t g2GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
{
return 0;
}

81
gimbal_ctrl/driver/src/G2/g2_gimbal_struct.h
View File

@ -1,81 +0,0 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-03-01 09:21:57
* @LastEditors: L LC @amov
* @LastEditTime: 2023-04-18 10:13:23
* @FilePath: /gimbal-sdk-multi-platform/src/G2/g2_gimbal_struct.h
*/
#ifndef G2_GIMBAL_STRUCT_H
#define G2_GIMBAL_STRUCT_H
#include <stdint.h>
namespace G2
{
#define G2_MAX_GIMBAL_PAYLOAD 64
#define G2_PAYLOAD_OFFSET 6
#define G2_SCALE_FACTOR 0.01f
#define G2_SERIAL_HEAD 0XAF
#define G2_SERIAL_VERSION 0X02
typedef enum
{
IAP_COMMAND_JUMP = 80,
IAP_COMMAND_FLASH_ERASE,
IAP_COMMAND_BOLCK_INFO,
IAP_COMMAND_BLOCK_WRITE,
IAP_COMMAND_SOFT_INFO,
IAP_COMMAND_HARDWARE_INFO,
IAP_COMMAND_BLOCK_START,
IAP_COMMAND_BLOCK_END = 117,
} GIMBAL_CMD_T;
typedef enum
{
IAP_STATE_FAILD = 0,
IAP_STATE_SUCCEED,
IAP_STATE_READY,
IAP_STATE_FIRMWARE_BROKE,
IAP_STATE_JUMP_FAILD,
IAP_STATE_ADDR_ERR,
IAP_STATE_CRC_ERR,
IAP_STATE_WRITE_ERR,
IAP_STATE_WRITE_TIMEOUT,
} GIMBAL_IAP_STATE_T;
typedef enum
{
GIMBAL_SERIAL_STATE_IDEL = 0,
GIMBAL_SERIAL_STATE_HEAD_RCV,
GIMBAL_SERIAL_STATE_VERSION_RCV,
GIMBAL_SERIAL_STATE_TARGET_RCV,
GIMBAL_SERIAL_STATE_SOURCE_RCV,
GIMBAL_SERIAL_STATE_LENGHT_RCV,
GIMBAL_SERIAL_STATE_DATA_RCV,
GIMBAL_SERIAL_STATE_CRC_RCV1,
GIMBAL_SERIAL_STATE_END,
} GIMBAL_CMD_PARSER_STATE_T;
#pragma pack(1)
typedef struct
{
uint8_t head;
uint8_t version;
uint8_t target;
uint8_t source;
uint8_t len;
uint8_t command;
uint8_t data[G2_MAX_GIMBAL_PAYLOAD];
union
{
uint8_t f8[2];
uint16_t f16;
} crc;
} GIMBAL_FRAME_T;
#pragma pack(0)
}
#endif

78
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov
* @LastEditTime: 2023-04-11 17:29:58
* @LastEditTime: 2023-07-24 12:03:44
* @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_driver.cpp
*/
#include "Q10f_gimbal_driver.h"
@ -16,27 +16,10 @@
*
* @param _IO The IOStreamBase object that will be used to communicate with the gimbal.
*/
Q10fGimbalDriver::Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO)
Q10fGimbalDriver::Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO)
{
memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T));
memset(&txQueue, 0, sizeof(RING_FIFO_CB_T));
rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T));
if (rxBuffer == NULL)
{
std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
exit(1);
}
txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T));
if (txBuffer == NULL)
{
free(rxBuffer);
std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
exit(1);
}
Ring_Fifo_init(&rxQueue, sizeof(Q10f::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T));
Ring_Fifo_init(&txQueue, sizeof(Q10f::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T));
rxQueue = new fifoRing(sizeof(Q10f::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
txQueue = new fifoRing(sizeof(Q10f::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
parserState = Q10f::GIMBAL_SERIAL_STATE_IDLE;
@ -77,32 +60,14 @@ uint32_t Q10fGimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payl
}
txTemp.len = payloadSize;
txMutex.lock();
if (Ring_Fifo_in_cell(&txQueue, &txTemp))
if (txQueue->inCell(&txTemp))
{
ret = payloadSize + sizeof(uint32_t) + sizeof(uint8_t);
}
txMutex.unlock();
return ret;
}
/**
* > This function is used to get a packet from the receive queue
*
* @param void This is the type of data that will be stored in the queue.
*
* @return A boolean value.
*/
bool Q10fGimbalDriver::getRxPack(OUT void *pack)
{
bool state = false;
rxMutex.lock();
state = Ring_Fifo_out_cell(&rxQueue, pack);
rxMutex.unlock();
return state;
}
void Q10fGimbalDriver::convert(void *buf)
{
Q10f::GIMBAL_FRAME_T *temp;
@ -116,7 +81,7 @@ void Q10fGimbalDriver::convert(void *buf)
state.abs.yaw = tempPos->yawIMUAngle * Q10F_SCALE_FACTOR_ANGLE;
state.abs.roll = tempPos->rollIMUAngle * Q10F_SCALE_FACTOR_ANGLE;
state.abs.pitch = tempPos->pitchIMUAngle * Q10F_SCALE_FACTOR_ANGLE;
state.rel.yaw = tempPos->rollStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
state.rel.yaw = tempPos->yawStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
state.rel.roll = tempPos->rollStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
state.rel.pitch = tempPos->pitchStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
@ -136,32 +101,9 @@ void Q10fGimbalDriver::convert(void *buf)
}
}
/**
* The function is called by the main thread to send a command to the gimbal.
*
* The function first checks to see if the serial port is busy and if it is open. If it is not busy and
* it is open, the function locks the txMutex and then checks to see if there is a command in the
* txQueue. If there is a command in the txQueue, the function copies the command to the tx buffer and
* then unlocks the txMutex. The function then sends the command to the gimbal.
*
* The txQueue is a ring buffer that holds commands that are waiting to be sent to the gimbal. The
* txQueue is a ring buffer because the gimbal can only process one command at a time. If the gimbal is
* busy processing a command, the command will be placed in the txQueue and sent to the gimbal when the
* gimbal is ready to receive the command.
*/
void Q10fGimbalDriver::send(void)
uint32_t Q10fGimbalDriver::calPackLen(void *pack)
{
if (!IO->isBusy() && IO->isOpen())
{
bool state = false;
txMutex.lock();
state = Ring_Fifo_out_cell(&txQueue, &tx);
txMutex.unlock();
if (state)
{
IO->outPutBytes((uint8_t *)&tx, tx.len + Q10F_PAYLOAD_OFFSET + sizeof(uint8_t));
}
}
return ((Q10f::GIMBAL_FRAME_T *)pack)->len + Q10F_PAYLOAD_OFFSET + sizeof(uint8_t);
}
/**
@ -238,9 +180,7 @@ bool Q10fGimbalDriver::parser(IN uint8_t byte)
if (byte == suncheck)
{
state = true;
rxMutex.lock();
Ring_Fifo_in_cell(&rxQueue, &rx);
rxMutex.unlock();
rxQueue->inCell(&rx);
}
else
{

27
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h
View File

@ -7,6 +7,7 @@
* @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_driver.h
*/
#include "../amov_gimbal.h"
#include "../amov_gimbal_private.h"
#include "Q10f_gimbal_struct.h"
#include <mutex>
#include <malloc.h>
@ -15,31 +16,16 @@
#ifndef __Q10F_DRIVER_H
#define __Q10F_DRIVER_H
extern "C"
{
#include "Ring_Fifo.h"
}
class Q10fGimbalDriver : protected amovGimbal::IamovGimbalBase
class Q10fGimbalDriver : protected amovGimbal::amovGimbalBase
{
private:
Q10f::GIMBAL_SERIAL_STATE_T parserState;
Q10f::GIMBAL_FRAME_T rx;
Q10f::GIMBAL_FRAME_T tx;
std::mutex rxMutex;
uint8_t *rxBuffer;
RING_FIFO_CB_T rxQueue;
std::mutex txMutex;
uint8_t *txBuffer;
RING_FIFO_CB_T txQueue;
bool parser(IN uint8_t byte);
void send(void);
void convert(void *buf);
uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
bool getRxPack(OUT void *pack);
uint32_t calPackLen(void *pack);
public:
// funtions
@ -55,17 +41,12 @@ public:
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
// builds
static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
{
return new Q10fGimbalDriver(_IO);
}
Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO);
~Q10fGimbalDriver()
{
free(rxBuffer);
free(txBuffer);
}
};
#endif

2
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2023-03-02 10:00:52
* @LastEditors: L LC @amov
* @LastEditTime: 2023-03-29 11:47:18
* @LastEditTime: 2023-07-24 14:22:57
* @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_funtion.cpp
*/
#include "Q10f_gimbal_driver.h"

2
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:07
* @LastEditors: L LC @amov
* @LastEditTime: 2023-03-28 18:15:47
* @LastEditTime: 2023-07-24 11:51:59
* @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_struct.h
*/
#ifndef Q10F_GIMBAL_STRUCT_H

163
gimbal_ctrl/driver/src/amov_gimabl.cpp → gimbal_ctrl/driver/src/amov_gimbal.cpp
View File

@ -3,14 +3,15 @@
* @Author: L LC @amov
* @Date: 2022-10-28 11:54:11
* @LastEditors: L LC @amov
* @LastEditTime: 2023-04-11 18:13:25
* @FilePath: /gimbal-sdk-multi-platform/src/amov_gimabl.cpp
* @LastEditTime: 2023-08-17 11:57:11
* @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal.cpp
*/
#include "amov_gimbal.h"
// #include "amov_gimbal.h"
#include "amov_gimbal_private.h"
#include "g1_gimbal_driver.h"
#include "g2_gimbal_driver.h"
#include "Q10f_gimbal_driver.h"
#include "AT10_gimbal_driver.h"
#include <iostream>
#include <thread>
@ -24,23 +25,24 @@ typedef enum
AMOV_GIMBAL_TYPE_G1 = 1,
AMOV_GIMBAL_TYPE_G2,
AMOV_GIMBAL_TYPE_Q10,
AMOV_GIMBAL_TYPE_AT10,
} AMOV_GIMBAL_TYPE_T;
namespace amovGimbal
{
typedef amovGimbal::IamovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO);
typedef amovGimbal::amovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO);
typedef std::map<std::string, createCallback> callbackMap;
std::map<std::string, AMOV_GIMBAL_TYPE_T> amovGimbalTypeList =
{
{"G1", AMOV_GIMBAL_TYPE_G1},
{"G2", AMOV_GIMBAL_TYPE_G2},
{"Q10f", AMOV_GIMBAL_TYPE_Q10}};
{"Q10f", AMOV_GIMBAL_TYPE_Q10},
{"AT10", AMOV_GIMBAL_TYPE_AT10}};
callbackMap amovGimbals =
{
{"G1", g1GimbalDriver::creat},
{"G2", g2GimbalDriver::creat},
{"Q10f", Q10fGimbalDriver::creat}};
{"Q10f", Q10fGimbalDriver::creat},
{"AT10", AT10GimbalDriver::creat}};
}
/* The amovGimbalCreator class is a factory class that creates an instance of the amovGimbal class */
@ -48,7 +50,7 @@ namespace amovGimbal
class amovGimbalCreator
{
public:
static amovGimbal::IamovGimbalBase *createAmovGimbal(const std::string &type, amovGimbal::IOStreamBase *_IO)
static amovGimbal::amovGimbalBase *createAmovGimbal(const std::string &type, amovGimbal::IOStreamBase *_IO)
{
amovGimbal::callbackMap::iterator temp = amovGimbal::amovGimbals.find(type);
@ -77,13 +79,71 @@ private:
~amovGimbalCreator();
};
/**
* This is a constructor for the amovGimbalBase class that initializes its parent classes with an
* IOStreamBase object.
*
* @param _IO _IO is a pointer to an object of type amovGimbal::IOStreamBase, which is the base class
* for input/output streams used by the amovGimbal class. This parameter is passed to the constructor
* of amovGimbalBase, which is a derived class of I
*/
amovGimbal::amovGimbalBase::amovGimbalBase(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(), amovGimbal::PamovGimbalBase(_IO)
{
}
/**
* The function is a destructor that sleeps for 50 milliseconds and closes an IO object.
*/
amovGimbal::amovGimbalBase::~amovGimbalBase()
{
std::this_thread::sleep_for(std::chrono::milliseconds(50));
IO->close();
}
/**
* This function retrieves a packet from a ring buffer queue and returns a boolean value indicating
* whether the operation was successful or not.
*
* @param void void is a keyword in C++ that represents the absence of a type. In this function, it is
* used to indicate that the function does not return any value.
*
* @return a boolean value, which indicates whether or not a data packet was successfully retrieved
* from a ring buffer queue.
*/
bool amovGimbal::amovGimbalBase::getRxPack(OUT void *pack)
{
bool state = false;
state = rxQueue->outCell(pack);
return state;
}
/**
* This function sends data from a buffer to an output device if it is not busy and open.
*/
void amovGimbal::amovGimbalBase::send(void)
{
uint8_t tempBuffer[MAX_PACK_SIZE];
if (!IO->isBusy() && IO->isOpen())
{
bool state = false;
state = txQueue->outCell(&tempBuffer);
if (state)
{
IO->outPutBytes((uint8_t *)&tempBuffer, calPackLen(tempBuffer));
}
}
}
/**
* "If the input byte is available, then parse it."
*
* The function is a loop that runs forever. It calls the IO->inPutByte() function to get a byte from
* the serial port. If the byte is available, then it calls the parser() function to parse the byte
*/
void amovGimbal::IamovGimbalBase::parserLoop(void)
void amovGimbal::amovGimbalBase::parserLoop(void)
{
uint8_t temp;
@ -96,7 +156,7 @@ void amovGimbal::IamovGimbalBase::parserLoop(void)
}
}
void amovGimbal::IamovGimbalBase::sendLoop(void)
void amovGimbal::amovGimbalBase::sendLoop(void)
{
while (1)
{
@ -104,7 +164,7 @@ void amovGimbal::IamovGimbalBase::sendLoop(void)
}
}
void amovGimbal::IamovGimbalBase::mainLoop(void)
void amovGimbal::amovGimbalBase::mainLoop(void)
{
uint8_t tempBuffer[MAX_PACK_SIZE];
@ -112,52 +172,66 @@ void amovGimbal::IamovGimbalBase::mainLoop(void)
{
if (getRxPack(tempBuffer))
{
msgCustomCallback(tempBuffer);
convert(tempBuffer);
}
}
}
void amovGimbal::amovGimbalBase::stackStart(void)
{
if (!this->IO->isOpen())
{
this->IO->open();
}
std::thread parserLoop(&amovGimbalBase::parserLoop, this);
std::thread sendLoop(&amovGimbalBase::sendLoop, this);
parserLoop.detach();
sendLoop.detach();
}
/**
* It starts two threads, one for reading data from the serial port and one for sending data to the
* serial port
*/
void amovGimbal::IamovGimbalBase::startStack(void)
void amovGimbal::gimbal::startStack(void)
{
if (!IO->isOpen())
{
IO->open();
}
std::thread mainLoop(&IamovGimbalBase::parserLoop, this);
std::thread sendLoop(&IamovGimbalBase::sendLoop, this);
mainLoop.detach();
sendLoop.detach();
((amovGimbalBase *)(this->dev))->stackStart();
}
void amovGimbal::gimbal::setParserCallback(amovGimbal::pStateInvoke callback)
{
((amovGimbalBase *)(this->dev))->updateGimbalStateCallback = callback;
}
void amovGimbal::amovGimbalBase::parserStart(pStateInvoke callback)
{
this->updateGimbalStateCallback = callback;
std::thread mainLoop(&amovGimbalBase::mainLoop, this);
mainLoop.detach();
}
/**
* The function creates a thread that runs the mainLoop function
*/
void amovGimbal::IamovGimbalBase::parserAuto(pStateInvoke callback)
void amovGimbal::gimbal::parserAuto(pStateInvoke callback)
{
this->updateGimbalStateCallback = callback;
std::thread mainLoop(&IamovGimbalBase::mainLoop, this);
mainLoop.detach();
((amovGimbalBase *)(this->dev))->parserStart(callback);
}
amovGimbal::AMOV_GIMBAL_STATE_T amovGimbal::IamovGimbalBase::getGimabalState(void)
void amovGimbal::gimbal::setMsgCallback(pMsgInvoke callback)
{
mState.lock();
AMOV_GIMBAL_STATE_T temp = state;
mState.unlock();
((amovGimbalBase *)(this->dev))->msgCustomCallback = callback;
}
amovGimbal::AMOV_GIMBAL_STATE_T amovGimbal::gimbal::getGimabalState(void)
{
((amovGimbalBase *)(this->dev))->mState.lock();
AMOV_GIMBAL_STATE_T temp = ((amovGimbalBase *)(this->dev))->state;
((amovGimbalBase *)(this->dev))->mState.unlock();
return temp;
}
amovGimbal::IamovGimbalBase::~IamovGimbalBase()
{
std::this_thread::sleep_for(std::chrono::milliseconds(50));
IO->close();
}
/**
* Default implementation of interface functions, not pure virtual functions for ease of extension.
*/
@ -211,6 +285,21 @@ uint32_t amovGimbal::IamovGimbalBase::setVideo(const amovGimbal::AMOV_GIMBAL_VID
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, const AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::extensionFuntions(void *cmd)
{
return 0;
}
/**
* The function creates a new gimbal object, which is a pointer to a new amovGimbal object, which is a
* pointer to a new Gimbal object, which is a pointer to a new IOStreamBase object

65
gimbal_ctrl/driver/src/amov_gimbal.h
View File

@ -3,8 +3,8 @@
* @Author: L LC @amov
* @Date: 2022-10-27 18:34:26
* @LastEditors: L LC @amov
* @LastEditTime: 2023-04-18 11:42:05
* @FilePath: /spirecv-gimbal-sdk/gimbal_ctrl/driver/src/amov_gimbal.h
* @LastEditTime: 2023-08-16 22:21:28
* @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal.h
*/
#ifndef AMOV_GIMBAL_H
@ -14,14 +14,9 @@
#include <stdbool.h>
#include <iostream>
#include <thread>
#include <unistd.h>
#include <mutex>
#include "amov_gimbal_struct.h"
#define MAX_QUEUE_SIZE 50
#define MAX_QUEUE_SIZE 100
namespace amovGimbal
{
#define IN
@ -33,6 +28,10 @@ namespace amovGimbal
double &fovX, double &fovY)
{
}
static inline void idleMsgCallback(void *)
{
}
// Control data input and output
class IOStreamBase
@ -50,43 +49,14 @@ namespace amovGimbal
virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) = 0;
};
// Device interface
class IamovGimbalBase
{
protected:
AMOV_GIMBAL_STATE_T state;
std::mutex mState;
IOStreamBase *IO;
pStateInvoke updateGimbalStateCallback;
virtual bool parser(IN uint8_t byte) = 0;
virtual void send(void) = 0;
virtual void convert(void *buf) = 0;
virtual uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize) = 0;
virtual bool getRxPack(OUT void *pack) = 0;
void parserLoop(void);
void sendLoop(void);
void mainLoop(void);
public:
IamovGimbalBase(SET IOStreamBase *_IO)
{
IO = _IO;
}
virtual ~IamovGimbalBase();
void setParserCallback(pStateInvoke callback)
{
this->updateGimbalStateCallback = callback;
}
// Protocol stack function items
virtual void startStack(void);
virtual void parserAuto(pStateInvoke callback = idleCallback);
virtual void nodeSet(SET uint32_t _self, SET uint32_t _remote);
IamovGimbalBase() {}
virtual ~IamovGimbalBase() {}
// functions
virtual AMOV_GIMBAL_STATE_T getGimabalState(void);
virtual void nodeSet(SET uint32_t _self, SET uint32_t _remote);
virtual uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
virtual uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
virtual uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
@ -96,6 +66,9 @@ namespace amovGimbal
virtual uint32_t setGimbalROI(const AMOV_GIMBAL_ROI_T area);
virtual uint32_t takePic(void);
virtual uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, const AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
virtual uint32_t extensionFuntions(void *cmd);
};
class gimbal
@ -105,7 +78,17 @@ namespace amovGimbal
IOStreamBase *IO;
public:
// Instantiated device handle
IamovGimbalBase *dev;
// Protocol stack function items
void startStack(void);
void parserAuto(pStateInvoke callback = idleCallback);
void setParserCallback(pStateInvoke callback);
void setMsgCallback(pMsgInvoke callback);
AMOV_GIMBAL_STATE_T getGimabalState(void);
std::string name()
{
return typeName;

79
gimbal_ctrl/driver/src/amov_gimbal_private.h Normal file
View File

@ -0,0 +1,79 @@
/*
* @Description :
* @Author : Aiyangsky
* @Date : 2023-05-13 10:39:20
* @LastEditors: L LC @amov
* @LastEditTime: 2023-08-16 22:30:53
* @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal_private.h
*/
#ifndef __AMOV_GIMABL_PRIVATE_H
#define __AMOV_GIMABL_PRIVATE_H
#include <stdint.h>
#include <stdbool.h>
#include <iostream>
#include <thread>
#include <unistd.h>
#include <mutex>
#include "amov_gimbal.h"
#include "Ring_Fifo.h"
#include "amov_tool.h"
namespace amovGimbal
{
class PamovGimbalBase
{
public:
AMOV_GIMBAL_STATE_T state;
std::mutex mState;
IOStreamBase *IO;
pStateInvoke updateGimbalStateCallback;
pMsgInvoke msgCustomCallback = idleMsgCallback;
fifoRing *rxQueue;
fifoRing *txQueue;
PamovGimbalBase(SET IOStreamBase *_IO)
{
IO = _IO;
}
virtual ~PamovGimbalBase()
{
if (txQueue != nullptr)
{
delete txQueue;
}
if (rxQueue != nullptr)
{
delete rxQueue;
}
}
};
class amovGimbalBase : public IamovGimbalBase, public PamovGimbalBase
{
public:
virtual uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize) = 0;
virtual bool parser(IN uint8_t byte) = 0;
virtual void convert(void *buf) = 0;
virtual uint32_t calPackLen(void *pack) = 0;
virtual void send(void);
virtual bool getRxPack(OUT void *pack);
virtual void parserLoop(void);
virtual void sendLoop(void);
virtual void mainLoop(void);
virtual void stackStart(void);
virtual void parserStart(amovGimbal::pStateInvoke callback);
public:
amovGimbalBase(IOStreamBase *_IO);
virtual ~amovGimbalBase();
};
}
#endif

21
gimbal_ctrl/driver/src/amov_gimbal_struct.h
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2022-10-31 11:56:43
* @LastEditors: L LC @amov
* @LastEditTime: 2023-04-18 10:12:33
* @LastEditTime: 2023-08-16 22:21:46
* @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal_struct.h
*/
@ -17,6 +17,7 @@ namespace amovGimbal
typedef void (*pStateInvoke)(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
double &fovX, double &fovY);
typedef void (*pMsgInvoke)(void *msg);
typedef enum
{
@ -48,8 +49,7 @@ namespace amovGimbal
{
double x;
double y;
}AMOV_GIMBAL_FOV_T;
} AMOV_GIMBAL_FOV_T;
typedef struct
{
@ -69,6 +69,21 @@ namespace amovGimbal
uint32_t width;
} AMOV_GIMBAL_ROI_T;
typedef struct
{
double q0;
double q1;
double q2;
double q3;
} AMOV_GIMBAL_QUATERNION_T;
typedef struct
{
double x;
double y;
double z;
} AMOV_GIMBAL_VELOCITY_T;
} // namespace amovGimbal
#endif

29
gimbal_ctrl/driver/src/amov_tool.h Normal file
View File

@ -0,0 +1,29 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-07-31 18:30:33
* @LastEditors: L LC @amov
* @LastEditTime: 2023-07-31 18:55:18
* @FilePath: /gimbal-sdk-multi-platform/src/amov_tool.h
*/
namespace amovGimbalTools
{
static inline unsigned short conversionBigLittle(unsigned short value)
{
unsigned short temp = 0;
temp |= ((value >> 8) & 0X00FF);
temp |= ((value << 8) & 0XFF00);
return temp;
}
static inline unsigned int conversionBigLittle(unsigned int value)
{
unsigned int temp = 0;
temp |= ((value << 24) & 0XFF000000);
temp |= ((value << 8) & 0X00FF0000);
temp |= ((value >> 8) & 0X0000FF00);
temp |= ((value << 24) & 0X000000FF);
return temp;
}
}

134
gimbal_ctrl/sv_gimbal.cpp
View File

@ -15,6 +15,28 @@
#include <iostream>
#include <string>
#include <thread>
namespace sv
{
std::map<std::string, void *> Gimbal::IOList;
std::mutex Gimbal::IOListMutex;
std::map<GimbalType, std::string> gimbaltypeList =
{
{GimbalType::G1, "G1"},
{GimbalType::Q10f, "Q10f"},
{GimbalType::AT10, "AT10"}};
std::string &cvGimbalType2Str(const GimbalType &type)
{
std::map<GimbalType, std::string>::iterator temp = gimbaltypeList.find(type);
if (temp != gimbaltypeList.end())
{
return (temp->second);
}
throw "Error: Unsupported gimbal device type!!!!";
exit(-1);
}
}
/**
* This function sets the serial port for a Gimbal object.
@ -99,9 +121,65 @@ void sv::Gimbal::setNetPort(const int &port)
void sv::Gimbal::setStateCallback(sv::PStateInvoke callback)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
pdevTemp->dev->setParserCallback(callback);
pdevTemp->setParserCallback(callback);
}
/**
* The function `creatIO` creates an IO object for a gimbal device and adds it to a list of IO objects
* if it doesn't already exist.
*
* @param dev A pointer to an instance of the `Gimbal` class.
* @param IO The parameter "IO" is a pointer to an object that represents the input/output (IO) device
* for the gimbal. It is used to establish a connection with the gimbal and perform communication
* operations.
*
* @return a boolean value.
*/
void *sv::Gimbal::creatIO(sv::Gimbal *dev)
{
IOListMutex.lock();
std::map<std::string, void *>::iterator list = IOList.find(dev->m_serial_port);
std::pair<std::string, void *> key("NULL", nullptr);
if (list == IOList.end())
{
if (dev->m_gimbal_link == GimbalLink::SERIAL)
{
UART *ser;
ser = new UART(dev->m_serial_port,
(uint32_t)dev->m_serial_baud_rate,
serial::Timeout::simpleTimeout(dev->m_serial_timeout),
(serial::bytesize_t)dev->m_serial_byte_size,
(serial::parity_t)dev->m_serial_parity,
(serial::stopbits_t)dev->m_serial_stopbits,
(serial::flowcontrol_t)dev->m_serial_flowcontrol);
key.first = dev->m_serial_port;
key.second = (void *)ser;
IOList.insert(key);
}
else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP)
{
}
else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP)
{
}
else
{
std::cout << "Error: gimbal IO has opened!!!" << std::endl;
}
}
IOListMutex.unlock();
return key.second;
}
void sv::Gimbal::removeIO(sv::Gimbal *dev)
{
IOListMutex.lock();
IOList.erase(dev->m_serial_port);
IOListMutex.unlock();
}
/**
* The function opens a communication interface with a gimbal device and sets up a parser to handle
* incoming data.
@ -115,53 +193,22 @@ void sv::Gimbal::setStateCallback(sv::PStateInvoke callback)
*/
bool sv::Gimbal::open(PStateInvoke callback)
{
if (this->m_gimbal_link == GimbalLink::SERIAL)
bool ret = false;
this->IO = creatIO(this);
if (this->IO != nullptr)
{
this->IO = new UART(this->m_serial_port,
(uint32_t)this->m_serial_baud_rate,
serial::Timeout::simpleTimeout(this->m_serial_timeout),
(serial::bytesize_t)this->m_serial_byte_size,
(serial::parity_t)this->m_serial_parity,
(serial::stopbits_t)this->m_serial_stopbits,
(serial::flowcontrol_t)this->m_serial_flowcontrol);
}
// Subsequent additions
else if (this->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP)
{
return false;
}
else if (this->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP)
{
return false;
}
else
{
throw "Error: Unsupported communication interface class!!!";
return false;
}
std::string driverName;
switch (this->m_gimbal_type)
{
case sv::GimbalType::G1:
driverName = "G1";
break;
case sv::GimbalType::Q10f:
driverName = "Q10f";
break;
default:
throw "Error: Unsupported driver!!!";
return false;
break;
}
driverName = sv::cvGimbalType2Str(this->m_gimbal_type);
this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO);
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
pdevTemp->startStack();
pdevTemp->parserAuto(callback);
pdevTemp->dev->startStack();
pdevTemp->dev->parserAuto(callback);
ret = true;
}
return true;
return ret;
}
/**
@ -333,7 +380,7 @@ int sv::Gimbal::getVideoState()
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
int ret;
amovGimbal::AMOV_GIMBAL_STATE_T state;
state = pdevTemp->dev->getGimabalState();
state = pdevTemp->getGimabalState();
if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
{
ret = 1;
@ -406,6 +453,7 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y
sv::Gimbal::~Gimbal()
{
removeIO(this);
delete (amovGimbal::gimbal *)this->dev;
delete (amovGimbal::IOStreamBase *)this->IO;
}

11
include/sv_gimbal.h
View File

@ -10,6 +10,10 @@
#define __SV_GIMBAL__
#include <string>
#include <map>
#include <iterator>
#include <thread>
#include <mutex>
namespace sv
{
@ -21,7 +25,8 @@ namespace sv
enum class GimbalType
{
G1,
Q10f
Q10f,
AT10,
};
enum class GimbalLink
{
@ -95,6 +100,10 @@ namespace sv
GimbalType m_gimbal_type;
GimbalLink m_gimbal_link;
static std::map<std::string, void *> IOList;
static std::mutex IOListMutex;
static void *creatIO(Gimbal *dev);
static void removeIO(Gimbal *dev);
public:
//! Constructor
/*!