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

support net

This commit is contained in:
AiYangSky 2023-12-05 17:50:24 +08:00
parent ad54f95313
commit 62051a730b
35 changed files with 1525 additions and 882 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:47:48
* @LastEditTime: 2023-12-05 16:30:27
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h
*/
#ifndef AT10_GIMBAL_CRC32_H

66
gimbal_ctrl/driver/src/AT10/AT10_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-11-28 11:47:53
* @LastEditTime: 2023-12-05 17:23:48
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp
*/
#include "AT10_gimbal_driver.h"
@ -24,13 +24,6 @@ AT10GimbalDriver::AT10GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::
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);
}
/**
@ -84,21 +77,7 @@ void AT10GimbalDriver::convert(void *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();
std::cout << "Undefined old frame from AT10\r\n";
break;
case AT10::GIMBAL_CMD_STD:
@ -123,15 +102,15 @@ void AT10GimbalDriver::convert(void *buf)
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;
state.video = AMOV_GIMBAL_VIDEO_TAKE;
}
else
{
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
state.video = 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);
state.fov.x, state.fov.y, updataCaller);
mState.unlock();
break;
@ -316,21 +295,9 @@ bool AT10GimbalDriver::parser(IN uint8_t byte)
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));
}
@ -363,19 +330,26 @@ void AT10GimbalDriver::stackStart(void)
}
std::thread sendHeartLoop(&AT10GimbalDriver::sendHeart, this);
std::thread sendStdLoop(&AT10GimbalDriver::sendStd, this);
this->sendThreadHanle = sendStdLoop.native_handle();
this->sendHreatThreadHandle = sendHeartLoop.native_handle();
sendHeartLoop.detach();
sendStdLoop.detach();
}
void AT10GimbalDriver::parserLoop(void)
{
uint8_t temp;
uint8_t temp[65536];
uint32_t i = 0, getCount = 0;
while (1)
{
if (IO->inPutByte(&temp))
getCount = IO->inPutBytes(temp);
for (i = 0; i < getCount; i++)
{
parser(temp);
parser(temp[i]);
}
}
}
@ -387,7 +361,7 @@ void AT10GimbalDriver::getStdRxPack(void)
{
if (stdRxQueue->outCell(tempBuffer))
{
msgCustomCallback(tempBuffer);
msgCustomCallback(tempBuffer, msgCaller);
convert(tempBuffer);
}
}
@ -400,13 +374,13 @@ void AT10GimbalDriver::getExtRxPack(void)
{
if (rxQueue->outCell(tempBuffer))
{
msgCustomCallback(tempBuffer);
msgCustomCallback(tempBuffer, msgCaller);
convert(tempBuffer);
}
}
}
void AT10GimbalDriver::parserStart(amovGimbal::pStateInvoke callback)
void AT10GimbalDriver::parserStart(pAmovGimbalStateInvoke callback)
{
this->updateGimbalStateCallback = callback;
@ -414,6 +388,10 @@ void AT10GimbalDriver::parserStart(amovGimbal::pStateInvoke callback)
std::thread getStdRxPackLoop(&AT10GimbalDriver::getStdRxPack, this);
std::thread getExtRxPackLooP(&AT10GimbalDriver::getExtRxPack, this);
this->parserThreadHanle = parser.native_handle();
this->stackThreadHanle = getStdRxPackLoop.native_handle();
this->extStackThreadHandle = getExtRxPackLooP.native_handle();
parser.detach();
getStdRxPackLoop.detach();
getExtRxPackLooP.detach();

35
gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h
View File

@ -3,19 +3,18 @@
* @Author: L LC @amov
* @Date: 2022-10-28 12:24:21
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:47:58
* @LastEditTime: 2023-12-05 16:30:42
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h
*/
#include "../amov_gimbal.h"
#ifndef __AT10_DRIVER_H
#define __AT10_DRIVER_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:
@ -32,12 +31,13 @@ private:
void sendHeart(void);
void sendStd(void);
void parserStart(amovGimbal::pStateInvoke callback);
void parserStart(pAmovGimbalStateInvoke callback);
void parserLoop(void);
void getExtRxPack(void);
void getStdRxPack(void);
// bool getRxPack(OUT void *pack);
std::thread::native_handle_type sendHreatThreadHandle;
std::thread::native_handle_type extStackThreadHandle;
bool parser(IN uint8_t byte);
void convert(void *buf);
@ -46,18 +46,18 @@ private:
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 setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const 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 setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t takePic(void);
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
uint32_t extensionFuntions(void* cmd);
uint32_t extensionFuntions(void *cmd);
// builds
static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
@ -76,6 +76,13 @@ public:
{
delete stdTxQueue;
}
// set thread kill anytime
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
parserThreadHanle = parserThreadHanle == 0 ? 0 : pthread_cancel(parserThreadHanle);
sendThreadHanle = sendThreadHanle == 0 ? 0 : pthread_cancel(sendThreadHanle);
stackThreadHanle = stackThreadHanle == 0 ? 0 : pthread_cancel(stackThreadHanle);
sendHreatThreadHandle = sendHreatThreadHandle == 0 ? 0 : pthread_cancel(sendHreatThreadHandle);
extStackThreadHandle = extStackThreadHandle == 0 ? 0 : pthread_cancel(extStackThreadHandle);
}
};

32
gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp
View File

@ -3,8 +3,8 @@
* @Author: L LC @amov
* @Date: 2023-03-02 10:00:52
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:48:03
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp
* @LastEditTime: 2023-11-27 16:27:18
* @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_funtion.cpp
*/
#include "AT10_gimbal_driver.h"
#include "AT10_gimbal_crc32.h"
@ -17,7 +17,7 @@
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t AT10GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
uint32_t AT10GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
{
int16_t yaw, pitch;
AT10::GIMBAL_CMD_A1_MSG_T temp;
@ -35,14 +35,14 @@ uint32_t AT10GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &po
}
/**
* It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type
* It takes a struct of type 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)
uint32_t AT10GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
{
int16_t speedYaw, speedPitch;
AT10::GIMBAL_CMD_A1_MSG_T temp;
@ -66,7 +66,7 @@ uint32_t AT10GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t AT10GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
uint32_t AT10GimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
{
state.maxFollow.pitch = fabs(followSpeed.pitch * 100);
state.maxFollow.yaw = fabs(followSpeed.yaw * 100);
@ -110,10 +110,10 @@ uint32_t AT10GimbalDriver::takePic(void)
*
* @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)
uint32_t AT10GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
{
uint16_t temp;
if (newState == amovGimbal::AMOV_GIMBAL_VIDEO_T::AMOV_GIMBAL_VIDEO_TAKE)
if (newState == AMOV_GIMBAL_VIDEO_T::AMOV_GIMBAL_VIDEO_TAKE)
{
temp = 0x14 << 3;
}
@ -127,20 +127,20 @@ uint32_t AT10GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newSta
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)
uint32_t AT10GimbalDriver::setGimbalZoom(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:
case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
temp = 0X08 << 3;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
temp = 0X09 << 3;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
temp = 0X01 << 3;
break;
default:
@ -160,18 +160,18 @@ uint32_t AT10GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl
}
}
uint32_t AT10GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
uint32_t AT10GimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
uint16_t temp = 0;
switch (zoom)
{
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
temp = 0X0B << 3;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
temp = 0X0A << 3;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
temp = 0X01 << 3;
break;
default:

4
gimbal_ctrl/driver/src/AT10/AT10_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-11-28 11:48:08
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h
* @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

2
gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:47:02
* @LastEditTime: 2023-12-05 16:30:13
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h
*/
#ifndef G1_GIMBAL_CRC32_H

4
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-11-28 11:47:07
* @LastEditTime: 2023-12-05 17:22:57
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp
*/
#include "g1_gimbal_driver.h"
@ -75,7 +75,7 @@ void g1GimbalDriver::convert(void *buf)
state.rel.pitch = tempPos->HALL_pitch * G1_SCALE_FACTOR;
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);
state.fov.x, state.fov.y, updataCaller);
mState.unlock();
break;

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

