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 * @Author: L LC @amov
* @Date: 2022-10-27 18:10:06 * @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h
*/ */
#ifndef 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 * @Author: L LC @amov
* @Date: 2022-10-27 18:10:06 * @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp
*/ */
#include "AT10_gimbal_driver.h" #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); stdRxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE);
stdTxQueue = 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; 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) switch (temp->head)
{ {
case AT10::GIMBAL_CMD_RCV_STATE: case AT10::GIMBAL_CMD_RCV_STATE:
std::cout << "Undefined old frame from AT10"; std::cout << "Undefined old frame from AT10\r\n";
// AT10::GIMBAL_RCV_POS_MSG_T *tempPos;
// tempPos = reinterpret_cast<AT10::GIMBAL_RCV_POS_MSG_T *>(((uint8_t *)buf) + AT10_EXT_PAYLOAD_OFFSET);
// mState.lock();
// state.abs.yaw = tempPos->yawIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE;
// state.abs.roll = tempPos->rollIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE;
// state.abs.pitch = tempPos->pitchIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE;
// state.rel.yaw = tempPos->yawStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED;
// state.rel.roll = tempPos->rollStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED;
// state.rel.pitch = tempPos->pitchStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED;
// updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
// state.abs.roll, state.abs.pitch, state.abs.yaw,
// state.fov.x, state.fov.y);
// mState.unlock();
break; break;
case AT10::GIMBAL_CMD_STD: case AT10::GIMBAL_CMD_STD:
@ -123,15 +102,15 @@ void AT10GimbalDriver::convert(void *buf)
state.fov.y = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovY) * 0.1; state.fov.y = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovY) * 0.1;
if ((amovGimbalTools::conversionBigLittle(tempRcv->D1.camera) & 0X0003) == 0X01) if ((amovGimbalTools::conversionBigLittle(tempRcv->D1.camera) & 0X0003) == 0X01)
{ {
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; state.video = AMOV_GIMBAL_VIDEO_TAKE;
} }
else else
{ {
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; state.video = AMOV_GIMBAL_VIDEO_OFF;
} }
updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
state.abs.roll, state.abs.pitch, state.abs.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(); mState.unlock();
break; break;
@ -316,21 +295,9 @@ bool AT10GimbalDriver::parser(IN uint8_t byte)
void AT10GimbalDriver::sendHeart(void) void AT10GimbalDriver::sendHeart(void)
{ {
// uint8_t tempBuffer[72];
uint8_t temp = 0X00; uint8_t temp = 0X00;
while (1) 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)); std::this_thread::sleep_for(std::chrono::milliseconds(100));
pack(AT10::GIMBAL_CMD_STD_HEART, &temp, sizeof(temp)); 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 sendHeartLoop(&AT10GimbalDriver::sendHeart, this);
std::thread sendStdLoop(&AT10GimbalDriver::sendStd, this); std::thread sendStdLoop(&AT10GimbalDriver::sendStd, this);
this->sendThreadHanle = sendStdLoop.native_handle();
this->sendHreatThreadHandle = sendHeartLoop.native_handle();
sendHeartLoop.detach(); sendHeartLoop.detach();
sendStdLoop.detach(); sendStdLoop.detach();
} }
void AT10GimbalDriver::parserLoop(void) void AT10GimbalDriver::parserLoop(void)
{ {
uint8_t temp; uint8_t temp[65536];
uint32_t i = 0, getCount = 0;
while (1) 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)) if (stdRxQueue->outCell(tempBuffer))
{ {
msgCustomCallback(tempBuffer); msgCustomCallback(tempBuffer, msgCaller);
convert(tempBuffer); convert(tempBuffer);
} }
} }
@ -400,13 +374,13 @@ void AT10GimbalDriver::getExtRxPack(void)
{ {
if (rxQueue->outCell(tempBuffer)) if (rxQueue->outCell(tempBuffer))
{ {
msgCustomCallback(tempBuffer); msgCustomCallback(tempBuffer, msgCaller);
convert(tempBuffer); convert(tempBuffer);
} }
} }
} }
void AT10GimbalDriver::parserStart(amovGimbal::pStateInvoke callback) void AT10GimbalDriver::parserStart(pAmovGimbalStateInvoke callback)
{ {
this->updateGimbalStateCallback = callback; this->updateGimbalStateCallback = callback;
@ -414,6 +388,10 @@ void AT10GimbalDriver::parserStart(amovGimbal::pStateInvoke callback)
std::thread getStdRxPackLoop(&AT10GimbalDriver::getStdRxPack, this); std::thread getStdRxPackLoop(&AT10GimbalDriver::getStdRxPack, this);
std::thread getExtRxPackLooP(&AT10GimbalDriver::getExtRxPack, this); std::thread getExtRxPackLooP(&AT10GimbalDriver::getExtRxPack, this);
this->parserThreadHanle = parser.native_handle();
this->stackThreadHanle = getStdRxPackLoop.native_handle();
this->extStackThreadHandle = getExtRxPackLooP.native_handle();
parser.detach(); parser.detach();
getStdRxPackLoop.detach(); getStdRxPackLoop.detach();
getExtRxPackLooP.detach(); getExtRxPackLooP.detach();

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

@ -3,19 +3,18 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2022-10-28 12:24:21 * @Date: 2022-10-28 12:24:21
* @LastEditors: L LC @amov * @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 * @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 "../amov_gimbal_private.h"
#include "AT10_gimbal_struct.h" #include "AT10_gimbal_struct.h"
#include <mutex> #include <mutex>
#include <malloc.h> #include <malloc.h>
#include <iostream> #include <iostream>
#ifndef __AT10_DRIVER_H
#define __AT10_DRIVER_H
class AT10GimbalDriver : protected amovGimbal::amovGimbalBase class AT10GimbalDriver : protected amovGimbal::amovGimbalBase
{ {
private: private:
@ -32,12 +31,13 @@ private:
void sendHeart(void); void sendHeart(void);
void sendStd(void); void sendStd(void);
void parserStart(amovGimbal::pStateInvoke callback); void parserStart(pAmovGimbalStateInvoke callback);
void parserLoop(void); void parserLoop(void);
void getExtRxPack(void); void getExtRxPack(void);
void getStdRxPack(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); bool parser(IN uint8_t byte);
void convert(void *buf); void convert(void *buf);
@ -46,16 +46,16 @@ private:
public: public:
// funtions // funtions
uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed); uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
uint32_t setGimabalHome(void); uint32_t setGimabalHome(void);
uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalFocus(amovGimbal::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 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);
@ -76,6 +76,13 @@ public:
{ {
delete stdTxQueue; 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 * @Author: L LC @amov
* @Date: 2023-03-02 10:00:52 * @Date: 2023-03-02 10:00:52
* @LastEditors: L LC @amov * @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:48:03 * @LastEditTime: 2023-11-27 16:27:18
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_funtion.cpp
*/ */
#include "AT10_gimbal_driver.h" #include "AT10_gimbal_driver.h"
#include "AT10_gimbal_crc32.h" #include "AT10_gimbal_crc32.h"
@ -17,7 +17,7 @@
* *
* @return The return value is the number of bytes written to the buffer. * @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; int16_t yaw, pitch;
AT10::GIMBAL_CMD_A1_MSG_T temp; 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 * G1::GIMBAL_SET_POS_MSG_T
* *
* @param speed the speed of the gimbal * @param speed the speed of the gimbal
* *
* @return The return value is the number of bytes written to the buffer. * @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; int16_t speedYaw, speedPitch;
AT10::GIMBAL_CMD_A1_MSG_T temp; 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. * @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.pitch = fabs(followSpeed.pitch * 100);
state.maxFollow.yaw = fabs(followSpeed.yaw * 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. * @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; 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; 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)); 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) if (targetRate == 0.0f)
{ {
uint16_t temp = 0; uint16_t temp = 0;
switch (zoom) switch (zoom)
{ {
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN: case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
temp = 0X08 << 3; temp = 0X08 << 3;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT: case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
temp = 0X09 << 3; temp = 0X09 << 3;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP: case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
temp = 0X01 << 3; temp = 0X01 << 3;
break; break;
default: 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; uint16_t temp = 0;
switch (zoom) switch (zoom)
{ {
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN: case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
temp = 0X0B << 3; temp = 0X0B << 3;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT: case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
temp = 0X0A << 3; temp = 0X0A << 3;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP: case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
temp = 0X01 << 3; temp = 0X01 << 3;
break; break;
default: default:

4
gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h
View File

@ -3,8 +3,8 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2022-10-27 18:10:07 * @Date: 2022-10-27 18:10:07
* @LastEditors: L LC @amov * @LastEditors: L LC @amov
* @LastEditTime: 2023-11-28 11:48:08 * @LastEditTime: 2023-08-25 19:32:59
* @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_struct.h
*/ */
#ifndef AT10_GIMBAL_STRUCT_H #ifndef AT10_GIMBAL_STRUCT_H
#define 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 * @Author: L LC @amov
* @Date: 2022-10-27 18:10:06 * @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h
*/ */
#ifndef 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 * @Author: L LC @amov
* @Date: 2022-10-27 18:10:06 * @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp
*/ */
#include "g1_gimbal_driver.h" #include "g1_gimbal_driver.h"
@ -75,7 +75,7 @@ void g1GimbalDriver::convert(void *buf)
state.rel.pitch = tempPos->HALL_pitch * G1_SCALE_FACTOR; state.rel.pitch = tempPos->HALL_pitch * G1_SCALE_FACTOR;
updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
state.abs.roll, state.abs.pitch, state.abs.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(); mState.unlock();
break; break;

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

@ -3,19 +3,18 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2022-10-28 12:24:21 * @Date: 2022-10-28 12:24:21
* @LastEditors: L LC @amov * @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 * @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 "../amov_gimbal_private.h"
#include "g1_gimbal_struct.h" #include "g1_gimbal_struct.h"
#include <mutex> #include <mutex>
#include <malloc.h> #include <malloc.h>
#include <iostream> #include <iostream>
#ifndef __G1_DRIVER_H
#define __G1_DRIVER_H
class g1GimbalDriver : protected amovGimbal::amovGimbalBase class g1GimbalDriver : protected amovGimbal::amovGimbalBase
{ {
private: private:
@ -29,22 +28,22 @@ private:
public: public:
// funtions // funtions
uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed); uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
uint32_t setGimabalHome(void); uint32_t setGimabalHome(void);
uint32_t takePic(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, uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &speed,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData); void *extenData);
uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &seppd,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData); void *extenData);
uint32_t extensionFuntions(void *cmd); 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 * @Author: L LC @amov
* @Date: 2023-03-02 10:00:52 * @Date: 2023-03-02 10:00:52
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp
*/ */
#include "g1_gimbal_driver.h" #include "g1_gimbal_driver.h"
@ -18,7 +18,7 @@
* *
* @return The return value is the number of bytes written to the buffer. * @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; G1::GIMBAL_SET_POS_MSG_T temp;
temp.mode = G1::GIMBAL_CMD_POS_MODE_ANGLE; 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 * G1::GIMBAL_SET_POS_MSG_T
* *
* @param speed the speed of the gimbal * @param speed the speed of the gimbal
* *
* @return The return value is the number of bytes written to the buffer. * @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; G1::GIMBAL_SET_POS_MSG_T temp;
temp.mode = G1::GIMBAL_CMD_POS_MODE_SPEED; 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. * @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.pitch = followSpeed.pitch / G1_SCALE_FACTOR;
state.maxFollow.roll = followSpeed.roll / 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. * @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; uint8_t temp = G1::GIMBAL_CMD_CAMERA_REC;
mState.lock(); 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 else
{ {
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; state.video = AMOV_GIMBAL_VIDEO_TAKE;
} }
mState.unlock(); 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 * @param quaterion The "quaterion" parameter is a structure of type "AMOV_GIMBAL_QUATERNION_T" which
* contains the following fields: * 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 * the velocity of the gimbal. It contains three components: `x`, `y`, and `z`, which represent the
* velocity in the respective axes. * 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). * 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 * @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 * 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. * @return a uint32_t value.
*/ */
uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATERNION_T &quaterion, uint32_t g1GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &speed,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData) void *extenData)
{ {
G1::GIMBAL_ATT_CORR_MSG_T temp; 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 * The function `attitudeCorrection` calculates the attitude correction for a gimbal based on the given
* position, velocity, and acceleration values. * 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. * 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 * @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, * 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). * 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 * @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 * 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. * @return a uint32_t value.
*/ */
uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, uint32_t g1GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &seppd,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData) void *extenData)
{ {
G1::GIMBAL_ATT_CORR_MSG_T temp; 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 * @Author: L LC @amov
* @Date: 2022-10-27 18:10:07 * @Date: 2022-10-27 18:10:07
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h
*/ */
#ifndef 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 * @Author: L LC @amov
* @Date: 2023-10-20 16:33:07 * @Date: 2023-10-20 16:33:07
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h
*/ */
#ifndef 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 * @Author: L LC @amov
* @Date: 2023-10-20 16:08:17 * @Date: 2023-10-20 16:08:17
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp
*/ */
#include <string.h> #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 * pointer type. It is used to specify a callback function that will be invoked when the gimbal state
* is updated. * is updated.
*/ */
void GX40GimbalDriver::parserStart(amovGimbal::pStateInvoke callback) void GX40GimbalDriver::parserStart(pAmovGimbalStateInvoke callback)
{ {
this->updateGimbalStateCallback = callback; this->updateGimbalStateCallback = callback;
@ -160,8 +160,8 @@ void GX40GimbalDriver::convert(void *buf)
secondary = (GX40::GIMBAL_SECONDARY_SLAVE_FRAME_T *)temp->secondaryData; secondary = (GX40::GIMBAL_SECONDARY_SLAVE_FRAME_T *)temp->secondaryData;
mState.lock(); mState.lock();
this->state.workMode = (amovGimbal::AMOV_GIMBAL_SERVO_MODE_T)primary->workMode; this->state.workMode = (AMOV_GIMBAL_SERVO_MODE_T)primary->workMode;
this->state.cameraFlag = (amovGimbal::AMOV_GIMBAL_CAMERA_FLAG_T)primary->state; this->state.cameraFlag = (AMOV_GIMBAL_CAMERA_FLAG_T)primary->state;
// 应该需要再解算一下，才能出具体的框架角度 // 应该需要再解算一下，才能出具体的框架角度
this->state.rel.yaw = -(primary->motorYaw * XF_ANGLE_DPI); 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; 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, updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
state.abs.roll, state.abs.pitch, state.abs.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(); mState.unlock();
} }
@ -213,7 +213,6 @@ uint32_t GX40GimbalDriver::calPackLen(void *pack)
* *
* @return a boolean value, either true or false. * @return a boolean value, either true or false.
*/ */
#include <stdio.h>
bool GX40GimbalDriver::parser(IN uint8_t byte) bool GX40GimbalDriver::parser(IN uint8_t byte)
{ {
bool state = false; bool state = false;

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

@ -4,10 +4,13 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2023-10-20 16:08:13 * @Date: 2023-10-20 16:08:13
* @LastEditors: L LC @amov * @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 * @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 "../amov_gimbal_private.h"
#include "GX40_gimbal_struct.h" #include "GX40_gimbal_struct.h"
#include <mutex> #include <mutex>
@ -16,9 +19,6 @@
#include <chrono> #include <chrono>
#include <time.h> #include <time.h>
#ifndef __GX40_DRIVER_H
#define __GX40_DRIVER_H
class GX40GimbalDriver : public amovGimbal::amovGimbalBase class GX40GimbalDriver : public amovGimbal::amovGimbalBase
{ {
GX40::GIMBAL_FRAME_PARSER_STATE_T parserState; GX40::GIMBAL_FRAME_PARSER_STATE_T parserState;
@ -29,14 +29,14 @@ class GX40GimbalDriver : public amovGimbal::amovGimbalBase
int16_t targetPos[3]; int16_t targetPos[3];
amovGimbal::AMOV_GIMBAL_POS_T carrierPos; AMOV_GIMBAL_POS_T carrierPos;
amovGimbal::AMOV_GIMBAL_VELOCITY_T carrierSpeed; AMOV_GIMBAL_VELOCITY_T carrierSpeed;
amovGimbal::AMOV_GIMBAL_VELOCITY_T carrierAcc; AMOV_GIMBAL_VELOCITY_T carrierAcc;
GX40::GIMBAL_SECONDARY_MASTER_FRAME_T carrierGNSS; GX40::GIMBAL_SECONDARY_MASTER_FRAME_T carrierGNSS;
std::thread::native_handle_type nopSendThreadHandle; std::thread::native_handle_type nopSendThreadHandle;
void nopSend(void); void nopSend(void);
void parserStart(amovGimbal::pStateInvoke callback); void parserStart(pAmovGimbalStateInvoke callback);
public: public:
uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize); uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
@ -45,18 +45,18 @@ public:
uint32_t calPackLen(void *pack); uint32_t calPackLen(void *pack);
// funtions // funtions
uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalHome(void); uint32_t setGimabalHome(void);
uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalFocus(amovGimbal::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 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_POS_T &pos, uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &seppd,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData); void *extenData);
uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt); 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 * @Author: L LC @amov
* @Date: 2023-11-02 17:50:26 * @Date: 2023-11-02 17:50:26
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp
*/ */
#include <string.h> #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. * 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. * contains the roll, pitch, and yaw values of the gimbal position.
* *
* @return a packed value of type uint32_t. * @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(); carrierStateMutex.lock();
targetPos[0] = (int16_t)(-pos.roll / 0.01f); 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. * 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. * 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. * @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(); carrierStateMutex.lock();
targetPos[0] = (int16_t)(-speed.roll / 0.1f); 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. * 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: * 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. * @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; uint8_t temp = 0X01;
mState.lock(); 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 else
{ {
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; state.video = AMOV_GIMBAL_VIDEO_TAKE;
} }
mState.unlock(); 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 * The function `attitudeCorrection` updates the state of a gimbal driver with position, velocity, and
* acceleration data. * 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 * position of the gimbal. It likely contains information such as the pitch, yaw, and roll angles of
* the gimbal. * the gimbal.
* @param seppd The parameter `seppd` stands for "Separate Pointing Device" and represents the velocity * @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 * of the gimbal in separate axes (e.g., pitch, yaw, roll). It is of type
* `amovGimbal::AMOV_GIMBAL_VELOCITY_T`. * `AMOV_GIMBAL_VELOCITY_T`.
* @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. * acceleration of the gimbal.
* @param extenData The extenData parameter is a pointer to additional data that can be passed to the * @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 * 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, * not provided in the code snippet,
* *
* @return the size of the data being passed as arguments. The size is calculated by adding the sizes * @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), * of the three types: sizeof(AMOV_GIMBAL_POS_T),
* sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T), and sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T). * sizeof(AMOV_GIMBAL_VELOCITY_T), and sizeof(AMOV_GIMBAL_VELOCITY_T).
*/ */
uint32_t GX40GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, uint32_t GX40GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &seppd,
const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, const AMOV_GIMBAL_VELOCITY_T &acc,
void *extenData) void *extenData)
{ {
carrierStateMutex.lock(); carrierStateMutex.lock();
@ -136,7 +136,7 @@ uint32_t GX40GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_
carrierAcc = acc; carrierAcc = acc;
upDataTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()); upDataTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
carrierStateMutex.unlock(); 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 function `setGimbalZoom` in the `GX40GimbalDriver` class sets the zoom level of a gimbal based on
* the specified zoom type and target rate. * 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 * type. It represents the zoom action to be performed on the gimbal. The possible values for this
* parameter are: * parameter are:
* @param targetRate The targetRate parameter is a float value representing the desired zoom rate for * @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. * @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 temp[4];
uint8_t len = 0; uint8_t len = 0;
@ -177,13 +177,13 @@ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl
len = 1; len = 1;
switch (zoom) switch (zoom)
{ {
case amovGimbal::AMOV_GIMBAL_ZOOM_IN: case AMOV_GIMBAL_ZOOM_IN:
temp[0] = GX40::GIMBAL_CMD_ZOMM_IN; temp[0] = GX40::GIMBAL_CMD_ZOMM_IN;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_OUT: case AMOV_GIMBAL_ZOOM_OUT:
temp[0] = GX40::GIMBAL_CMD_ZOOM_OUT; temp[0] = GX40::GIMBAL_CMD_ZOOM_OUT;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_STOP: case AMOV_GIMBAL_ZOOM_STOP:
temp[0] = GX40::GIMBAL_CMD_ZOOM_STOP; temp[0] = GX40::GIMBAL_CMD_ZOOM_STOP;
break; 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 * The function "setGimbalFocus" sets the focus of a gimbal by specifying the zoom level and target
* rate. * 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 * type representing different zoom levels for the gimbal. It is used to specify the desired zoom level
* for the gimbal focus. * for the gimbal focus.
* @param targetRate The targetRate parameter is a float value representing the desired zoom rate for * @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. * @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; uint8_t temp = 0X01;

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

@ -3,7 +3,7 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2023-10-20 16:08:13 * @Date: 2023-10-20 16:08:13
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h
*/ */
#ifndef 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 * @Author: L LC @amov
* @Date: 2022-10-27 18:10:06 * @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h
*/ */
#ifndef Q10F_GIMBAL_CRC32_H #ifndef Q10F_GIMBAL_CRC32_H
#define Q10F_GIMBAL_CRC32_H #define Q10F_GIMBAL_CRC32_H
namespace Q10f namespace Q10f
{ {
static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght) 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 * @Author: L LC @amov
* @Date: 2022-10-27 18:10:06 * @Date: 2022-10-27 18:10:06
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp
*/ */
#include "Q10f_gimbal_driver.h" #include "Q10f_gimbal_driver.h"
@ -86,7 +86,7 @@ void Q10fGimbalDriver::convert(void *buf)
state.rel.pitch = tempPos->pitchStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; state.rel.pitch = tempPos->pitchStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
state.abs.roll, state.abs.pitch, state.abs.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(); mState.unlock();
break; break;

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

@ -3,19 +3,18 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2022-10-28 12:24:21 * @Date: 2022-10-28 12:24:21
* @LastEditors: L LC @amov * @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 * @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 "../amov_gimbal_private.h"
#include "Q10f_gimbal_struct.h" #include "Q10f_gimbal_struct.h"
#include <mutex> #include <mutex>
#include <malloc.h> #include <malloc.h>
#include <iostream> #include <iostream>
#ifndef __Q10F_DRIVER_H
#define __Q10F_DRIVER_H
class Q10fGimbalDriver : protected amovGimbal::amovGimbalBase class Q10fGimbalDriver : protected amovGimbal::amovGimbalBase
{ {
private: private:
@ -29,16 +28,16 @@ private:
public: public:
// funtions // funtions
uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed); uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
uint32_t setGimabalHome(void); uint32_t setGimabalHome(void);
uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
uint32_t setGimbalFocus(amovGimbal::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 takePic(void);
uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
// builds // builds
static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) 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 * @Author: L LC @amov
* @Date: 2023-03-02 10:00:52 * @Date: 2023-03-02 10:00:52
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp
*/ */
#include "Q10f_gimbal_driver.h" #include "Q10f_gimbal_driver.h"
@ -17,7 +17,7 @@
* *
* @return The return value is the number of bytes written to the buffer. * @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; Q10f::GIMBAL_SET_POS_MSG_T temp;
temp.modeR = Q10f::GIMBAL_CMD_POS_MODE_ANGLE_SPEED; 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. * @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; Q10f::GIMBAL_SET_POS_MSG_T temp;
temp.modeR = Q10f::GIMBAL_CMD_POS_MODE_SPEED; 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. * @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.pitch = followSpeed.pitch / 0.1220740379f;
state.maxFollow.roll = followSpeed.roll / 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. * @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}; uint8_t cmd[2] = {0X01, 0XFF};
if (newState == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) if (newState == AMOV_GIMBAL_VIDEO_TAKE)
{ {
cmd[0] = 0X02; cmd[0] = 0X02;
state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; state.video = AMOV_GIMBAL_VIDEO_TAKE;
} }
else else
{ {
cmd[0] = 0X03; 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)); 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}; uint8_t cmd[5] = {0X00, 0X00, 0X00, 0X00, 0XFF};
if (targetRate == 0.0f) if (targetRate == 0.0f)
@ -134,13 +134,13 @@ uint32_t Q10fGimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl
cmd[1] = 0XFF; cmd[1] = 0XFF;
switch (zoom) switch (zoom)
{ {
case amovGimbal::AMOV_GIMBAL_ZOOM_IN: case AMOV_GIMBAL_ZOOM_IN:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN; cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_OUT: case AMOV_GIMBAL_ZOOM_OUT:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT; cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_STOP: case AMOV_GIMBAL_ZOOM_STOP:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP; cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP;
break; break;
default: 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}; uint8_t cmd[2] = {0X00, 0XFF};
switch (zoom) switch (zoom)
{ {
case amovGimbal::AMOV_GIMBAL_ZOOM_IN: case AMOV_GIMBAL_ZOOM_IN:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN; cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_OUT: case AMOV_GIMBAL_ZOOM_OUT:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT; cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT;
break; break;
case amovGimbal::AMOV_GIMBAL_ZOOM_STOP: case AMOV_GIMBAL_ZOOM_STOP:
cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP; cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP;
break; break;
default: default:

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

@ -3,7 +3,7 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2022-10-27 18:10:07 * @Date: 2022-10-27 18:10:07
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h
*/ */
#ifndef 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 * @Author : Aiyangsky
* @Date : 2023-05-13 10:39:20 * @Date : 2023-05-13 10:39:20
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_private.h
*/ */
#ifndef __AMOV_GIMABL_PRIVATE_H #ifndef __AMOV_GIMABL_PRIVATE_H
@ -12,12 +12,12 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <iostream> #include <iostream>
#include <thread> #include <thread>
#include <unistd.h> #include <unistd.h>
#include <mutex> #include <mutex>
#include "amov_gimbal.h" #include "amovGimbal/amov_gimbal.h"
#include "amovGimbal/amov_gimbal_c.h"
#include "Ring_Fifo.h" #include "Ring_Fifo.h"
#include "amov_tool.h" #include "amov_tool.h"
@ -28,9 +28,19 @@ namespace amovGimbal
public: public:
AMOV_GIMBAL_STATE_T state; AMOV_GIMBAL_STATE_T state;
std::mutex mState; std::mutex mState;
IOStreamBase *IO;
pStateInvoke updateGimbalStateCallback; // IO类
pMsgInvoke msgCustomCallback = idleMsgCallback; 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 *rxQueue;
fifoRing *txQueue; fifoRing *txQueue;
@ -39,7 +49,7 @@ namespace amovGimbal
std::thread::native_handle_type sendThreadHanle = 0; std::thread::native_handle_type sendThreadHanle = 0;
std::thread::native_handle_type stackThreadHanle = 0; std::thread::native_handle_type stackThreadHanle = 0;
PamovGimbalBase(SET IOStreamBase *_IO) PamovGimbalBase(IOStreamBase *_IO)
{ {
IO = _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 class amovGimbalBase : public IamovGimbalBase, public PamovGimbalBase
{ {
public: public:
@ -77,7 +118,7 @@ namespace amovGimbal
virtual void mainLoop(void); virtual void mainLoop(void);
virtual void stackStart(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); 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 * @Author: L LC @amov
* @Date: 2023-07-31 18:30:33 * @Date: 2023-07-31 18:30:33
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_tool.h
*/ */
#ifndef __AMOVGIMABL_TOOL_H
#define __AMOVGIMABL_TOOL_H
namespace amovGimbalTools namespace amovGimbalTools
{ {
static inline unsigned short conversionBigLittle(unsigned short value) static inline unsigned short conversionBigLittle(unsigned short value)
@ -27,3 +29,4 @@ namespace amovGimbalTools
return temp; return temp;
} }
} }
#endif

148
gimbal_ctrl/sv_gimbal.cpp
View File

@ -3,11 +3,10 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2023-04-12 09:12:52 * @Date: 2023-04-12 09:12:52
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/sv_gimbal.cpp
*/ */
#include "amov_gimbal.h" #include "amovGimbal/amov_gimbal.h"
#include "amov_gimbal_struct.h"
#include "sv_gimbal.h" #include "sv_gimbal.h"
#include "sv_gimbal_io.hpp" #include "sv_gimbal_io.hpp"
@ -20,19 +19,42 @@ namespace sv
std::map<std::string, void *> Gimbal::IOList; std::map<std::string, void *> Gimbal::IOList;
std::mutex Gimbal::IOListMutex; std::mutex Gimbal::IOListMutex;
std::map<GimbalType, std::string> gimbaltypeList = typedef struct
{ {
{GimbalType::G1, "G1"}, std::string name;
{GimbalType::Q10f, "Q10f"}, GimbalLink supportLink;
{GimbalType::AT10, "AT10"}, } gimbalTrait;
{GimbalType::GX40, "GX40"}};
std::string &cvGimbalType2Str(const GimbalType &type) std::map<GimbalType, gimbalTrait> gimbaltypeList =
{ {
std::map<GimbalType, std::string>::iterator temp = gimbaltypeList.find(type); {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()) 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!!!!"; throw "Error: Unsupported gimbal device type!!!!";
exit(-1); 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 * @param port The parameter "port" is an integer that represents the TCP network port number.
* 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.
*/ */
void sv::Gimbal::setNetPort(const int &port) void sv::Gimbal::setTcpNetPort(const int &port)
{ {
this->m_net_port = 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. * 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) void sv::Gimbal::setStateCallback(sv::PStateInvoke callback)
{ {
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; 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 * The function `sv::Gimbal::creatIO` creates an IO object based on the specified gimbal type and link
* if it doesn't already exist. * type, and returns a pointer to the created object.
* *
* @param dev A pointer to an instance of the `Gimbal` class. * @param dev The "dev" parameter is a pointer to an object of type "sv::Gimbal". It is used to access
* @param IO The parameter "IO" is a pointer to an object that represents the input/output (IO) device * the member variables of the Gimbal object, such as "m_serial_port", "m_serial_baud_rate",
* for the gimbal. It is used to establish a connection with the gimbal and perform communication * "m_serial_timeout", etc. These variables store information about
* operations.
* *
* @return a boolean value. * @return a void pointer.
*/ */
void *sv::Gimbal::creatIO(sv::Gimbal *dev) void *sv::Gimbal::creatIO(sv::Gimbal *dev)
{ {
IOListMutex.lock(); IOListMutex.lock();
std::map<std::string, void *>::iterator list = IOList.find(dev->m_serial_port); std::map<std::string, void *>::iterator list = IOList.find(dev->m_serial_port);
std::pair<std::string, void *> key("NULL", nullptr); 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()) 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) 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) 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 else
{ {
std::cout << "Error: gimbal IO has opened!!!" << std::endl; std::cout << "Error: gimbal IO has opened!!!" << std::endl;
} }
}
IOListMutex.unlock(); IOListMutex.unlock();
return key.second; 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) void sv::Gimbal::removeIO(sv::Gimbal *dev)
{ {
IOListMutex.lock(); IOListMutex.lock();
@ -181,6 +236,7 @@ void sv::Gimbal::removeIO(sv::Gimbal *dev)
IOListMutex.unlock(); IOListMutex.unlock();
} }
/** /**
* The function opens a communication interface with a gimbal device and sets up a parser to handle * The function opens a communication interface with a gimbal device and sets up a parser to handle
* incoming data. * incoming data.
@ -192,7 +248,6 @@ void sv::Gimbal::removeIO(sv::Gimbal *dev)
* *
* @return A boolean value is being returned. * @return A boolean value is being returned.
*/ */
#include "stdlib.h"
bool sv::Gimbal::open(PStateInvoke callback) bool sv::Gimbal::open(PStateInvoke callback)
{ {
bool ret = false; bool ret = false;
@ -202,11 +257,12 @@ bool sv::Gimbal::open(PStateInvoke callback)
if (this->IO != nullptr) if (this->IO != nullptr)
{ {
std::string driverName; 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); this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO);
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
pdevTemp->startStack(); pdevTemp->startStack();
pdevTemp->parserAuto(callback); m_callback = callback;
pdevTemp->parserAuto(sv::Gimbal::gimbalUpdataCallback, this);
ret = true; ret = true;
} }
@ -224,7 +280,7 @@ bool sv::Gimbal::open(PStateInvoke callback)
bool sv::Gimbal::setHome() bool sv::Gimbal::setHome()
{ {
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
if (pdevTemp->dev->setGimabalHome() > 0) if (pdevTemp->setGimabalHome() > 0)
{ {
return true; return true;
} }
@ -253,7 +309,7 @@ bool sv::Gimbal::setZoom(double x)
return false; return false;
} }
if (pdevTemp->dev->setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_STOP, x) > 0) if (pdevTemp->setGimbalZoom(AMOV_GIMBAL_ZOOM_STOP, x) > 0)
{ {
return true; return true;
} }
@ -277,7 +333,7 @@ bool sv::Gimbal::setAutoZoom(int state)
{ {
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; 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; return true;
} }
@ -303,7 +359,7 @@ bool sv::Gimbal::setAutoFocus(int state)
{ {
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; 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; return true;
} }
@ -323,7 +379,7 @@ bool sv::Gimbal::takePhoto()
{ {
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
if (pdevTemp->dev->takePic() > 0) if (pdevTemp->takePic() > 0)
{ {
return true; return true;
} }
@ -347,21 +403,21 @@ bool sv::Gimbal::takeVideo(int state)
{ {
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
amovGimbal::AMOV_GIMBAL_VIDEO_T newState; AMOV_GIMBAL_VIDEO_T newState;
switch (state) switch (state)
{ {
case 0: case 0:
newState = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; newState = AMOV_GIMBAL_VIDEO_OFF;
break; break;
case 1: case 1:
newState = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; newState = AMOV_GIMBAL_VIDEO_TAKE;
break; break;
default: default:
newState = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; newState = AMOV_GIMBAL_VIDEO_OFF;
break; break;
} }
if (pdevTemp->dev->setVideo(newState) > 0) if (pdevTemp->setVideo(newState) > 0)
{ {
return true; return true;
} }
@ -382,13 +438,13 @@ int sv::Gimbal::getVideoState()
{ {
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
int ret; int ret;
amovGimbal::AMOV_GIMBAL_STATE_T state; AMOV_GIMBAL_STATE_T state;
state = pdevTemp->getGimabalState(); state = pdevTemp->getGimabalState();
if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) if (state.video == AMOV_GIMBAL_VIDEO_TAKE)
{ {
ret = 1; ret = 1;
} }
else if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_OFF) else if (state.video == AMOV_GIMBAL_VIDEO_OFF)
{ {
ret = 0; ret = 0;
} }
@ -416,7 +472,7 @@ void sv::Gimbal::setAngleEuler(double roll, double pitch, double yaw,
{ {
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
amovGimbal::AMOV_GIMBAL_POS_T temp; AMOV_GIMBAL_POS_T temp;
if (pitch_rate == 0.0f) if (pitch_rate == 0.0f)
pitch_rate = 360; pitch_rate = 360;
@ -428,11 +484,11 @@ void sv::Gimbal::setAngleEuler(double roll, double pitch, double yaw,
temp.pitch = pitch_rate; temp.pitch = pitch_rate;
temp.roll = roll_rate; temp.roll = roll_rate;
temp.yaw = yaw_rate; temp.yaw = yaw_rate;
pdevTemp->dev->setGimabalFollowSpeed(temp); pdevTemp->setGimabalFollowSpeed(temp);
temp.pitch = pitch; temp.pitch = pitch;
temp.roll = roll; temp.roll = roll;
temp.yaw = yaw; 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::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
amovGimbal::AMOV_GIMBAL_POS_T temp; AMOV_GIMBAL_POS_T temp;
temp.pitch = pitch_rate; temp.pitch = pitch_rate;
temp.roll = roll_rate; temp.roll = roll_rate;
temp.yaw = yaw_rate; temp.yaw = yaw_rate;
pdevTemp->dev->setGimabalSpeed(temp); pdevTemp->setGimabalSpeed(temp);
} }
sv::Gimbal::~Gimbal() sv::Gimbal::~Gimbal()

189
gimbal_ctrl/sv_gimbal_io.hpp
View File

@ -3,15 +3,29 @@
* @Author: L LC @amov * @Author: L LC @amov
* @Date: 2023-04-12 12:22:09 * @Date: 2023-04-12 12:22:09
* @LastEditors: L LC @amov * @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 * @FilePath: /SpireCV/gimbal_ctrl/sv_gimbal_io.hpp
*/ */
#ifndef __SV_GIMABL_IO_H #ifndef __SV_GIMABL_IO_H
#define __SV_GIMABL_IO_H #define __SV_GIMABL_IO_H
#include "amov_gimbal.h" #include "amovGimbal/amov_gimbal.h"
#include "serial/serial.h" #include "serial/serial.h"
#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
{
class UART : public amovGimbal::IOStreamBase class UART : public amovGimbal::IOStreamBase
{ {
private: private:
@ -36,21 +50,18 @@ public:
{ {
return false; return false;
} }
virtual uint32_t inPutBytes(IN uint8_t *byte)
virtual bool inPutByte(IN uint8_t *byte)
{ {
if (ser->read(byte, 1)) if (ser->read(byte, 1))
{ {
return true; return 1;
} }
return false; return 0;
} }
virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
{ {
return ser->write(byte, lenght); return ser->write(byte, lenght);
} }
UART(const std::string &port, uint32_t baudrate, serial::Timeout timeout, UART(const std::string &port, uint32_t baudrate, serial::Timeout timeout,
serial::bytesize_t bytesize, serial::parity_t parity, serial::stopbits_t stopbits, serial::bytesize_t bytesize, serial::parity_t parity, serial::stopbits_t stopbits,
serial::flowcontrol_t flowcontrol) serial::flowcontrol_t flowcontrol)
@ -60,8 +71,168 @@ public:
~UART() ~UART()
{ {
ser->close(); ser->close();
delete ser;
} }
}; };
int scoketClose(int scoket)
{
#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;
}
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);
}
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());
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
{
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 #endif

43
include/sv_gimbal.h
View File

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