@ -3,19 +3,18 @@
* @Author: L LC @amov
* @Date: 2022-10-28 12:24:21
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:47:12
* @LastEditTime: 2023-12-05 16:29:58
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h
*/
#include "../amov_gimbal.h"
#ifndef __G1_DRIVER_H
#define __G1_DRIVER_H
#include "../amov_gimbal_private.h"
#include "g1_gimbal_struct.h"
#include <mutex>
#include <malloc.h>
#include <iostream>
#ifndef __G1_DRIVER_H
#define __G1_DRIVER_H
class g1GimbalDriver : protected amovGimbal::amovGimbalBase
{
private:
@ -29,22 +28,22 @@ private:
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 setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
uint32_t setGimabalHome(void);
uint32_t takePic(void);
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
uint32_t setVideo(const 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,
uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed,
const 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,
uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData);
uint32_t extensionFuntions(void *cmd);

40
gimbal_ctrl/driver/src/G1/g1_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-11-28 11:47:19
* @LastEditTime: 2023-12-05 16:29:51
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp
*/
#include "g1_gimbal_driver.h"
@ -18,7 +18,7 @@
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t g1GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
uint32_t g1GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
{
G1::GIMBAL_SET_POS_MSG_T temp;
temp.mode = G1::GIMBAL_CMD_POS_MODE_ANGLE;
@ -32,14 +32,14 @@ uint32_t g1GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
}
/**
* It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type
* It takes a struct of type 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 g1GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
uint32_t g1GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
{
G1::GIMBAL_SET_POS_MSG_T temp;
temp.mode = G1::GIMBAL_CMD_POS_MODE_SPEED;
@ -59,7 +59,7 @@ uint32_t g1GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &sp
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t g1GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
uint32_t g1GimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
{
state.maxFollow.pitch = followSpeed.pitch / G1_SCALE_FACTOR;
state.maxFollow.roll = followSpeed.roll / G1_SCALE_FACTOR;
@ -100,18 +100,18 @@ uint32_t g1GimbalDriver::takePic(void)
*
* @return The return value is the number of bytes written to the serial port.
*/
uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
uint32_t g1GimbalDriver::setVideo(const 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 == AMOV_GIMBAL_VIDEO_TAKE)
{
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
state.video = AMOV_GIMBAL_VIDEO_OFF;
}
else
{
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE;
state.video = AMOV_GIMBAL_VIDEO_TAKE;
}
mState.unlock();
@ -124,10 +124,10 @@ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState
*
* @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
* @param speed The "speed" parameter is of type `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
* @param acc The "acc" parameter is of type "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
@ -136,9 +136,9 @@ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState
*
* @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,
uint32_t g1GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData)
{
G1::GIMBAL_ATT_CORR_MSG_T temp;
@ -161,12 +161,12 @@ uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATER
* 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
* @param pos The "pos" parameter is of type 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
* gimbal in terms of pitch, roll, and yaw. It is of type `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
* @param acc The "acc" parameter is of type "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
@ -174,9 +174,9 @@ uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATER
*
* @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,
uint32_t g1GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData)
{
G1::GIMBAL_ATT_CORR_MSG_T temp;

2
gimbal_ctrl/driver/src/G1/g1_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-11-28 11:47:24
* @LastEditTime: 2023-12-05 16:29:48
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h
*/
#ifndef G1_GIMBAL_STRUCT_H

2
gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2023-10-20 16:33:07
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:46:51
* @LastEditTime: 2023-12-05 16:29:39
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h
*/
#ifndef GX40_GIMBAL_CRC16_H

11
gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2023-10-20 16:08:17
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:46:45
* @LastEditTime: 2023-12-05 17:37:59
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp
*/
#include <string.h>
@ -51,7 +51,7 @@ void GX40GimbalDriver::nopSend(void)
* pointer type. It is used to specify a callback function that will be invoked when the gimbal state
* is updated.
*/
void GX40GimbalDriver::parserStart(amovGimbal::pStateInvoke callback)
void GX40GimbalDriver::parserStart(pAmovGimbalStateInvoke callback)
{
this->updateGimbalStateCallback = callback;
@ -160,8 +160,8 @@ void GX40GimbalDriver::convert(void *buf)
secondary = (GX40::GIMBAL_SECONDARY_SLAVE_FRAME_T *)temp->secondaryData;
mState.lock();
this->state.workMode = (amovGimbal::AMOV_GIMBAL_SERVO_MODE_T)primary->workMode;
this->state.cameraFlag = (amovGimbal::AMOV_GIMBAL_CAMERA_FLAG_T)primary->state;
this->state.workMode = (AMOV_GIMBAL_SERVO_MODE_T)primary->workMode;
this->state.cameraFlag = (AMOV_GIMBAL_CAMERA_FLAG_T)primary->state;
// 应该需要再解算一下，才能出具体的框架角度
this->state.rel.yaw = -(primary->motorYaw * XF_ANGLE_DPI);
this->state.rel.yaw = this->state.rel.yaw < -180.0f ? this->state.rel.yaw + 360.0f : this->state.rel.yaw;
@ -186,7 +186,7 @@ void GX40GimbalDriver::convert(void *buf)
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);
state.fov.x, state.fov.y, updataCaller);
mState.unlock();
}
@ -213,7 +213,6 @@ uint32_t GX40GimbalDriver::calPackLen(void *pack)
*
* @return a boolean value, either true or false.
*/
#include <stdio.h>
bool GX40GimbalDriver::parser(IN uint8_t byte)
{
bool state = false;

36
gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h
View File

@ -4,10 +4,13 @@
* @Author: L LC @amov
* @Date: 2023-10-20 16:08:13
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:46:35
* @LastEditTime: 2023-12-05 16:29:20
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h
*/
#include "../amov_gimbal.h"
#ifndef __GX40_DRIVER_H
#define __GX40_DRIVER_H
#include "../amov_gimbal_private.h"
#include "GX40_gimbal_struct.h"
#include <mutex>
@ -16,9 +19,6 @@
#include <chrono>
#include <time.h>
#ifndef __GX40_DRIVER_H
#define __GX40_DRIVER_H
class GX40GimbalDriver : public amovGimbal::amovGimbalBase
{
GX40::GIMBAL_FRAME_PARSER_STATE_T parserState;
@ -28,15 +28,15 @@ class GX40GimbalDriver : public amovGimbal::amovGimbalBase
std::mutex carrierStateMutex;
int16_t targetPos[3];
amovGimbal::AMOV_GIMBAL_POS_T carrierPos;
amovGimbal::AMOV_GIMBAL_VELOCITY_T carrierSpeed;
amovGimbal::AMOV_GIMBAL_VELOCITY_T carrierAcc;
AMOV_GIMBAL_POS_T carrierPos;
AMOV_GIMBAL_VELOCITY_T carrierSpeed;
AMOV_GIMBAL_VELOCITY_T carrierAcc;
GX40::GIMBAL_SECONDARY_MASTER_FRAME_T carrierGNSS;
std::thread::native_handle_type nopSendThreadHandle;
void nopSend(void);
void parserStart(amovGimbal::pStateInvoke callback);
void parserStart(pAmovGimbalStateInvoke callback);
public:
uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
@ -45,18 +45,18 @@ public:
uint32_t calPackLen(void *pack);
// funtions
uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
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 setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t takePic(void);
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc,
uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData);
uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);

52
gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2023-11-02 17:50:26
* @LastEditors: L LC @amov
* @LastEditTime: 2023-12-04 09:56:21
* @LastEditTime: 2023-12-05 16:29:13
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp
*/
#include <string.h>
@ -12,12 +12,12 @@
/**
* The function sets the target position of a gimbal based on the input roll, pitch, and yaw values.
*
* @param pos The parameter "pos" is of type "amovGimbal::AMOV_GIMBAL_POS_T". It is a structure that
* @param pos The parameter "pos" is of type "AMOV_GIMBAL_POS_T". It is a structure that
* contains the roll, pitch, and yaw values of the gimbal position.
*
* @return a packed value of type uint32_t.
*/
uint32_t GX40GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
{
carrierStateMutex.lock();
targetPos[0] = (int16_t)(-pos.roll / 0.01f);
@ -30,12 +30,12 @@ uint32_t GX40GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &po
/**
* The function sets the gimbal speed based on the provided roll, pitch, and yaw values.
*
* @param speed The parameter "speed" is of type "amovGimbal::AMOV_GIMBAL_POS_T". It is a structure
* @param speed The parameter "speed" is of type "AMOV_GIMBAL_POS_T". It is a structure
* that contains the roll, pitch, and yaw values of the gimbal speed.
*
* @return the result of the pack() function, which is of type uint32_t.
*/
uint32_t GX40GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
uint32_t GX40GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
{
carrierStateMutex.lock();
targetPos[0] = (int16_t)(-speed.roll / 0.1f);
@ -81,23 +81,23 @@ uint32_t GX40GimbalDriver::takePic(void)
/**
* The function `setVideo` toggles the video state of a gimbal driver and returns a packed command.
*
* @param newState The parameter `newState` is of type `amovGimbal::AMOV_GIMBAL_VIDEO_T`, which is an
* @param newState The parameter `newState` is of type `AMOV_GIMBAL_VIDEO_T`, which is an
* enumeration representing the state of the video in the gimbal. It can have two possible values:
*
* @return the result of the `pack` function, which is a `uint32_t` value.
*/
uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
uint32_t GX40GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
{
uint8_t temp = 0X01;
mState.lock();
if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
if (state.video == AMOV_GIMBAL_VIDEO_TAKE)
{
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
state.video = AMOV_GIMBAL_VIDEO_OFF;
}
else
{
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE;
state.video = AMOV_GIMBAL_VIDEO_TAKE;
}
mState.unlock();
@ -108,13 +108,13 @@ uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newSta
* The function `attitudeCorrection` updates the state of a gimbal driver with position, velocity, and
* acceleration data.
*
* @param pos The "pos" parameter is of type "amovGimbal::AMOV_GIMBAL_POS_T" and represents the current
* @param pos The "pos" parameter is of type "AMOV_GIMBAL_POS_T" and represents the current
* position of the gimbal. It likely contains information such as the pitch, yaw, and roll angles of
* the gimbal.
* @param seppd The parameter `seppd` stands for "Separate Pointing Device" and represents the velocity
* of the gimbal in separate axes (e.g., pitch, yaw, roll). It is of type
* `amovGimbal::AMOV_GIMBAL_VELOCITY_T`.
* @param acc The "acc" parameter is of type "amovGimbal::AMOV_GIMBAL_VELOCITY_T" and represents the
* `AMOV_GIMBAL_VELOCITY_T`.
* @param acc The "acc" parameter is of type "AMOV_GIMBAL_VELOCITY_T" and represents the
* acceleration of the gimbal.
* @param extenData The extenData parameter is a pointer to additional data that can be passed to the
* attitudeCorrection function. It can be used to provide any extra information or context that may be
@ -122,12 +122,12 @@ uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newSta
* not provided in the code snippet,
*
* @return the size of the data being passed as arguments. The size is calculated by adding the sizes
* of the three types: sizeof(amovGimbal::AMOV_GIMBAL_POS_T),
* sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T), and sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T).
* of the three types: sizeof(AMOV_GIMBAL_POS_T),
* sizeof(AMOV_GIMBAL_VELOCITY_T), and sizeof(AMOV_GIMBAL_VELOCITY_T).
*/
uint32_t GX40GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc,
uint32_t GX40GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData)
{
carrierStateMutex.lock();
@ -136,7 +136,7 @@ uint32_t GX40GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_
carrierAcc = acc;
upDataTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
carrierStateMutex.unlock();
return sizeof(amovGimbal::AMOV_GIMBAL_POS_T) + sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T) + sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T);
return sizeof(AMOV_GIMBAL_POS_T) + sizeof(AMOV_GIMBAL_VELOCITY_T) + sizeof(AMOV_GIMBAL_VELOCITY_T);
}
/**
@ -159,7 +159,7 @@ uint32_t GX40GimbalDriver::extensionFuntions(void *cmd)
* The function `setGimbalZoom` in the `GX40GimbalDriver` class sets the zoom level of a gimbal based on
* the specified zoom type and target rate.
*
* @param zoom The "zoom" parameter is of type amovGimbal::AMOV_GIMBAL_ZOOM_T, which is an enumeration
* @param zoom The "zoom" parameter is of type AMOV_GIMBAL_ZOOM_T, which is an enumeration
* type. It represents the zoom action to be performed on the gimbal. The possible values for this
* parameter are:
* @param targetRate The targetRate parameter is a float value representing the desired zoom rate for
@ -167,7 +167,7 @@ uint32_t GX40GimbalDriver::extensionFuntions(void *cmd)
*
* @return a value of type uint32_t.
*/
uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
uint32_t GX40GimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
uint8_t temp[4];
uint8_t len = 0;
@ -177,13 +177,13 @@ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl
len = 1;
switch (zoom)
{
case amovGimbal::AMOV_GIMBAL_ZOOM_IN:
case AMOV_GIMBAL_ZOOM_IN:
temp[0] = GX40::GIMBAL_CMD_ZOMM_IN;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_OUT:
case AMOV_GIMBAL_ZOOM_OUT:
temp[0] = GX40::GIMBAL_CMD_ZOOM_OUT;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_STOP:
case AMOV_GIMBAL_ZOOM_STOP:
temp[0] = GX40::GIMBAL_CMD_ZOOM_STOP;
break;
}
@ -204,7 +204,7 @@ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl
* The function "setGimbalFocus" sets the focus of a gimbal by specifying the zoom level and target
* rate.
*
* @param zoom The zoom parameter is of type amovGimbal::AMOV_GIMBAL_ZOOM_T, which is an enumeration
* @param zoom The zoom parameter is of type AMOV_GIMBAL_ZOOM_T, which is an enumeration
* type representing different zoom levels for the gimbal. It is used to specify the desired zoom level
* for the gimbal focus.
* @param targetRate The targetRate parameter is a float value representing the desired zoom rate for
@ -212,7 +212,7 @@ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl
*
* @return the result of the pack() function, which is of type uint32_t.
*/
uint32_t GX40GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
uint32_t GX40GimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
uint8_t temp = 0X01;

2
gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h
View File

@ -3,7 +3,7 @@
* @Author: L LC @amov
* @Date: 2023-10-20 16:08:13
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:46:22
* @LastEditTime: 2023-12-05 16:28:54
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h
*/
#ifndef GX40_GIMBAL_STRUCT_H

3
gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h
View File

@ -3,12 +3,11 @@
* @Author: L LC @amov
* @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:45:25
* @LastEditTime: 2023-12-05 16:28:29
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h
*/
#ifndef Q10F_GIMBAL_CRC32_H
#define Q10F_GIMBAL_CRC32_H
namespace Q10f
{
static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght)

4
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-11-28 11:45:31
* @LastEditTime: 2023-12-05 17:23:15
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp
*/
#include "Q10f_gimbal_driver.h"
@ -86,7 +86,7 @@ void Q10fGimbalDriver::convert(void *buf)
state.rel.pitch = tempPos->pitchStatorRotorAngle * Q10F_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);
state.fov.x, state.fov.y, updataCaller);
mState.unlock();
break;

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

@ -3,19 +3,18 @@
* @Author: L LC @amov
* @Date: 2022-10-28 12:24:21
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:45:36
* @LastEditTime: 2023-12-05 16:27:45
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h
*/
#include "../amov_gimbal.h"
#ifndef __Q10F_DRIVER_H
#define __Q10F_DRIVER_H
#include "../amov_gimbal_private.h"
#include "Q10f_gimbal_struct.h"
#include <mutex>
#include <malloc.h>
#include <iostream>
#ifndef __Q10F_DRIVER_H
#define __Q10F_DRIVER_H
class Q10fGimbalDriver : protected amovGimbal::amovGimbalBase
{
private:
@ -29,16 +28,16 @@ private:
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 setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const 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 setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t takePic(void);
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
// builds
static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)

32
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-11-28 11:45:41
* @LastEditTime: 2023-12-05 16:27:39
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp
*/
#include "Q10f_gimbal_driver.h"
@ -17,7 +17,7 @@
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t Q10fGimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
uint32_t Q10fGimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
{
Q10f::GIMBAL_SET_POS_MSG_T temp;
temp.modeR = Q10f::GIMBAL_CMD_POS_MODE_ANGLE_SPEED;
@ -41,7 +41,7 @@ uint32_t Q10fGimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &po
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t Q10fGimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
uint32_t Q10fGimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
{
Q10f::GIMBAL_SET_POS_MSG_T temp;
temp.modeR = Q10f::GIMBAL_CMD_POS_MODE_SPEED;
@ -64,7 +64,7 @@ uint32_t Q10fGimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &
*
* @return The return value is the number of bytes written to the buffer.
*/
uint32_t Q10fGimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
uint32_t Q10fGimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
{
state.maxFollow.pitch = followSpeed.pitch / 0.1220740379f;
state.maxFollow.roll = followSpeed.roll / 0.1220740379f;
@ -108,25 +108,25 @@ uint32_t Q10fGimbalDriver::takePic(void)
*
* @return The return value is the number of bytes written to the serial port.
*/
uint32_t Q10fGimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
uint32_t Q10fGimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
{
uint8_t cmd[2] = {0X01, 0XFF};
if (newState == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
if (newState == AMOV_GIMBAL_VIDEO_TAKE)
{
cmd[0] = 0X02;
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE;
state.video = AMOV_GIMBAL_VIDEO_TAKE;
}
else
{
cmd[0] = 0X03;
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
state.video = AMOV_GIMBAL_VIDEO_OFF;
}
return pack(Q10f::GIMBAL_CMD_CAMERA, (uint8_t *)cmd, sizeof(cmd));
}
uint32_t Q10fGimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
uint32_t Q10fGimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
uint8_t cmd[5] = {0X00, 0X00, 0X00, 0X00, 0XFF};
if (targetRate == 0.0f)
@ -134,13 +134,13 @@ uint32_t Q10fGimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl
cmd[1] = 0XFF;
switch (zoom)
{
case amovGimbal::AMOV_GIMBAL_ZOOM_IN:
case AMOV_GIMBAL_ZOOM_IN:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_OUT:
case AMOV_GIMBAL_ZOOM_OUT:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_STOP:
case AMOV_GIMBAL_ZOOM_STOP:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP;
break;
default:
@ -159,18 +159,18 @@ uint32_t Q10fGimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl
}
}
uint32_t Q10fGimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
uint32_t Q10fGimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
uint8_t cmd[2] = {0X00, 0XFF};
switch (zoom)
{
case amovGimbal::AMOV_GIMBAL_ZOOM_IN:
case AMOV_GIMBAL_ZOOM_IN:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_OUT:
case AMOV_GIMBAL_ZOOM_OUT:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT;
break;
case amovGimbal::AMOV_GIMBAL_ZOOM_STOP:
case AMOV_GIMBAL_ZOOM_STOP:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP;
break;
default:

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-11-28 11:45:46
* @LastEditTime: 2023-12-05 16:27:27
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h
*/
#ifndef Q10F_GIMBAL_STRUCT_H

111
gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h Executable file
View File

@ -0,0 +1,111 @@
/*
* @Description: External interface of amov gimbals
* @Author: L LC @amov
* @Date: 2022-10-27 18:34:26
* @LastEditors: L LC @amov
* @LastEditTime: 2023-12-05 17:37:09
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h
*/
#ifndef AMOV_GIMBAL_H
#define AMOV_GIMBAL_H
#include <stdint.h>
#include <stdbool.h>
#include <iostream>
#include "amov_gimbal_struct.h"
namespace amovGimbal
{
#define IN
#define OUT
#define SET
#ifndef MAX_QUEUE_SIZE
#define MAX_QUEUE_SIZE 100
#endif
static inline void idleCallback(double frameAngleRoll, double frameAnglePitch, double frameAngleYaw,
double imuAngleRoll, double imuAnglePitch, double imuAngleYaw,
double fovX, double fovY, void *caller)
{
}
static inline void idleMsgCallback(void *msg, void *caller)
{
}
// Control data input and output
class IOStreamBase
{
public:
IOStreamBase() {}
virtual ~IOStreamBase() {}
virtual bool open() = 0;
virtual bool close() = 0;
virtual bool isOpen() = 0;
virtual bool isBusy() = 0;
// These two functions need to be thread-safe
virtual uint32_t inPutBytes(IN uint8_t *byte) = 0;
virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) = 0;
};
class gimbal
{
private:
std::string typeName;
// Instantiated device handle
void *devHandle;
public:
static void inBytesCallback(uint8_t *pData, uint32_t len, gimbal *handle);
// Protocol stack function items
void startStack(void);
void parserAuto(pAmovGimbalStateInvoke callback = idleCallback, void *caller = nullptr);
void setParserCallback(pAmovGimbalStateInvoke callback, void *caller = nullptr);
void setMsgCallback(pAmovGimbalMsgInvoke callback, void *caller = nullptr);
void setRcvBytes(pAmovGimbalInputBytesInvoke callbaclk, void *caller = nullptr);
void setSendBytes(pAmovGimbalOutputBytesInvoke callbaclk, void *caller = nullptr);
AMOV_GIMBAL_STATE_T getGimabalState(void);
// non-block functions
uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
uint32_t setGimabalHome(void);
uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalROI(const AMOV_GIMBAL_ROI_T area);
uint32_t takePic(void);
uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
uint32_t extensionFuntions(void *cmd);
// block functions
bool setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos);
bool setGimabalHomeBlock(void);
bool setGimbalZoomBlock(float targetRate);
bool takePicBlock(void);
bool calibrationBlock(void);
std::string name()
{
return typeName;
}
gimbal(const std::string &type, IOStreamBase *_IO,
uint32_t _self = 0x02, uint32_t _remote = 0X80);
~gimbal();
};
}
#endif

55
gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_c.h Normal file
View File

@ -0,0 +1,55 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-11-24 16:01:22
* @LastEditors: L LC @amov
* @LastEditTime: 2023-12-05 17:20:21
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_c.h
*/
#ifndef AMOV_GIMBAL_C_H
#define AMOV_GIMBAL_C_H
#include <stdint.h>
#include "amov_gimbal_struct.h"
extern "C"
{
// initialization funtions
void amovGimbalSetRcvBytes(pAmovGimbalInputBytesInvoke callbaclk, void *handle, void *caller);
void amovGimbalSetSendBytes(pAmovGimbalOutputBytesInvoke callbaclk, void *handle, void *caller);
void amovGimbalInBytesCallback(uint8_t *pData, uint32_t len, void *handle);
void amovGimbalCreat(char *type, uint32_t selfId, uint32_t gimbalId, void *handle, void *caller);
void amovGimbalStart(pAmovGimbalStateInvoke callback, void *handle, void *caller);
void amovGimbalChangeStateCallback(pAmovGimbalStateInvoke callback, void *handle, void *caller);
void amovGimbalSetMsgCallback(pAmovGimbalMsgInvoke callback, void *handle, void *caller);
// non-block functions
uint32_t amovGimbalSetGimabalPos(AMOV_GIMBAL_POS_T *pos, void *handle);
uint32_t amovGimbalSetGimabalSpeed(AMOV_GIMBAL_POS_T *speed, void *handle);
uint32_t amovGimbalSetGimabalFollowSpeed(AMOV_GIMBAL_POS_T *followSpeed, void *handle);
uint32_t amovGimbalSetGimabalHome(void *handle);
uint32_t amovGimbalSetGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate, void *handle);
uint32_t amovGimbalSetGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate, void *handle);
uint32_t amovGimbalSetGimbalROI(AMOV_GIMBAL_ROI_T *area, void *handle);
uint32_t amovGimbalTakePic(void *handle);
uint32_t amovGimbalSetVideo(AMOV_GIMBAL_VIDEO_T newState, void *handle);
uint32_t amovGimbalAttitudeCorrectionQ(AMOV_GIMBAL_QUATERNION_T *quaterion,
AMOV_GIMBAL_VELOCITY_T *speed,
AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle);
uint32_t amovGimbalAttitudeCorrectionE(AMOV_GIMBAL_POS_T *pos,
AMOV_GIMBAL_VELOCITY_T *speed,
AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle);
uint32_t amovGimbalSetGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt, void *handle);
uint32_t amovGimbalExtensionFuntions(void *cmd, void *handle);
void getGimabalState(AMOV_GIMBAL_STATE_T *state, void *handle);
void getGimbalType(char *type, void *handle);
// block functions
bool amovGimbalSetGimbalPosBlock(AMOV_GIMBAL_POS_T *pos, void *handle);
bool amovGimbalSetGimabalHomeBlock(void *handle);
bool amovGimbalSetGimbalZoomBlock(float targetRate, void *handle);
bool amovGimbalTakePicBlock(void *handle);
bool amovGimbalCalibrationBlock(void *handle);
}
#endif

102
gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_struct.h Executable file
View File

@ -0,0 +1,102 @@
/*
* @Description: Common Data Structures of gimbal
* @Author: L LC @amov
* @Date: 2022-10-31 11:56:43
* @LastEditors: L LC @amov
* @LastEditTime: 2023-12-05 17:03:02
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_struct.h
*/
#include <stdint.h>
#ifndef __AMOV_GIMABL_STRUCT_H
#define __AMOV_GIMABL_STRUCT_H
typedef void (*pAmovGimbalStateInvoke)(double frameAngleRoll, double frameAnglePitch, double frameAngleYaw,
double imuAngleRoll, double imuAnglePitch, double imuAngleYaw,
double fovX, double fovY, void *caller);
typedef void (*pAmovGimbalMsgInvoke)(void *msg, void *caller);
typedef uint32_t (*pAmovGimbalInputBytesInvoke)(uint8_t *pData, void *caller);
typedef uint32_t (*pAmovGimbalOutputBytesInvoke)(uint8_t *pData, uint32_t len, void *caller);
typedef enum
{
AMOV_GIMBAL_SERVO_MODE_FPV = 0X10,
AMOV_GIMBAL_SERVO_MODE_LOCK = 0X11,
AMOV_GIMBAL_SERVO_MODE_FOLLOW = 0X12,
AMOV_GIMBAL_SERVO_MODE_OVERLOOK = 0X13,
AMOV_GIMBAL_SERVO_MODE_EULER = 0X14,
AMOV_GIMBAL_SERVO_MODE_WATCH = 0X16,
AMOV_GIMBAL_SERVO_MODE_TRACK = 0X17,
} AMOV_GIMBAL_SERVO_MODE_T;
typedef enum
{
AMOV_GIMBAL_CAMERA_FLAG_INVERSION = 0X1000,
AMOV_GIMBAL_CAMERA_FLAG_IR = 0X0200,
AMOV_GIMBAL_CAMERA_FLAG_RF = 0X0100,
AMOV_GIMBAL_CAMERA_FLAG_LOCK = 0X0001,
} AMOV_GIMBAL_CAMERA_FLAG_T;
typedef enum
{
AMOV_GIMBAL_VIDEO_TAKE,
AMOV_GIMBAL_VIDEO_OFF
} AMOV_GIMBAL_VIDEO_T;
typedef enum
{
AMOV_GIMBAL_ZOOM_IN,
AMOV_GIMBAL_ZOOM_OUT,
AMOV_GIMBAL_ZOOM_STOP
} AMOV_GIMBAL_ZOOM_T;
typedef struct
{
double yaw;
double roll;
double pitch;
} AMOV_GIMBAL_POS_T;
typedef struct
{
double x;
double y;
} AMOV_GIMBAL_FOV_T;
typedef struct
{
AMOV_GIMBAL_SERVO_MODE_T workMode;
AMOV_GIMBAL_CAMERA_FLAG_T cameraFlag;
AMOV_GIMBAL_VIDEO_T video;
AMOV_GIMBAL_POS_T abs;
AMOV_GIMBAL_POS_T rel;
AMOV_GIMBAL_POS_T relSpeed;
AMOV_GIMBAL_POS_T maxFollow;
AMOV_GIMBAL_FOV_T fov;
} AMOV_GIMBAL_STATE_T;
typedef struct
{
uint32_t centreX;
uint32_t centreY;
uint32_t hight;
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; // or N
double y; // or E
double z; // or UP
} AMOV_GIMBAL_VELOCITY_T;
#endif

357
gimbal_ctrl/driver/src/amov_gimbal.cpp
View File

@ -1,357 +0,0 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2022-10-28 11:54:11
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:44:37
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal.cpp
*/
#include "amov_gimbal_private.h"
#include "g1_gimbal_driver.h"
#include "Q10f_gimbal_driver.h"
#include "AT10_gimbal_driver.h"
#include "GX40_gimbal_driver.h"
#include <iostream>
#include <thread>
#include <map>
#include <iterator>
#define MAX_PACK_SIZE 280
namespace amovGimbal
{
typedef amovGimbal::amovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO);
typedef std::map<std::string, createCallback> callbackMap;
callbackMap amovGimbals =
{
{"G1", g1GimbalDriver::creat},
{"Q10f", Q10fGimbalDriver::creat},
{"AT10", AT10GimbalDriver::creat},
{"GX40", GX40GimbalDriver::creat}};
}
/* The amovGimbalCreator class is a factory class that creates an instance of the amovGimbal class */
// Factory used to create the gimbal instance
class amovGimbalCreator
{
public:
static amovGimbal::amovGimbalBase *createAmovGimbal(const std::string &type, amovGimbal::IOStreamBase *_IO)
{
amovGimbal::callbackMap::iterator temp = amovGimbal::amovGimbals.find(type);
if (temp != amovGimbal::amovGimbals.end())
{
return (temp->second)(_IO);
}
std::cout << type << " is Unsupported device type!" << std::endl;
return NULL;
}
private:
amovGimbalCreator()
{
}
static amovGimbalCreator *pInstance;
static amovGimbalCreator *getInstance()
{
if (pInstance == NULL)
{
pInstance = new amovGimbalCreator();
}
return pInstance;
}
~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));
// printf("\r\n");
// for(uint16_t i =0 ; i< calPackLen(tempBuffer);i++)
// {
// printf("%02X ",tempBuffer[i]);
// }
}
}
}
/**
* "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::amovGimbalBase::parserLoop(void)
{
uint8_t temp;
while (1)
{
if (IO->inPutByte(&temp))
{
parser(temp);
}
}
}
void amovGimbal::amovGimbalBase::sendLoop(void)
{
while (1)
{
send();
}
}
void amovGimbal::amovGimbalBase::mainLoop(void)
{
uint8_t tempBuffer[MAX_PACK_SIZE];
while (1)
{
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);
this->sendThreadHanle = sendLoop.native_handle();
this->parserThreadHanle = parserLoop.native_handle();
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::gimbal::startStack(void)
{
((amovGimbalBase *)(this->dev))->stackStart();
}
void amovGimbal::gimbal::setParserCallback(amovGimbal::pStateInvoke callback)
{
((amovGimbalBase *)(this->dev))->updateGimbalStateCallback = callback;
}
void amovGimbal::amovGimbalBase::nodeSet(SET uint32_t _self, SET uint32_t _remote)
{
return;
}
void amovGimbal::amovGimbalBase::parserStart(pStateInvoke callback)
{
this->updateGimbalStateCallback = callback;
std::thread mainLoop(&amovGimbalBase::mainLoop, this);
this->stackThreadHanle = mainLoop.native_handle();
mainLoop.detach();
}
/**
* The function creates a thread that runs the mainLoop function
*/
void amovGimbal::gimbal::parserAuto(pStateInvoke callback)
{
((amovGimbalBase *)(this->dev))->parserStart(callback);
}
void amovGimbal::gimbal::setMsgCallback(pMsgInvoke callback)
{
((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;
}
/**
* Default implementation of interface functions, not pure virtual functions for ease of extension.
*/
uint32_t amovGimbal::IamovGimbalBase::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::setGimabalHome(void)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::setGimbalROI(const amovGimbal::AMOV_GIMBAL_ROI_T area)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::takePic(void)
{
return 0;
}
uint32_t amovGimbal::IamovGimbalBase::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
{
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;
}
uint32_t amovGimbal::IamovGimbalBase::setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt)
{
return 0;
}
bool amovGimbal::IamovGimbalBase::setGimbalPosBlock(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
{
return false;
}
bool amovGimbal::IamovGimbalBase::setGimabalHomeBlock(void)
{
return false;
}
bool amovGimbal::IamovGimbalBase::setGimbalZoomBlock(float targetRate)
{
return false;
}
bool amovGimbal::IamovGimbalBase::takePicBlock(void)
{
return false;
}
bool amovGimbal::IamovGimbalBase::calibrationBlock(void)
{
return false;
}
/**
* 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
*
* @param type the type of the device, which is the same as the name of the class
* @param _IO The IOStreamBase object that is used to communicate with the device.
* @param _self the node ID of the device
* @param _remote the node ID of the remote device
*/
amovGimbal::gimbal::gimbal(const std::string &type, IOStreamBase *_IO,
uint32_t _self, uint32_t _remote)
{
typeName = type;
IO = _IO;
dev = amovGimbalCreator::createAmovGimbal(typeName, IO);
((amovGimbalBase *)(dev))->nodeSet(_self, _remote);
}
amovGimbal::gimbal::~gimbal()
{
// 先干掉请求线程
std::this_thread::sleep_for(std::chrono::milliseconds(50));
delete dev;
}

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

@ -1,109 +0,0 @@
/*
* @Description: External interface of amov gimbals
* @Author: L LC @amov
* @Date: 2022-10-27 18:34:26
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:44:44
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal.h
*/
#ifndef AMOV_GIMBAL_H
#define AMOV_GIMBAL_H
#include <stdint.h>
#include <stdbool.h>
#include <iostream>
#include "amov_gimbal_struct.h"
#define MAX_QUEUE_SIZE 100
namespace amovGimbal
{
#define IN
#define OUT
#define SET
static inline void idleCallback(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
double &fovX, double &fovY)
{
}
static inline void idleMsgCallback(void *)
{
}
// Control data input and output
class IOStreamBase
{
public:
IOStreamBase() {}
virtual ~IOStreamBase() {}
virtual bool open() = 0;
virtual bool close() = 0;
virtual bool isOpen() = 0;
virtual bool isBusy() = 0;
// These two functions need to be thread-safe
virtual bool inPutByte(IN uint8_t *byte) = 0;
virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) = 0;
};
// Device interface
class IamovGimbalBase
{
public:
IamovGimbalBase() {}
virtual ~IamovGimbalBase() {}
// non-block functions
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);
virtual uint32_t setGimabalHome(void);
virtual uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
virtual uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
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 setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
virtual uint32_t extensionFuntions(void *cmd);
// block functions
virtual bool setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos);
virtual bool setGimabalHomeBlock(void);
virtual bool setGimbalZoomBlock(float targetRate);
virtual bool takePicBlock(void);
virtual bool calibrationBlock(void);
};
class gimbal
{
private:
std::string typeName;
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;
}
gimbal(const std::string &type, IOStreamBase *_IO,
uint32_t _self = 0x02, uint32_t _remote = 0X80);
~gimbal();
};
}
#endif

90
gimbal_ctrl/driver/src/amov_gimbal_factory.cpp Normal file
View File

@ -0,0 +1,90 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-11-24 15:48:47
* @LastEditors: L LC @amov
* @LastEditTime: 2023-12-05 16:27:10
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_factory.cpp
*/
#include "amov_gimbal_private.h"
#include "g1_gimbal_driver.h"
#include "Q10f_gimbal_driver.h"
#include "AT10_gimbal_driver.h"
#include "GX40_gimbal_driver.h"
#include <map>
#include <iterator>
namespace amovGimbalFactory
{
typedef amovGimbal::amovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO);
typedef std::map<std::string, createCallback> callbackMap;
callbackMap amovGimbals =
{
{"G1", g1GimbalDriver::creat},
{"Q10f", Q10fGimbalDriver::creat},
{"AT10", AT10GimbalDriver::creat},
{"GX40", GX40GimbalDriver::creat}};
/* The amovGimbalCreator class is a factory class that creates an instance of the amovGimbal class */
// Factory used to create the gimbal instance
class amovGimbalCreator
{
public:
static amovGimbal::amovGimbalBase *createAmovGimbal(const std::string &type, amovGimbal::IOStreamBase *_IO)
{
callbackMap::iterator temp = amovGimbals.find(type);
if (temp != amovGimbals.end())
{
return (temp->second)(_IO);
}
std::cout << type << " is Unsupported device type!" << std::endl;
return NULL;
}
private:
amovGimbalCreator()
{
}
static amovGimbalCreator *pInstance;
static amovGimbalCreator *getInstance()
{
if (pInstance == NULL)
{
pInstance = new amovGimbalCreator();
}
return pInstance;
}
~amovGimbalCreator();
};
} // namespace amovGimbalFactory
/**
* 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
*
* @param type the type of the device, which is the same as the name of the class
* @param _IO The IOStreamBase object that is used to communicate with the device.
* @param _self the node ID of the device
* @param _remote the node ID of the remote device
*/
amovGimbal::gimbal::gimbal(const std::string &type, IOStreamBase *_IO,
uint32_t _self, uint32_t _remote)
{
typeName = type;
devHandle = amovGimbalFactory::amovGimbalCreator::createAmovGimbal(typeName, _IO);
((amovGimbalBase *)(devHandle))->nodeSet(_self, _remote);
}
amovGimbal::gimbal::~gimbal()
{
// 先干掉请求线程
std::this_thread::sleep_for(std::chrono::milliseconds(50));
delete ((amovGimbalBase *)(devHandle));
}

229
gimbal_ctrl/driver/src/amov_gimbal_interface.cpp Normal file
View File

@ -0,0 +1,229 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-11-24 16:00:28
* @LastEditors: L LC @amov
* @LastEditTime: 2023-12-05 17:18:34
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_interface.cpp
*/
#include "amov_gimbal_private.h"
// must realize
void amovGimbal::gimbal::startStack(void)
{
((amovGimbalBase *)(this->devHandle))->stackStart();
}
void amovGimbal::gimbal::parserAuto(pAmovGimbalStateInvoke callback, void *caller)
{
((amovGimbalBase *)(this->devHandle))->parserStart(callback, caller);
}
void amovGimbal::gimbal::setParserCallback(pAmovGimbalStateInvoke callback, void *caller)
{
((amovGimbalBase *)(this->devHandle))->updateGimbalStateCallback = callback;
((amovGimbalBase *)(this->devHandle))->updataCaller = caller;
}
void amovGimbal::gimbal::setMsgCallback(pAmovGimbalMsgInvoke callback, void *caller)
{
((amovGimbalBase *)(this->devHandle))->msgCustomCallback = callback;
((amovGimbalBase *)(this->devHandle))->msgCaller = caller;
}
AMOV_GIMBAL_STATE_T amovGimbal::gimbal::getGimabalState(void)
{
((amovGimbalBase *)(this->devHandle))->mState.lock();
AMOV_GIMBAL_STATE_T temp = ((amovGimbalBase *)(this->devHandle))->state;
((amovGimbalBase *)(this->devHandle))->mState.unlock();
return temp;
}
// gimbal funtions maybe realize
uint32_t amovGimbal::gimbal::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
{
return ((amovGimbalBase *)(this->devHandle))->setGimabalPos(pos);
}
uint32_t amovGimbal::IamovGimbalBase::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
{
return 0;
}
uint32_t amovGimbal::gimbal::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
{
return ((amovGimbalBase *)(this->devHandle))->setGimabalSpeed(speed);
}
uint32_t amovGimbal::IamovGimbalBase::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
{
return 0;
}
uint32_t amovGimbal::gimbal::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
{
return ((amovGimbalBase *)(this->devHandle))->setGimabalFollowSpeed(followSpeed);
}
uint32_t amovGimbal::IamovGimbalBase::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
{
return 0;
}
uint32_t amovGimbal::gimbal::setGimabalHome(void)
{
return ((amovGimbalBase *)(this->devHandle))->setGimabalHome();
}
uint32_t amovGimbal::IamovGimbalBase::setGimabalHome(void)
{
return 0;
}
uint32_t amovGimbal::gimbal::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
return ((amovGimbalBase *)(this->devHandle))->setGimbalZoom(zoom, targetRate);
}
uint32_t amovGimbal::IamovGimbalBase::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
return 0;
}
uint32_t amovGimbal::gimbal::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
return ((amovGimbalBase *)(this->devHandle))->setGimbalFocus(zoom, targetRate);
}
uint32_t amovGimbal::IamovGimbalBase::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
{
return 0;
}
uint32_t amovGimbal::gimbal::setGimbalROI(const AMOV_GIMBAL_ROI_T area)
{
return ((amovGimbalBase *)(this->devHandle))->setGimbalROI(area);
}
uint32_t amovGimbal::IamovGimbalBase::setGimbalROI(const AMOV_GIMBAL_ROI_T area)
{
return 0;
}
uint32_t amovGimbal::gimbal::takePic(void)
{
return ((amovGimbalBase *)(this->devHandle))->takePic();
}
uint32_t amovGimbal::IamovGimbalBase::takePic(void)
{
return 0;
}
uint32_t amovGimbal::gimbal::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
{
return ((amovGimbalBase *)(this->devHandle))->setVideo(newState);
}
uint32_t amovGimbal::IamovGimbalBase::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
{
return 0;
}
uint32_t amovGimbal::gimbal::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
{
return ((amovGimbalBase *)(this->devHandle))->attitudeCorrection(quaterion, speed, acc, extenData);
}
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::gimbal::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
{
return ((amovGimbalBase *)(this->devHandle))->attitudeCorrection(pos, speed, acc, extenData);
}
uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
{
return 0;
}
uint32_t amovGimbal::gimbal::setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt)
{
return ((amovGimbalBase *)(this->devHandle))->setGNSSInfo(lng, lat, alt, nState, relAlt);
}
uint32_t amovGimbal::IamovGimbalBase::setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt)
{
return 0;
}
uint32_t amovGimbal::gimbal::extensionFuntions(void *cmd)
{
return ((amovGimbalBase *)(this->devHandle))->extensionFuntions(cmd);
}
uint32_t amovGimbal::IamovGimbalBase::extensionFuntions(void *cmd)
{
return 0;
}
bool amovGimbal::gimbal::setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos)
{
return ((amovGimbalBase *)(this->devHandle))->setGimbalPosBlock(pos);
}
bool amovGimbal::IamovGimbalBase::setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos)
{
return false;
}
bool amovGimbal::gimbal::setGimabalHomeBlock(void)
{
return ((amovGimbalBase *)(this->devHandle))->setGimabalHomeBlock();
}
bool amovGimbal::IamovGimbalBase::setGimabalHomeBlock(void)
{
return false;
}
bool amovGimbal::gimbal::setGimbalZoomBlock(float targetRate)
{
return ((amovGimbalBase *)(this->devHandle))->setGimbalZoomBlock(targetRate);
}
bool amovGimbal::IamovGimbalBase::setGimbalZoomBlock(float targetRate)
{
return false;
}
bool amovGimbal::gimbal::takePicBlock(void)
{
return ((amovGimbalBase *)(this->devHandle))->takePicBlock();
}
bool amovGimbal::IamovGimbalBase::takePicBlock(void)
{
return false;
}
bool amovGimbal::gimbal::calibrationBlock(void)
{
return ((amovGimbalBase *)(this->devHandle))->calibrationBlock();
}
bool amovGimbal::IamovGimbalBase::calibrationBlock(void)
{
return false;
}

152
gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp Normal file
View File

@ -0,0 +1,152 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-11-27 12:28:32
* @LastEditors: L LC @amov
* @LastEditTime: 2023-12-05 17:21:42
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp
*/
#include "amov_gimbal_private.h"
#include <string>
void amovGimbalSetRcvBytes(pAmovGimbalInputBytesInvoke callbaclk, void *handle, void *caller)
{
((amovGimbal::gimbal *)handle)->setRcvBytes(callbaclk, caller);
}
void amovGimbalSetSendBytes(pAmovGimbalOutputBytesInvoke callbaclk, void *handle, void *caller)
{
((amovGimbal::gimbal *)handle)->setSendBytes(callbaclk, caller);
}
void amovGimbalInBytesCallback(uint8_t *pData, uint32_t len, void *handle)
{
amovGimbal::gimbal::inBytesCallback(pData, len, (amovGimbal::gimbal *)handle);
}
void amovGimbalCreat(char *type, uint32_t selfId, uint32_t gimbalId, void *handle, void *caller)
{
std::string strType = type;
handle = new amovGimbal::gimbal(strType, nullptr, selfId, gimbalId);
}
void amovGimbalStart(pAmovGimbalStateInvoke callback, void *handle, void *caller)
{
((amovGimbal::gimbal *)handle)->startStack();
((amovGimbal::gimbal *)handle)->parserAuto(callback, caller);
}
void amovGimbalChangeStateCallback(pAmovGimbalStateInvoke callback, void *handle, void *caller)
{
((amovGimbal::gimbal *)handle)->setParserCallback(callback, caller);
}
void amovGimbalSetMsgCallback(pAmovGimbalMsgInvoke callback, void *handle, void *caller)
{
((amovGimbal::gimbal *)handle)->setMsgCallback(callback, caller);
}
uint32_t amovGimbalSetGimabalPos(AMOV_GIMBAL_POS_T *pos, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimabalPos(*pos);
}
uint32_t amovGimbalSetGimabalSpeed(AMOV_GIMBAL_POS_T *speed, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimabalSpeed(*speed);
}
uint32_t amovGimbalSetGimabalFollowSpeed(AMOV_GIMBAL_POS_T *followSpeed, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimabalFollowSpeed(*followSpeed);
}
uint32_t amovGimbalSetGimabalHome(void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimabalHome();
}
uint32_t amovGimbalSetGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimbalZoom(zoom, targetRate);
}
uint32_t amovGimbalSetGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimbalFocus(zoom, targetRate);
}
uint32_t amovGimbalSetGimbalROI(AMOV_GIMBAL_ROI_T *area, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimbalROI(*area);
}
uint32_t amovGimbalTakePic(void *handle)
{
return ((amovGimbal::gimbal *)handle)->takePic();
}
uint32_t amovGimbalSetVideo(AMOV_GIMBAL_VIDEO_T newState, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setVideo(newState);
}
uint32_t amovGimbalAttitudeCorrectionQ(AMOV_GIMBAL_QUATERNION_T *quaterion,
AMOV_GIMBAL_VELOCITY_T *speed,
AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle)
{
return ((amovGimbal::gimbal *)handle)->attitudeCorrection(*quaterion, *speed, *acc, extenData);
}
uint32_t amovGimbalAttitudeCorrectionE(AMOV_GIMBAL_POS_T *pos,
AMOV_GIMBAL_VELOCITY_T *speed,
AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle)
{
return ((amovGimbal::gimbal *)handle)->attitudeCorrection(*pos, *speed, *acc, extenData);
}
uint32_t amovGimbalSetGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGNSSInfo(lng, lat, alt, nState, relAlt);
}
uint32_t amovGimbalExtensionFuntions(void *cmd, void *handle)
{
return ((amovGimbal::gimbal *)handle)->extensionFuntions(cmd);
}
void getGimabalState(AMOV_GIMBAL_STATE_T *state, void *handle)
{
*state = ((amovGimbal::gimbal *)handle)->getGimabalState();
}
void getGimbalType(char *type, void *handle)
{
std::string temp = ((amovGimbal::gimbal *)handle)->name();
temp.copy(type, temp.size(), 0);
}
bool amovGimbalSetGimbalPosBlock(AMOV_GIMBAL_POS_T *pos, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimbalPosBlock(*pos);
}
bool amovGimbalSetGimabalHomeBlock(void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimabalHomeBlock();
}
bool amovGimbalSetGimbalZoomBlock(float targetRate, void *handle)
{
return ((amovGimbal::gimbal *)handle)->setGimbalZoomBlock(targetRate);
}
bool amovGimbalTakePicBlock(void *handle)
{
return ((amovGimbal::gimbal *)handle)->takePicBlock();
}
bool amovGimbalCalibrationBlock(void *handle)
{
return ((amovGimbal::gimbal *)handle)->calibrationBlock();
}

57
gimbal_ctrl/driver/src/amov_gimbal_private.h
View File

@ -3,7 +3,7 @@
* @Author : Aiyangsky
* @Date : 2023-05-13 10:39:20
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:45:18
* @LastEditTime: 2023-12-05 17:18:06
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_private.h
*/
#ifndef __AMOV_GIMABL_PRIVATE_H
@ -12,12 +12,12 @@
#include <stdint.h>
#include <stdbool.h>
#include <iostream>
#include <thread>
#include <unistd.h>
#include <mutex>
#include "amov_gimbal.h"
#include "amovGimbal/amov_gimbal.h"
#include "amovGimbal/amov_gimbal_c.h"
#include "Ring_Fifo.h"
#include "amov_tool.h"
@ -28,9 +28,19 @@ namespace amovGimbal
public:
AMOV_GIMBAL_STATE_T state;
std::mutex mState;
IOStreamBase *IO;
pStateInvoke updateGimbalStateCallback;
pMsgInvoke msgCustomCallback = idleMsgCallback;
// IO类
IOStreamBase *IO = nullptr;
// 适用于C的函数指针
void *inBytesCaller = nullptr;
pAmovGimbalInputBytesInvoke inBytes = nullptr;
void *outBytesCaller = nullptr;
pAmovGimbalOutputBytesInvoke outBytes = nullptr;
void *updataCaller = nullptr;
pAmovGimbalStateInvoke updateGimbalStateCallback;
void *msgCaller = nullptr;
pAmovGimbalMsgInvoke msgCustomCallback = idleMsgCallback;
fifoRing *rxQueue;
fifoRing *txQueue;
@ -39,7 +49,7 @@ namespace amovGimbal
std::thread::native_handle_type sendThreadHanle = 0;
std::thread::native_handle_type stackThreadHanle = 0;
PamovGimbalBase(SET IOStreamBase *_IO)
PamovGimbalBase(IOStreamBase *_IO)
{
IO = _IO;
}
@ -61,6 +71,37 @@ namespace amovGimbal
}
};
// Device interface
class IamovGimbalBase
{
public:
IamovGimbalBase() {}
virtual ~IamovGimbalBase() {}
// non-block functions
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);
virtual uint32_t setGimabalHome(void);
virtual uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
virtual uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
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 setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
virtual uint32_t extensionFuntions(void *cmd);
// block functions
virtual bool setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos);
virtual bool setGimabalHomeBlock(void);
virtual bool setGimbalZoomBlock(float targetRate);
virtual bool takePicBlock(void);
virtual bool calibrationBlock(void);
};
class amovGimbalBase : public IamovGimbalBase, public PamovGimbalBase
{
public:
@ -77,7 +118,7 @@ namespace amovGimbal
virtual void mainLoop(void);
virtual void stackStart(void);
virtual void parserStart(amovGimbal::pStateInvoke callback);
virtual void parserStart(pAmovGimbalStateInvoke callback, void *caller);
virtual void nodeSet(SET uint32_t _self, SET uint32_t _remote);

197
gimbal_ctrl/driver/src/amov_gimbal_realize.cpp Normal file
View File

@ -0,0 +1,197 @@
/*
* @Description:
* @Author: L LC @amov
* @Date: 2023-11-24 15:55:37
* @LastEditors: L LC @amov
* @LastEditTime: 2023-12-05 17:19:19
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_realize.cpp
*/
#include "amov_gimbal_private.h"
#include <thread>
#define MAX_PACK_SIZE 280
/**
* 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 == nullptr)
{
while (1)
{
if (txQueue->outCell(&tempBuffer))
{
this->outBytes((uint8_t *)&tempBuffer, calPackLen(tempBuffer), outBytesCaller);
}
}
}
else
{
while (1)
{
if (!IO->isBusy() && IO->isOpen())
{
if (txQueue->outCell(&tempBuffer))
{
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::amovGimbalBase::parserLoop(void)
{
uint8_t temp[65536];
uint32_t i = 0, getCount = 0;
if (IO == nullptr)
{
while (1)
{
getCount = inBytes(temp, inBytesCaller);
for (i = 0; i < getCount; i++)
{
parser(temp[i]);
}
}
}
else
{
while (1)
{
getCount = IO->inPutBytes(temp);
for (i = 0; i < getCount; i++)
{
parser(temp[i]);
}
}
}
}
void amovGimbal::amovGimbalBase::sendLoop(void)
{
send();
}
void amovGimbal::amovGimbalBase::mainLoop(void)
{
uint8_t tempBuffer[MAX_PACK_SIZE];
while (1)
{
if (getRxPack(tempBuffer))
{
msgCustomCallback(tempBuffer, msgCaller);
convert(tempBuffer);
}
}
}
void amovGimbal::amovGimbalBase::stackStart(void)
{
if (!this->IO->isOpen() && this->IO != nullptr)
{
this->IO->open();
}
// 当且仅当需要库主动查询时才启用解析器线程
if (inBytes != nullptr || this->IO != nullptr)
{
std::thread parserLoop(&amovGimbalBase::parserLoop, this);
this->parserThreadHanle = parserLoop.native_handle();
parserLoop.detach();
}
std::thread sendLoop(&amovGimbalBase::sendLoop, this);
this->sendThreadHanle = sendLoop.native_handle();
sendLoop.detach();
}
void amovGimbal::amovGimbalBase::parserStart(pAmovGimbalStateInvoke callback, void *caller)
{
this->updateGimbalStateCallback = callback;
this->updataCaller = caller;
std::thread mainLoop(&amovGimbalBase::mainLoop, this);
this->stackThreadHanle = mainLoop.native_handle();
mainLoop.detach();
}
void amovGimbal::amovGimbalBase::nodeSet(SET uint32_t _self, SET uint32_t _remote)
{
return;
}
void amovGimbal::gimbal::setRcvBytes(pAmovGimbalInputBytesInvoke callbaclk, void *caller)
{
((amovGimbal::amovGimbalBase *)(this->devHandle))->inBytes = callbaclk;
((amovGimbal::amovGimbalBase *)(this->devHandle))->inBytesCaller = caller;
}
void amovGimbal::gimbal::setSendBytes(pAmovGimbalOutputBytesInvoke callbaclk, void *caller)
{
((amovGimbal::amovGimbalBase *)(this->devHandle))->outBytes = callbaclk;
((amovGimbal::amovGimbalBase *)(this->devHandle))->outBytesCaller = caller;
}
void amovGimbal::gimbal::inBytesCallback(uint8_t *pData, uint32_t len, gimbal *handle)
{
uint32_t i = 0;
for (i = 0; i < len; i++)
{
((amovGimbalBase *)((handle)->devHandle))->parser(pData[i]);
}
}

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

@ -1,104 +0,0 @@
/*
* @Description: Common Data Structures of gimbal
* @Author: L LC @amov
* @Date: 2022-10-31 11:56:43
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:44:30
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_struct.h
*/
#include <stdint.h>
#ifndef __AMOV_GIMABL_STRUCT_H
#define __AMOV_GIMABL_STRUCT_H
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
{
AMOV_GIMBAL_SERVO_MODE_FPV = 0X10,
AMOV_GIMBAL_SERVO_MODE_LOCK = 0X11,
AMOV_GIMBAL_SERVO_MODE_FOLLOW = 0X12,
AMOV_GIMBAL_SERVO_MODE_OVERLOOK = 0X13,
AMOV_GIMBAL_SERVO_MODE_EULER = 0X14,
AMOV_GIMBAL_SERVO_MODE_WATCH = 0X16,
AMOV_GIMBAL_SERVO_MODE_TRACK = 0X17,
} AMOV_GIMBAL_SERVO_MODE_T;
typedef enum
{
AMOV_GIMBAL_CAMERA_FLAG_INVERSION = 0X1000,
AMOV_GIMBAL_CAMERA_FLAG_IR = 0X0200,
AMOV_GIMBAL_CAMERA_FLAG_RF = 0X0100,
AMOV_GIMBAL_CAMERA_FLAG_LOCK = 0X0001,
} AMOV_GIMBAL_CAMERA_FLAG_T;
typedef enum
{
AMOV_GIMBAL_VIDEO_TAKE,
AMOV_GIMBAL_VIDEO_OFF
} AMOV_GIMBAL_VIDEO_T;
typedef enum
{
AMOV_GIMBAL_ZOOM_IN,
AMOV_GIMBAL_ZOOM_OUT,
AMOV_GIMBAL_ZOOM_STOP
} AMOV_GIMBAL_ZOOM_T;
typedef struct
{
double yaw;
double roll;
double pitch;
} AMOV_GIMBAL_POS_T;
typedef struct
{
double x;
double y;
} AMOV_GIMBAL_FOV_T;
typedef struct
{
AMOV_GIMBAL_SERVO_MODE_T workMode;
AMOV_GIMBAL_CAMERA_FLAG_T cameraFlag;
AMOV_GIMBAL_VIDEO_T video;
AMOV_GIMBAL_POS_T abs;
AMOV_GIMBAL_POS_T rel;
AMOV_GIMBAL_POS_T relSpeed;
AMOV_GIMBAL_POS_T maxFollow;
AMOV_GIMBAL_FOV_T fov;
} AMOV_GIMBAL_STATE_T;
typedef struct
{
uint32_t centreX;
uint32_t centreY;
uint32_t hight;
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; // or N
double y; // or E
double z; // or UP
} AMOV_GIMBAL_VELOCITY_T;
} // namespace amovGimbal
#endif

5
gimbal_ctrl/driver/src/amov_tool.h
View File

@ -3,10 +3,12 @@
* @Author: L LC @amov
* @Date: 2023-07-31 18:30:33
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:44:49
* @LastEditTime: 2023-12-05 16:26:05
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_tool.h
*/
#ifndef __AMOVGIMABL_TOOL_H
#define __AMOVGIMABL_TOOL_H
namespace amovGimbalTools
{
static inline unsigned short conversionBigLittle(unsigned short value)
@ -27,3 +29,4 @@ namespace amovGimbalTools
return temp;
}
}
#endif

158
gimbal_ctrl/sv_gimbal.cpp
View File

@ -3,11 +3,10 @@
* @Author: L LC @amov
* @Date: 2023-04-12 09:12:52
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:43:39
* @LastEditTime: 2023-12-05 17:33:29
* @FilePath: /SpireCV/gimbal_ctrl/sv_gimbal.cpp
*/
#include "amov_gimbal.h"
#include "amov_gimbal_struct.h"
#include "amovGimbal/amov_gimbal.h"
#include "sv_gimbal.h"
#include "sv_gimbal_io.hpp"
@ -20,19 +19,42 @@ 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"},
{GimbalType::GX40, "GX40"}};
std::string &cvGimbalType2Str(const GimbalType &type)
typedef struct
{
std::map<GimbalType, std::string>::iterator temp = gimbaltypeList.find(type);
std::string name;
GimbalLink supportLink;
} gimbalTrait;
std::map<GimbalType, gimbalTrait> gimbaltypeList =
{
{GimbalType::G1, {"G1", GimbalLink::SERIAL}},
{GimbalType::Q10f, {"Q10f", GimbalLink::SERIAL}},
{GimbalType::AT10, {"AT10", GimbalLink::SERIAL | GimbalLink::ETHERNET_TCP}},
{GimbalType::GX40, {"GX40", GimbalLink::SERIAL | GimbalLink::ETHERNET_TCP | GimbalLink::ETHERNET_UDP}}};
/**
* The function `svGimbalType2Str` converts a `GimbalType` enum value to its corresponding string
* representation.
*
* @return a reference to a string.
*/
std::string &svGimbalType2Str(const GimbalType &type)
{
std::map<GimbalType, gimbalTrait>::iterator temp = gimbaltypeList.find(type);
if (temp != gimbaltypeList.end())
{
return (temp->second);
return (temp->second).name;
}
throw "Error: Unsupported gimbal device type!!!!";
exit(-1);
}
GimbalLink &svGimbalTypeFindLinkType(const GimbalType &type)
{
std::map<GimbalType, gimbalTrait>::iterator temp = gimbaltypeList.find(type);
if (temp != gimbaltypeList.end())
{
return (temp->second).supportLink;
}
throw "Error: Unsupported gimbal device type!!!!";
exit(-1);
@ -101,17 +123,29 @@ void sv::Gimbal::setNetIp(const std::string &ip)
}
/**
* This function sets the network port for a Gimbal object in C++.
* The function sets the TCP network port for the Gimbal object.
*
* @param port The "port" parameter is an integer value that represents the network port number that
* the Gimbal object will use for communication. This function sets the value of the "m_net_port"
* member variable of the Gimbal object to the value passed in as the "port" parameter.
* @param port The parameter "port" is an integer that represents the TCP network port number.
*/
void sv::Gimbal::setNetPort(const int &port)
void sv::Gimbal::setTcpNetPort(const int &port)
{
this->m_net_port = port;
}
/**
* The function sets the UDP network ports for receiving and sending data.
*
* @param recvPort The recvPort parameter is the port number that the Gimbal object will use to receive
* UDP packets.
* @param sendPort The sendPort parameter is the port number used for sending data over UDP (User
* Datagram Protocol) network communication.
*/
void sv::Gimbal::setUdpNetPort(const int &recvPort, const int &sendPort)
{
this->m_net_recv_port = recvPort;
this->m_net_send_port = sendPort;
}
/**
* The function sets a parser callback for a gimbal device.
*
@ -122,25 +156,31 @@ void sv::Gimbal::setNetPort(const int &port)
void sv::Gimbal::setStateCallback(sv::PStateInvoke callback)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
pdevTemp->setParserCallback(callback);
m_callback = callback;
pdevTemp->setParserCallback(sv::Gimbal::gimbalUpdataCallback, this);
}
/**
* 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.
* The function `sv::Gimbal::creatIO` creates an IO object based on the specified gimbal type and link
* type, and returns a pointer to the created object.
*
* @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.
* @param dev The "dev" parameter is a pointer to an object of type "sv::Gimbal". It is used to access
* the member variables of the Gimbal object, such as "m_serial_port", "m_serial_baud_rate",
* "m_serial_timeout", etc. These variables store information about
*
* @return a boolean value.
* @return a void pointer.
*/
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);
GimbalLink link = svGimbalTypeFindLinkType(dev->m_gimbal_type);
if ((dev->m_gimbal_link & svGimbalTypeFindLinkType(dev->m_gimbal_type)) == GimbalLink::NONE)
{
throw std::runtime_error("gimbal Unsupported linktype !!!");
}
if (list == IOList.end())
{
@ -160,20 +200,35 @@ void *sv::Gimbal::creatIO(sv::Gimbal *dev)
}
else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP)
{
TCPClient *tcp;
tcp = new TCPClient(dev->m_net_ip, dev->m_net_port);
key.first = dev->m_net_ip;
key.second = (void *)tcp;
IOList.insert(key);
}
else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP)
{
UDP *udp;
udp = new UDP(dev->m_net_ip, dev->m_net_recv_port, dev->m_net_send_port);
key.first = dev->m_net_ip;
key.second = (void *)udp;
IOList.insert(key);
}
else
{
std::cout << "Error: gimbal IO has opened!!!" << std::endl;
}
}
else
{
std::cout << "Error: gimbal IO has opened!!!" << std::endl;
}
IOListMutex.unlock();
return key.second;
}
/**
* The function removes a Gimbal device from the IOList.
*
* @param dev dev is a pointer to an object of type sv::Gimbal.
*/
void sv::Gimbal::removeIO(sv::Gimbal *dev)
{
IOListMutex.lock();
@ -181,6 +236,7 @@ void sv::Gimbal::removeIO(sv::Gimbal *dev)
IOListMutex.unlock();
}
/**
* The function opens a communication interface with a gimbal device and sets up a parser to handle
* incoming data.
@ -192,7 +248,6 @@ void sv::Gimbal::removeIO(sv::Gimbal *dev)
*
* @return A boolean value is being returned.
*/
#include "stdlib.h"
bool sv::Gimbal::open(PStateInvoke callback)
{
bool ret = false;
@ -202,11 +257,12 @@ bool sv::Gimbal::open(PStateInvoke callback)
if (this->IO != nullptr)
{
std::string driverName;
driverName = sv::cvGimbalType2Str(this->m_gimbal_type);
driverName = sv::svGimbalType2Str(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);
m_callback = callback;
pdevTemp->parserAuto(sv::Gimbal::gimbalUpdataCallback, this);
ret = true;
}
@ -224,7 +280,7 @@ bool sv::Gimbal::open(PStateInvoke callback)
bool sv::Gimbal::setHome()
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
if (pdevTemp->dev->setGimabalHome() > 0)
if (pdevTemp->setGimabalHome() > 0)
{
return true;
}
@ -253,7 +309,7 @@ bool sv::Gimbal::setZoom(double x)
return false;
}
if (pdevTemp->dev->setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_STOP, x) > 0)
if (pdevTemp->setGimbalZoom(AMOV_GIMBAL_ZOOM_STOP, x) > 0)
{
return true;
}
@ -277,7 +333,7 @@ bool sv::Gimbal::setAutoZoom(int state)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
if (pdevTemp->dev->setGimbalZoom((amovGimbal::AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0)
if (pdevTemp->setGimbalZoom((AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0)
{
return true;
}
@ -303,7 +359,7 @@ bool sv::Gimbal::setAutoFocus(int state)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
if (pdevTemp->dev->setGimbalFocus((amovGimbal::AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0)
if (pdevTemp->setGimbalFocus((AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0)
{
return true;
}
@ -323,7 +379,7 @@ bool sv::Gimbal::takePhoto()
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
if (pdevTemp->dev->takePic() > 0)
if (pdevTemp->takePic() > 0)
{
return true;
}
@ -347,21 +403,21 @@ bool sv::Gimbal::takeVideo(int state)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
amovGimbal::AMOV_GIMBAL_VIDEO_T newState;
AMOV_GIMBAL_VIDEO_T newState;
switch (state)
{
case 0:
newState = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
newState = AMOV_GIMBAL_VIDEO_OFF;
break;
case 1:
newState = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE;
newState = AMOV_GIMBAL_VIDEO_TAKE;
break;
default:
newState = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
newState = AMOV_GIMBAL_VIDEO_OFF;
break;
}
if (pdevTemp->dev->setVideo(newState) > 0)
if (pdevTemp->setVideo(newState) > 0)
{
return true;
}
@ -382,13 +438,13 @@ int sv::Gimbal::getVideoState()
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
int ret;
amovGimbal::AMOV_GIMBAL_STATE_T state;
AMOV_GIMBAL_STATE_T state;
state = pdevTemp->getGimabalState();
if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
if (state.video == AMOV_GIMBAL_VIDEO_TAKE)
{
ret = 1;
}
else if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_OFF)
else if (state.video == AMOV_GIMBAL_VIDEO_OFF)
{
ret = 0;
}
@ -416,7 +472,7 @@ void sv::Gimbal::setAngleEuler(double roll, double pitch, double yaw,
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
amovGimbal::AMOV_GIMBAL_POS_T temp;
AMOV_GIMBAL_POS_T temp;
if (pitch_rate == 0.0f)
pitch_rate = 360;
@ -428,11 +484,11 @@ void sv::Gimbal::setAngleEuler(double roll, double pitch, double yaw,
temp.pitch = pitch_rate;
temp.roll = roll_rate;
temp.yaw = yaw_rate;
pdevTemp->dev->setGimabalFollowSpeed(temp);
pdevTemp->setGimabalFollowSpeed(temp);
temp.pitch = pitch;
temp.roll = roll;
temp.yaw = yaw;
pdevTemp->dev->setGimabalPos(temp);
pdevTemp->setGimabalPos(temp);
}
/**
@ -447,11 +503,11 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
amovGimbal::AMOV_GIMBAL_POS_T temp;
AMOV_GIMBAL_POS_T temp;
temp.pitch = pitch_rate;
temp.roll = roll_rate;
temp.yaw = yaw_rate;
pdevTemp->dev->setGimabalSpeed(temp);
pdevTemp->setGimabalSpeed(temp);
}
sv::Gimbal::~Gimbal()

257
gimbal_ctrl/sv_gimbal_io.hpp
View File

@ -3,65 +3,236 @@
* @Author: L LC @amov
* @Date: 2023-04-12 12:22:09
* @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:43:49
* @LastEditTime: 2023-12-05 17:38:59
* @FilePath: /SpireCV/gimbal_ctrl/sv_gimbal_io.hpp
*/
#ifndef __SV_GIMABL_IO_H
#define __SV_GIMABL_IO_H
#include "amov_gimbal.h"
#include "amovGimbal/amov_gimbal.h"
#include "serial/serial.h"
class UART : public amovGimbal::IOStreamBase
#include <string.h>
#include <stdio.h>
#if defined(_WIN32)
#include <winsock2.h>
#include <ws2tcpip.h>
#endif
#if defined(__linux__)
#include <arpa/inet.h>
#endif
#include <unistd.h>
namespace sv
{
private:
serial::Serial *ser;
class UART : public amovGimbal::IOStreamBase
{
private:
serial::Serial *ser;
public:
virtual bool open()
{
ser->open();
return true;
}
virtual bool close()
{
ser->close();
return true;
}
virtual bool isOpen()
{
return ser->isOpen();
}
virtual bool isBusy()
{
return false;
}
public:
virtual bool open()
{
ser->open();
return true;
}
virtual bool close()
{
ser->close();
return true;
}
virtual bool isOpen()
{
return ser->isOpen();
}
virtual bool isBusy()
{
return false;
}
virtual uint32_t inPutBytes(IN uint8_t *byte)
{
if (ser->read(byte, 1))
{
return 1;
}
return 0;
}
virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
{
return ser->write(byte, lenght);
}
UART(const std::string &port, uint32_t baudrate, serial::Timeout timeout,
serial::bytesize_t bytesize, serial::parity_t parity, serial::stopbits_t stopbits,
serial::flowcontrol_t flowcontrol)
{
ser = new serial::Serial(port, baudrate, timeout, bytesize, parity, stopbits, flowcontrol);
}
~UART()
{
ser->close();
}
};
virtual bool inPutByte(IN uint8_t *byte)
int scoketClose(int scoket)
{
if (ser->read(byte, 1))
#if defined(_WIN32)
return closesocket(scoket);
#endif
#if defined(__linux__)
return close(scoket);
#endif
}
class TCPClient : public amovGimbal::IOStreamBase
{
private:
int scoketFd;
sockaddr_in scoketAddr;
public:
virtual bool open()
{
return true;
}
return false;
}
virtual bool close()
{
sv::scoketClose(scoketFd);
return true;
}
virtual bool isOpen()
{
return true;
}
virtual bool isBusy()
{
return false;
}
virtual uint32_t inPutBytes(IN uint8_t *byte)
{
return recv(scoketFd, (char *)byte, 65536, 0);
}
virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
{
return send(scoketFd, (const char *)byte, lenght, 0);
}
virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
{
return ser->write(byte, lenght);
}
TCPClient(const std::string &addr, const uint16_t port)
{
if ((scoketFd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
{
throw std::runtime_error("scoket creat failed");
}
memset(&scoketAddr, 0, sizeof(scoketAddr));
scoketAddr.sin_family = AF_INET;
scoketAddr.sin_addr.s_addr = inet_addr(addr.c_str());
UART(const std::string &port, uint32_t baudrate, serial::Timeout timeout,
serial::bytesize_t bytesize, serial::parity_t parity, serial::stopbits_t stopbits,
serial::flowcontrol_t flowcontrol)
if (scoketAddr.sin_addr.s_addr == INADDR_NONE ||
scoketAddr.sin_addr.s_addr == INADDR_ANY)
{
sv::scoketClose(scoketFd);
throw std::runtime_error("scoket addr errr");
}
scoketAddr.sin_port = htons(port);
if (connect(scoketFd, (struct sockaddr *)&scoketAddr, sizeof(scoketAddr)) != 0)
{
sv::scoketClose(scoketFd);
throw std::runtime_error("scoket connect failed !");
}
}
~TCPClient()
{
close();
}
};
class UDP : public amovGimbal::IOStreamBase
{
ser = new serial::Serial(port, baudrate, timeout, bytesize, parity, stopbits, flowcontrol);
}
~UART()
{
ser->close();
delete ser;
}
};
private:
int rcvScoketFd, sendScoketFd;
sockaddr_in rcvScoketAddr, sendScoketAddr;
public:
virtual bool open()
{
return true;
}
virtual bool close()
{
sv::scoketClose(rcvScoketFd);
sv::scoketClose(sendScoketFd);
return true;
}
virtual bool isOpen()
{
return true;
}
virtual bool isBusy()
{
return false;
}
virtual uint32_t inPutBytes(IN uint8_t *byte)
{
sockaddr_in remoteAddr;
int len = sizeof(remoteAddr);
return recvfrom(rcvScoketFd, (char *)byte, 65536, 0,
(struct sockaddr *)&remoteAddr, reinterpret_cast<socklen_t *>(&len));
}
virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
{
return sendto(sendScoketFd, (const char *)byte, lenght, 0,
(struct sockaddr *)&sendScoketAddr, sizeof(sendScoketAddr));
}
UDP(const std::string &remoteAddr, const uint16_t rcvPort, uint16_t sendPort)
{
if ((rcvScoketFd = socket(AF_INET, SOCK_DGRAM, 0)) == -1 ||
(sendScoketFd = socket(AF_INET, SOCK_DGRAM, 0)) == -1)
{
sv::scoketClose(rcvScoketFd);
sv::scoketClose(sendScoketFd);
throw std::runtime_error("scoket creat failed");
}
memset(&rcvScoketAddr, 0, sizeof(rcvScoketAddr));
memset(&sendScoketAddr, 0, sizeof(sendScoketAddr));
sendScoketAddr.sin_family = AF_INET;
sendScoketAddr.sin_addr.s_addr = inet_addr(remoteAddr.c_str());
sendScoketAddr.sin_port = htons(sendPort);
if (sendScoketAddr.sin_addr.s_addr == INADDR_NONE ||
sendScoketAddr.sin_addr.s_addr == INADDR_ANY)
{
sv::scoketClose(sendScoketFd);
throw std::runtime_error("scoket addr errr");
}
rcvScoketAddr.sin_family = AF_INET;
rcvScoketAddr.sin_addr.s_addr = INADDR_ANY;
rcvScoketAddr.sin_port = htons(rcvPort);
if (rcvScoketAddr.sin_addr.s_addr == INADDR_NONE)
{
sv::scoketClose(rcvScoketFd);
throw std::runtime_error("scoket addr errr");
}
if (bind(rcvScoketFd, (struct sockaddr *)&rcvScoketAddr, sizeof(rcvScoketAddr)) == -1)
{
sv::scoketClose(rcvScoketFd);
throw std::runtime_error("scoket bind failed !");
}
}
~UDP()
{
close();
}
};
}
#endif

43
include/sv_gimbal.h
View File

@ -3,8 +3,8 @@
* @Author: jario-jin @amov
* @Date: 2023-04-12 09:12:52
* @LastEditors: L LC @amov
* @LastEditTime: 2023-04-18 11:49:27
* @FilePath: /spirecv-gimbal-sdk/include/sv_gimbal.h
* @LastEditTime: 2023-12-05 17:25:40
* @FilePath: /SpireCV/include/sv_gimbal.h
*/
#ifndef __SV_GIMBAL__
#define __SV_GIMBAL__
@ -29,13 +29,24 @@ namespace sv
AT10,
GX40,
};
enum class GimbalLink
enum class GimbalLink : int
{
SERIAL,
ETHERNET_TCP,
ETHERNET_UDP
NONE = 0x00,
SERIAL = 0x01,
ETHERNET_TCP = 0x02,
ETHERNET_UDP = 0x04,
};
constexpr GimbalLink operator|(GimbalLink a, GimbalLink b)
{
return static_cast<GimbalLink>(static_cast<int>(a) | static_cast<int>(b));
}
constexpr GimbalLink operator&(GimbalLink a, GimbalLink b)
{
return static_cast<GimbalLink>(static_cast<int>(a) & static_cast<int>(b));
}
enum class GimablSerialByteSize
{
FIVE_BYTES = 5,
@ -84,6 +95,7 @@ namespace sv
// Device pointers
void *dev;
void *IO;
PStateInvoke m_callback;
// Generic serial interface parameters list & default parameters
std::string m_serial_port = "/dev/ttyUSB0";
@ -95,8 +107,10 @@ namespace sv
int m_serial_timeout = 500;
// Ethernet interface parameters list & default parameters
std::string m_net_ip = "192.168.2.64";
int m_net_port = 9090;
std::string m_net_ip = "192.168.144.121";
int m_net_port = 2332;
int m_net_recv_port = 2338;
int m_net_send_port = 2337;
GimbalType m_gimbal_type;
GimbalLink m_gimbal_link;
@ -105,6 +119,14 @@ namespace sv
static std::mutex IOListMutex;
static void *creatIO(Gimbal *dev);
static void removeIO(Gimbal *dev);
static void gimbalUpdataCallback(double frameAngleRoll, double frameAnglePitch, double frameAngleYaw,
double imuAngleRoll, double imuAnglePitch, double imuAngleYaw,
double fovX, double fovY, void *handle)
{
((Gimbal *)(handle))->m_callback(frameAngleRoll, frameAnglePitch, frameAngleYaw, imuAngleRoll, imuAnglePitch, imuAngleYaw, fovX, fovY);
}
public:
//! Constructor
/*!
@ -126,7 +148,10 @@ namespace sv
// set Ethernet interface parameters
void setNetIp(const std::string &ip);
void setNetPort(const int &port);
// set tcp port
void setTcpNetPort(const int &port);
// set udp port
void setUdpNetPort(const int &recvPort, const int &sendPort);
// Create a device instance
void setStateCallback(PStateInvoke callback);