From 62051a730b469ca59717855fcabbdb9795e961d3 Mon Sep 17 00:00:00 2001 From: AiYangSky <1732570904@qq.com> Date: Tue, 5 Dec 2023 17:50:24 +0800 Subject: [PATCH] support net --- .../driver/src/AT10/AT10_gimbal_crc32.h | 2 +- .../driver/src/AT10/AT10_gimbal_driver.cpp | 66 ++-- .../driver/src/AT10/AT10_gimbal_driver.h | 35 +- .../driver/src/AT10/AT10_gimbal_funtion.cpp | 32 +- .../driver/src/AT10/AT10_gimbal_struct.h | 4 +- gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h | 2 +- .../driver/src/G1/g1_gimbal_driver.cpp | 4 +- gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h | 29 +- .../driver/src/G1/g1_gimbal_funtion.cpp | 40 +- gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h | 2 +- .../driver/src/GX40/GX40_gimbal_crc16.h | 2 +- .../driver/src/GX40/GX40_gimbal_driver.cpp | 11 +- .../driver/src/GX40/GX40_gimbal_driver.h | 36 +- .../driver/src/GX40/GX40_gimbal_funtion.cpp | 52 +-- .../driver/src/GX40/GX40_gimbal_struct.h | 2 +- .../driver/src/Q10f/Q10f_gimbal_crc32.h | 3 +- .../driver/src/Q10f/Q10f_gimbal_driver.cpp | 4 +- .../driver/src/Q10f/Q10f_gimbal_driver.h | 21 +- .../driver/src/Q10f/Q10f_gimbal_funtion.cpp | 32 +- .../driver/src/Q10f/Q10f_gimbal_struct.h | 2 +- .../driver/src/amovGimbal/amov_gimbal.h | 111 ++++++ .../driver/src/amovGimbal/amov_gimbal_c.h | 55 +++ .../src/amovGimbal/amov_gimbal_struct.h | 102 +++++ gimbal_ctrl/driver/src/amov_gimbal.cpp | 357 ------------------ gimbal_ctrl/driver/src/amov_gimbal.h | 109 ------ .../driver/src/amov_gimbal_factory.cpp | 90 +++++ .../driver/src/amov_gimbal_interface.cpp | 229 +++++++++++ .../driver/src/amov_gimbal_interface_c.cpp | 152 ++++++++ gimbal_ctrl/driver/src/amov_gimbal_private.h | 57 ++- .../driver/src/amov_gimbal_realize.cpp | 197 ++++++++++ gimbal_ctrl/driver/src/amov_gimbal_struct.h | 104 ----- gimbal_ctrl/driver/src/amov_tool.h | 5 +- gimbal_ctrl/sv_gimbal.cpp | 158 +++++--- gimbal_ctrl/sv_gimbal_io.hpp | 257 ++++++++++--- include/sv_gimbal.h | 43 ++- 35 files changed, 1525 insertions(+), 882 deletions(-) create mode 100755 gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h create mode 100644 gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_c.h create mode 100755 gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_struct.h delete mode 100755 gimbal_ctrl/driver/src/amov_gimbal.cpp delete mode 100755 gimbal_ctrl/driver/src/amov_gimbal.h create mode 100644 gimbal_ctrl/driver/src/amov_gimbal_factory.cpp create mode 100644 gimbal_ctrl/driver/src/amov_gimbal_interface.cpp create mode 100644 gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp create mode 100644 gimbal_ctrl/driver/src/amov_gimbal_realize.cpp delete mode 100755 gimbal_ctrl/driver/src/amov_gimbal_struct.h diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h index b8845c4..ce705e1 100755 --- a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:06 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:47:48 + * @LastEditTime: 2023-12-05 16:30:27 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h */ #ifndef AT10_GIMBAL_CRC32_H diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp index 655ca35..1b6c13e 100755 --- a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:06 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:47:53 + * @LastEditTime: 2023-12-05 17:23:48 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp */ #include "AT10_gimbal_driver.h" @@ -24,13 +24,6 @@ AT10GimbalDriver::AT10GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal:: stdRxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE); stdTxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE); parserState = AT10::GIMBAL_SERIAL_STATE_IDLE; - - // Initialize and enable attitude data return (50Hz) - // uint8_t cmd = 0XFF; - // pack(AT10::GIMBAL_CMD_SET_FEEDBACK_H, &cmd, 1); - // pack(AT10::GIMBAL_CMD_SET_FEEDBACK_L, &cmd, 1); - // cmd = 0X00; - // pack(AT10::GIMBAL_CMD_OPEN_FEEDBACK, &cmd, 1); } /** @@ -84,21 +77,7 @@ void AT10GimbalDriver::convert(void *buf) switch (temp->head) { case AT10::GIMBAL_CMD_RCV_STATE: - std::cout << "Undefined old frame from AT10"; - // AT10::GIMBAL_RCV_POS_MSG_T *tempPos; - // tempPos = reinterpret_cast(((uint8_t *)buf) + AT10_EXT_PAYLOAD_OFFSET); - // mState.lock(); - // state.abs.yaw = tempPos->yawIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE; - // state.abs.roll = tempPos->rollIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE; - // state.abs.pitch = tempPos->pitchIMUAngle * AT10_EXT_SCALE_FACTOR_ANGLE; - // state.rel.yaw = tempPos->yawStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED; - // state.rel.roll = tempPos->rollStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED; - // state.rel.pitch = tempPos->pitchStatorRotorAngle * AT10_EXT_SCALE_FACTOR_SPEED; - // updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, - // state.abs.roll, state.abs.pitch, state.abs.yaw, - // state.fov.x, state.fov.y); - // mState.unlock(); - + std::cout << "Undefined old frame from AT10\r\n"; break; case AT10::GIMBAL_CMD_STD: @@ -123,15 +102,15 @@ void AT10GimbalDriver::convert(void *buf) state.fov.y = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovY) * 0.1; if ((amovGimbalTools::conversionBigLittle(tempRcv->D1.camera) & 0X0003) == 0X01) { - state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; + state.video = AMOV_GIMBAL_VIDEO_TAKE; } else { - state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; + state.video = AMOV_GIMBAL_VIDEO_OFF; } updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, state.abs.roll, state.abs.pitch, state.abs.yaw, - state.fov.x, state.fov.y); + state.fov.x, state.fov.y, updataCaller); mState.unlock(); break; @@ -316,21 +295,9 @@ bool AT10GimbalDriver::parser(IN uint8_t byte) void AT10GimbalDriver::sendHeart(void) { - // uint8_t tempBuffer[72]; uint8_t temp = 0X00; while (1) { - // if (!IO->isBusy() && IO->isOpen()) - // { - // bool state = false; - - // state = txQueue->outCell(&tempBuffer); - // if (state) - // { - // IO->outPutBytes((uint8_t *)&tempBuffer, - // reinterpret_cast(tempBuffer)->len + AT10_EXT_PAYLOAD_OFFSET + sizeof(uint8_t)); - // } - // } std::this_thread::sleep_for(std::chrono::milliseconds(100)); pack(AT10::GIMBAL_CMD_STD_HEART, &temp, sizeof(temp)); } @@ -363,19 +330,26 @@ void AT10GimbalDriver::stackStart(void) } std::thread sendHeartLoop(&AT10GimbalDriver::sendHeart, this); std::thread sendStdLoop(&AT10GimbalDriver::sendStd, this); + + this->sendThreadHanle = sendStdLoop.native_handle(); + this->sendHreatThreadHandle = sendHeartLoop.native_handle(); + sendHeartLoop.detach(); sendStdLoop.detach(); } void AT10GimbalDriver::parserLoop(void) { - uint8_t temp; + uint8_t temp[65536]; + uint32_t i = 0, getCount = 0; while (1) { - if (IO->inPutByte(&temp)) + getCount = IO->inPutBytes(temp); + + for (i = 0; i < getCount; i++) { - parser(temp); + parser(temp[i]); } } } @@ -387,7 +361,7 @@ void AT10GimbalDriver::getStdRxPack(void) { if (stdRxQueue->outCell(tempBuffer)) { - msgCustomCallback(tempBuffer); + msgCustomCallback(tempBuffer, msgCaller); convert(tempBuffer); } } @@ -400,13 +374,13 @@ void AT10GimbalDriver::getExtRxPack(void) { if (rxQueue->outCell(tempBuffer)) { - msgCustomCallback(tempBuffer); + msgCustomCallback(tempBuffer, msgCaller); convert(tempBuffer); } } } -void AT10GimbalDriver::parserStart(amovGimbal::pStateInvoke callback) +void AT10GimbalDriver::parserStart(pAmovGimbalStateInvoke callback) { this->updateGimbalStateCallback = callback; @@ -414,6 +388,10 @@ void AT10GimbalDriver::parserStart(amovGimbal::pStateInvoke callback) std::thread getStdRxPackLoop(&AT10GimbalDriver::getStdRxPack, this); std::thread getExtRxPackLooP(&AT10GimbalDriver::getExtRxPack, this); + this->parserThreadHanle = parser.native_handle(); + this->stackThreadHanle = getStdRxPackLoop.native_handle(); + this->extStackThreadHandle = getExtRxPackLooP.native_handle(); + parser.detach(); getStdRxPackLoop.detach(); getExtRxPackLooP.detach(); diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h index 4f2eaf6..f2560b7 100755 --- a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h @@ -3,19 +3,18 @@ * @Author: L LC @amov * @Date: 2022-10-28 12:24:21 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:47:58 + * @LastEditTime: 2023-12-05 16:30:42 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h */ -#include "../amov_gimbal.h" +#ifndef __AT10_DRIVER_H +#define __AT10_DRIVER_H + #include "../amov_gimbal_private.h" #include "AT10_gimbal_struct.h" #include #include #include -#ifndef __AT10_DRIVER_H -#define __AT10_DRIVER_H - class AT10GimbalDriver : protected amovGimbal::amovGimbalBase { private: @@ -32,12 +31,13 @@ private: void sendHeart(void); void sendStd(void); - void parserStart(amovGimbal::pStateInvoke callback); + void parserStart(pAmovGimbalStateInvoke callback); void parserLoop(void); void getExtRxPack(void); void getStdRxPack(void); - // bool getRxPack(OUT void *pack); + std::thread::native_handle_type sendHreatThreadHandle; + std::thread::native_handle_type extStackThreadHandle; bool parser(IN uint8_t byte); void convert(void *buf); @@ -46,18 +46,18 @@ private: public: // funtions - uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); - uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); - uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed); + uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos); + uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed); + uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed); uint32_t setGimabalHome(void); - uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); - uint32_t setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); uint32_t takePic(void); - uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); + uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState); - uint32_t extensionFuntions(void* cmd); + uint32_t extensionFuntions(void *cmd); // builds static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) @@ -76,6 +76,13 @@ public: { delete stdTxQueue; } + // set thread kill anytime + pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); + parserThreadHanle = parserThreadHanle == 0 ? 0 : pthread_cancel(parserThreadHanle); + sendThreadHanle = sendThreadHanle == 0 ? 0 : pthread_cancel(sendThreadHanle); + stackThreadHanle = stackThreadHanle == 0 ? 0 : pthread_cancel(stackThreadHanle); + sendHreatThreadHandle = sendHreatThreadHandle == 0 ? 0 : pthread_cancel(sendHreatThreadHandle); + extStackThreadHandle = extStackThreadHandle == 0 ? 0 : pthread_cancel(extStackThreadHandle); } }; diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp index a3993f8..acf6812 100755 --- a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp @@ -3,8 +3,8 @@ * @Author: L LC @amov * @Date: 2023-03-02 10:00:52 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:48:03 - * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp + * @LastEditTime: 2023-11-27 16:27:18 + * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_funtion.cpp */ #include "AT10_gimbal_driver.h" #include "AT10_gimbal_crc32.h" @@ -17,7 +17,7 @@ * * @return The return value is the number of bytes written to the buffer. */ -uint32_t AT10GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos) +uint32_t AT10GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos) { int16_t yaw, pitch; AT10::GIMBAL_CMD_A1_MSG_T temp; @@ -35,14 +35,14 @@ uint32_t AT10GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &po } /** - * It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type + * It takes a struct of type AMOV_GIMBAL_POS_T and converts it to a struct of type * G1::GIMBAL_SET_POS_MSG_T * * @param speed the speed of the gimbal * * @return The return value is the number of bytes written to the buffer. */ -uint32_t AT10GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed) +uint32_t AT10GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed) { int16_t speedYaw, speedPitch; AT10::GIMBAL_CMD_A1_MSG_T temp; @@ -66,7 +66,7 @@ uint32_t AT10GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T & * * @return The return value is the number of bytes written to the buffer. */ -uint32_t AT10GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed) +uint32_t AT10GimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed) { state.maxFollow.pitch = fabs(followSpeed.pitch * 100); state.maxFollow.yaw = fabs(followSpeed.yaw * 100); @@ -110,10 +110,10 @@ uint32_t AT10GimbalDriver::takePic(void) * * @return The return value is the number of bytes written to the serial port. */ -uint32_t AT10GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState) +uint32_t AT10GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState) { uint16_t temp; - if (newState == amovGimbal::AMOV_GIMBAL_VIDEO_T::AMOV_GIMBAL_VIDEO_TAKE) + if (newState == AMOV_GIMBAL_VIDEO_T::AMOV_GIMBAL_VIDEO_TAKE) { temp = 0x14 << 3; } @@ -127,20 +127,20 @@ uint32_t AT10GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newSta return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t)); } -uint32_t AT10GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) +uint32_t AT10GimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate) { if (targetRate == 0.0f) { uint16_t temp = 0; switch (zoom) { - case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN: + case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN: temp = 0X08 << 3; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT: + case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT: temp = 0X09 << 3; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP: + case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP: temp = 0X01 << 3; break; default: @@ -160,18 +160,18 @@ uint32_t AT10GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl } } -uint32_t AT10GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) +uint32_t AT10GimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate) { uint16_t temp = 0; switch (zoom) { - case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN: + case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN: temp = 0X0B << 3; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT: + case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT: temp = 0X0A << 3; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP: + case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP: temp = 0X01 << 3; break; default: diff --git a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h index ee93582..54f0dee 100755 --- a/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h +++ b/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h @@ -3,8 +3,8 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:07 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:48:08 - * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h + * @LastEditTime: 2023-08-25 19:32:59 + * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_struct.h */ #ifndef AT10_GIMBAL_STRUCT_H #define AT10_GIMBAL_STRUCT_H diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h b/gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h index 55dd04a..3f3cabb 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:06 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:47:02 + * @LastEditTime: 2023-12-05 16:30:13 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h */ #ifndef G1_GIMBAL_CRC32_H diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp index 75ce4d4..ef8a059 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:06 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:47:07 + * @LastEditTime: 2023-12-05 17:22:57 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp */ #include "g1_gimbal_driver.h" @@ -75,7 +75,7 @@ void g1GimbalDriver::convert(void *buf) state.rel.pitch = tempPos->HALL_pitch * G1_SCALE_FACTOR; updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, state.abs.roll, state.abs.pitch, state.abs.yaw, - state.fov.x, state.fov.y); + state.fov.x, state.fov.y, updataCaller); mState.unlock(); break; diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h index 5346b59..68cf329 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h @@ -3,19 +3,18 @@ * @Author: L LC @amov * @Date: 2022-10-28 12:24:21 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:47:12 + * @LastEditTime: 2023-12-05 16:29:58 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h */ -#include "../amov_gimbal.h" +#ifndef __G1_DRIVER_H +#define __G1_DRIVER_H + #include "../amov_gimbal_private.h" #include "g1_gimbal_struct.h" #include #include #include -#ifndef __G1_DRIVER_H -#define __G1_DRIVER_H - class g1GimbalDriver : protected amovGimbal::amovGimbalBase { private: @@ -29,22 +28,22 @@ private: public: // funtions - uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); - uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); - uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed); + uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos); + uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed); + uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed); uint32_t setGimabalHome(void); uint32_t takePic(void); - uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); + uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState); - uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATERNION_T &quaterion, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &speed, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, + uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, + const AMOV_GIMBAL_VELOCITY_T &speed, + const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData); - uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, + uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, + const AMOV_GIMBAL_VELOCITY_T &seppd, + const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData); uint32_t extensionFuntions(void *cmd); diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp index 05d5d8c..0d97690 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2023-03-02 10:00:52 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:47:19 + * @LastEditTime: 2023-12-05 16:29:51 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp */ #include "g1_gimbal_driver.h" @@ -18,7 +18,7 @@ * * @return The return value is the number of bytes written to the buffer. */ -uint32_t g1GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos) +uint32_t g1GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos) { G1::GIMBAL_SET_POS_MSG_T temp; temp.mode = G1::GIMBAL_CMD_POS_MODE_ANGLE; @@ -32,14 +32,14 @@ uint32_t g1GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos) } /** - * It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type + * It takes a struct of type AMOV_GIMBAL_POS_T and converts it to a struct of type * G1::GIMBAL_SET_POS_MSG_T * * @param speed the speed of the gimbal * * @return The return value is the number of bytes written to the buffer. */ -uint32_t g1GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed) +uint32_t g1GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed) { G1::GIMBAL_SET_POS_MSG_T temp; temp.mode = G1::GIMBAL_CMD_POS_MODE_SPEED; @@ -59,7 +59,7 @@ uint32_t g1GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &sp * * @return The return value is the number of bytes written to the buffer. */ -uint32_t g1GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed) +uint32_t g1GimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed) { state.maxFollow.pitch = followSpeed.pitch / G1_SCALE_FACTOR; state.maxFollow.roll = followSpeed.roll / G1_SCALE_FACTOR; @@ -100,18 +100,18 @@ uint32_t g1GimbalDriver::takePic(void) * * @return The return value is the number of bytes written to the serial port. */ -uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState) +uint32_t g1GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState) { uint8_t temp = G1::GIMBAL_CMD_CAMERA_REC; mState.lock(); - if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) + if (state.video == AMOV_GIMBAL_VIDEO_TAKE) { - state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; + state.video = AMOV_GIMBAL_VIDEO_OFF; } else { - state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; + state.video = AMOV_GIMBAL_VIDEO_TAKE; } mState.unlock(); @@ -124,10 +124,10 @@ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState * * @param quaterion The "quaterion" parameter is a structure of type "AMOV_GIMBAL_QUATERNION_T" which * contains the following fields: - * @param speed The "speed" parameter is of type `amovGimbal::AMOV_GIMBAL_VELOCITY_T` and represents + * @param speed The "speed" parameter is of type `AMOV_GIMBAL_VELOCITY_T` and represents * the velocity of the gimbal. It contains three components: `x`, `y`, and `z`, which represent the * velocity in the respective axes. - * @param acc The "acc" parameter is of type "amovGimbal::AMOV_GIMBAL_VELOCITY_T" and represents the + * @param acc The "acc" parameter is of type "AMOV_GIMBAL_VELOCITY_T" and represents the * acceleration of the gimbal in three dimensions (x, y, z). * @param extenData The extenData parameter is a void pointer that can be used to pass additional data * to the attitudeCorrection function. In this case, it is being cast to a float pointer and then @@ -136,9 +136,9 @@ uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState * * @return a uint32_t value. */ -uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATERNION_T &quaterion, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &speed, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, +uint32_t g1GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, + const AMOV_GIMBAL_VELOCITY_T &speed, + const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData) { G1::GIMBAL_ATT_CORR_MSG_T temp; @@ -161,12 +161,12 @@ uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATER * The function `attitudeCorrection` calculates the attitude correction for a gimbal based on the given * position, velocity, and acceleration values. * - * @param pos The "pos" parameter is of type amovGimbal::AMOV_GIMBAL_POS_T and represents the current + * @param pos The "pos" parameter is of type AMOV_GIMBAL_POS_T and represents the current * position of the gimbal. It contains the pitch, roll, and yaw angles of the gimbal. * @param seppd seppd stands for "Separate Pointing Device" and it represents the velocity of the - * gimbal in terms of pitch, roll, and yaw. It is of type `amovGimbal::AMOV_GIMBAL_VELOCITY_T` which + * gimbal in terms of pitch, roll, and yaw. It is of type `AMOV_GIMBAL_VELOCITY_T` which * likely contains three float values for pitch, - * @param acc The "acc" parameter is of type "amovGimbal::AMOV_GIMBAL_VELOCITY_T" and represents the + * @param acc The "acc" parameter is of type "AMOV_GIMBAL_VELOCITY_T" and represents the * acceleration of the gimbal in three dimensions (x, y, z). * @param extenData The `extenData` parameter is a void pointer that can be used to pass additional * data to the `attitudeCorrection` function. In this code snippet, it is assumed that `extenData` is a @@ -174,9 +174,9 @@ uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_QUATER * * @return a uint32_t value. */ -uint32_t g1GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, +uint32_t g1GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, + const AMOV_GIMBAL_VELOCITY_T &seppd, + const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData) { G1::GIMBAL_ATT_CORR_MSG_T temp; diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h b/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h index 43b0a02..2ef19e8 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:07 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:47:24 + * @LastEditTime: 2023-12-05 16:29:48 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h */ #ifndef G1_GIMBAL_STRUCT_H diff --git a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h index c26b45c..62724e8 100644 --- a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h +++ b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2023-10-20 16:33:07 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:46:51 + * @LastEditTime: 2023-12-05 16:29:39 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h */ #ifndef GX40_GIMBAL_CRC16_H diff --git a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp index 019135a..74ead00 100644 --- a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2023-10-20 16:08:17 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:46:45 + * @LastEditTime: 2023-12-05 17:37:59 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp */ #include @@ -51,7 +51,7 @@ void GX40GimbalDriver::nopSend(void) * pointer type. It is used to specify a callback function that will be invoked when the gimbal state * is updated. */ -void GX40GimbalDriver::parserStart(amovGimbal::pStateInvoke callback) +void GX40GimbalDriver::parserStart(pAmovGimbalStateInvoke callback) { this->updateGimbalStateCallback = callback; @@ -160,8 +160,8 @@ void GX40GimbalDriver::convert(void *buf) secondary = (GX40::GIMBAL_SECONDARY_SLAVE_FRAME_T *)temp->secondaryData; mState.lock(); - this->state.workMode = (amovGimbal::AMOV_GIMBAL_SERVO_MODE_T)primary->workMode; - this->state.cameraFlag = (amovGimbal::AMOV_GIMBAL_CAMERA_FLAG_T)primary->state; + this->state.workMode = (AMOV_GIMBAL_SERVO_MODE_T)primary->workMode; + this->state.cameraFlag = (AMOV_GIMBAL_CAMERA_FLAG_T)primary->state; // 应该需要再解算一下，才能出具体的框架角度 this->state.rel.yaw = -(primary->motorYaw * XF_ANGLE_DPI); this->state.rel.yaw = this->state.rel.yaw < -180.0f ? this->state.rel.yaw + 360.0f : this->state.rel.yaw; @@ -186,7 +186,7 @@ void GX40GimbalDriver::convert(void *buf) updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, state.abs.roll, state.abs.pitch, state.abs.yaw, - state.fov.x, state.fov.y); + state.fov.x, state.fov.y, updataCaller); mState.unlock(); } @@ -213,7 +213,6 @@ uint32_t GX40GimbalDriver::calPackLen(void *pack) * * @return a boolean value, either true or false. */ -#include bool GX40GimbalDriver::parser(IN uint8_t byte) { bool state = false; diff --git a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h index d0b63a3..bfe4825 100644 --- a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h +++ b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h @@ -4,10 +4,13 @@ * @Author: L LC @amov * @Date: 2023-10-20 16:08:13 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:46:35 + * @LastEditTime: 2023-12-05 16:29:20 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h */ -#include "../amov_gimbal.h" + +#ifndef __GX40_DRIVER_H +#define __GX40_DRIVER_H + #include "../amov_gimbal_private.h" #include "GX40_gimbal_struct.h" #include @@ -16,9 +19,6 @@ #include #include -#ifndef __GX40_DRIVER_H -#define __GX40_DRIVER_H - class GX40GimbalDriver : public amovGimbal::amovGimbalBase { GX40::GIMBAL_FRAME_PARSER_STATE_T parserState; @@ -28,15 +28,15 @@ class GX40GimbalDriver : public amovGimbal::amovGimbalBase std::mutex carrierStateMutex; int16_t targetPos[3]; - - amovGimbal::AMOV_GIMBAL_POS_T carrierPos; - amovGimbal::AMOV_GIMBAL_VELOCITY_T carrierSpeed; - amovGimbal::AMOV_GIMBAL_VELOCITY_T carrierAcc; + + AMOV_GIMBAL_POS_T carrierPos; + AMOV_GIMBAL_VELOCITY_T carrierSpeed; + AMOV_GIMBAL_VELOCITY_T carrierAcc; GX40::GIMBAL_SECONDARY_MASTER_FRAME_T carrierGNSS; std::thread::native_handle_type nopSendThreadHandle; void nopSend(void); - void parserStart(amovGimbal::pStateInvoke callback); + void parserStart(pAmovGimbalStateInvoke callback); public: uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize); @@ -45,18 +45,18 @@ public: uint32_t calPackLen(void *pack); // funtions - uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); - uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); + uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos); + uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed); uint32_t setGimabalHome(void); - uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); - uint32_t setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); uint32_t takePic(void); - uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); - uint32_t attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, + uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState); + uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, + const AMOV_GIMBAL_VELOCITY_T &seppd, + const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData); uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt); diff --git a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp index c432346..3ba5771 100644 --- a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp +++ b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2023-11-02 17:50:26 * @LastEditors: L LC @amov - * @LastEditTime: 2023-12-04 09:56:21 + * @LastEditTime: 2023-12-05 16:29:13 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp */ #include @@ -12,12 +12,12 @@ /** * The function sets the target position of a gimbal based on the input roll, pitch, and yaw values. * - * @param pos The parameter "pos" is of type "amovGimbal::AMOV_GIMBAL_POS_T". It is a structure that + * @param pos The parameter "pos" is of type "AMOV_GIMBAL_POS_T". It is a structure that * contains the roll, pitch, and yaw values of the gimbal position. * * @return a packed value of type uint32_t. */ -uint32_t GX40GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos) +uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos) { carrierStateMutex.lock(); targetPos[0] = (int16_t)(-pos.roll / 0.01f); @@ -30,12 +30,12 @@ uint32_t GX40GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &po /** * The function sets the gimbal speed based on the provided roll, pitch, and yaw values. * - * @param speed The parameter "speed" is of type "amovGimbal::AMOV_GIMBAL_POS_T". It is a structure + * @param speed The parameter "speed" is of type "AMOV_GIMBAL_POS_T". It is a structure * that contains the roll, pitch, and yaw values of the gimbal speed. * * @return the result of the pack() function, which is of type uint32_t. */ -uint32_t GX40GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed) +uint32_t GX40GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed) { carrierStateMutex.lock(); targetPos[0] = (int16_t)(-speed.roll / 0.1f); @@ -81,23 +81,23 @@ uint32_t GX40GimbalDriver::takePic(void) /** * The function `setVideo` toggles the video state of a gimbal driver and returns a packed command. * - * @param newState The parameter `newState` is of type `amovGimbal::AMOV_GIMBAL_VIDEO_T`, which is an + * @param newState The parameter `newState` is of type `AMOV_GIMBAL_VIDEO_T`, which is an * enumeration representing the state of the video in the gimbal. It can have two possible values: * * @return the result of the `pack` function, which is a `uint32_t` value. */ -uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState) +uint32_t GX40GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState) { uint8_t temp = 0X01; mState.lock(); - if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) + if (state.video == AMOV_GIMBAL_VIDEO_TAKE) { - state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; + state.video = AMOV_GIMBAL_VIDEO_OFF; } else { - state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; + state.video = AMOV_GIMBAL_VIDEO_TAKE; } mState.unlock(); @@ -108,13 +108,13 @@ uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newSta * The function `attitudeCorrection` updates the state of a gimbal driver with position, velocity, and * acceleration data. * - * @param pos The "pos" parameter is of type "amovGimbal::AMOV_GIMBAL_POS_T" and represents the current + * @param pos The "pos" parameter is of type "AMOV_GIMBAL_POS_T" and represents the current * position of the gimbal. It likely contains information such as the pitch, yaw, and roll angles of * the gimbal. * @param seppd The parameter `seppd` stands for "Separate Pointing Device" and represents the velocity * of the gimbal in separate axes (e.g., pitch, yaw, roll). It is of type - * `amovGimbal::AMOV_GIMBAL_VELOCITY_T`. - * @param acc The "acc" parameter is of type "amovGimbal::AMOV_GIMBAL_VELOCITY_T" and represents the + * `AMOV_GIMBAL_VELOCITY_T`. + * @param acc The "acc" parameter is of type "AMOV_GIMBAL_VELOCITY_T" and represents the * acceleration of the gimbal. * @param extenData The extenData parameter is a pointer to additional data that can be passed to the * attitudeCorrection function. It can be used to provide any extra information or context that may be @@ -122,12 +122,12 @@ uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newSta * not provided in the code snippet, * * @return the size of the data being passed as arguments. The size is calculated by adding the sizes - * of the three types: sizeof(amovGimbal::AMOV_GIMBAL_POS_T), - * sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T), and sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T). + * of the three types: sizeof(AMOV_GIMBAL_POS_T), + * sizeof(AMOV_GIMBAL_VELOCITY_T), and sizeof(AMOV_GIMBAL_VELOCITY_T). */ -uint32_t GX40GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, +uint32_t GX40GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, + const AMOV_GIMBAL_VELOCITY_T &seppd, + const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData) { carrierStateMutex.lock(); @@ -136,7 +136,7 @@ uint32_t GX40GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_ carrierAcc = acc; upDataTs = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); carrierStateMutex.unlock(); - return sizeof(amovGimbal::AMOV_GIMBAL_POS_T) + sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T) + sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T); + return sizeof(AMOV_GIMBAL_POS_T) + sizeof(AMOV_GIMBAL_VELOCITY_T) + sizeof(AMOV_GIMBAL_VELOCITY_T); } /** @@ -159,7 +159,7 @@ uint32_t GX40GimbalDriver::extensionFuntions(void *cmd) * The function `setGimbalZoom` in the `GX40GimbalDriver` class sets the zoom level of a gimbal based on * the specified zoom type and target rate. * - * @param zoom The "zoom" parameter is of type amovGimbal::AMOV_GIMBAL_ZOOM_T, which is an enumeration + * @param zoom The "zoom" parameter is of type AMOV_GIMBAL_ZOOM_T, which is an enumeration * type. It represents the zoom action to be performed on the gimbal. The possible values for this * parameter are: * @param targetRate The targetRate parameter is a float value representing the desired zoom rate for @@ -167,7 +167,7 @@ uint32_t GX40GimbalDriver::extensionFuntions(void *cmd) * * @return a value of type uint32_t. */ -uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) +uint32_t GX40GimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate) { uint8_t temp[4]; uint8_t len = 0; @@ -177,13 +177,13 @@ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl len = 1; switch (zoom) { - case amovGimbal::AMOV_GIMBAL_ZOOM_IN: + case AMOV_GIMBAL_ZOOM_IN: temp[0] = GX40::GIMBAL_CMD_ZOMM_IN; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_OUT: + case AMOV_GIMBAL_ZOOM_OUT: temp[0] = GX40::GIMBAL_CMD_ZOOM_OUT; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_STOP: + case AMOV_GIMBAL_ZOOM_STOP: temp[0] = GX40::GIMBAL_CMD_ZOOM_STOP; break; } @@ -204,7 +204,7 @@ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl * The function "setGimbalFocus" sets the focus of a gimbal by specifying the zoom level and target * rate. * - * @param zoom The zoom parameter is of type amovGimbal::AMOV_GIMBAL_ZOOM_T, which is an enumeration + * @param zoom The zoom parameter is of type AMOV_GIMBAL_ZOOM_T, which is an enumeration * type representing different zoom levels for the gimbal. It is used to specify the desired zoom level * for the gimbal focus. * @param targetRate The targetRate parameter is a float value representing the desired zoom rate for @@ -212,7 +212,7 @@ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl * * @return the result of the pack() function, which is of type uint32_t. */ -uint32_t GX40GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) +uint32_t GX40GimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate) { uint8_t temp = 0X01; diff --git a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h index b5d65ce..39abbb3 100644 --- a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h +++ b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2023-10-20 16:08:13 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:46:22 + * @LastEditTime: 2023-12-05 16:28:54 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h */ #ifndef GX40_GIMBAL_STRUCT_H diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h index 7273d2b..c680cf7 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h @@ -3,12 +3,11 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:06 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:45:25 + * @LastEditTime: 2023-12-05 16:28:29 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h */ #ifndef Q10F_GIMBAL_CRC32_H #define Q10F_GIMBAL_CRC32_H - namespace Q10f { static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght) diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp index 4b7d27f..a89058c 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:06 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:45:31 + * @LastEditTime: 2023-12-05 17:23:15 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp */ #include "Q10f_gimbal_driver.h" @@ -86,7 +86,7 @@ void Q10fGimbalDriver::convert(void *buf) state.rel.pitch = tempPos->pitchStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, state.abs.roll, state.abs.pitch, state.abs.yaw, - state.fov.x, state.fov.y); + state.fov.x, state.fov.y, updataCaller); mState.unlock(); break; diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h index a480f5b..f6c3a67 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h @@ -3,19 +3,18 @@ * @Author: L LC @amov * @Date: 2022-10-28 12:24:21 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:45:36 + * @LastEditTime: 2023-12-05 16:27:45 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h */ -#include "../amov_gimbal.h" +#ifndef __Q10F_DRIVER_H +#define __Q10F_DRIVER_H + #include "../amov_gimbal_private.h" #include "Q10f_gimbal_struct.h" #include #include #include -#ifndef __Q10F_DRIVER_H -#define __Q10F_DRIVER_H - class Q10fGimbalDriver : protected amovGimbal::amovGimbalBase { private: @@ -29,16 +28,16 @@ private: public: // funtions - uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos); - uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed); - uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed); + uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos); + uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed); + uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed); uint32_t setGimabalHome(void); - uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); - uint32_t setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); uint32_t takePic(void); - uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); + uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState); // builds static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp index 6f2b7ab..c63da32 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2023-03-02 10:00:52 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:45:41 + * @LastEditTime: 2023-12-05 16:27:39 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp */ #include "Q10f_gimbal_driver.h" @@ -17,7 +17,7 @@ * * @return The return value is the number of bytes written to the buffer. */ -uint32_t Q10fGimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos) +uint32_t Q10fGimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos) { Q10f::GIMBAL_SET_POS_MSG_T temp; temp.modeR = Q10f::GIMBAL_CMD_POS_MODE_ANGLE_SPEED; @@ -41,7 +41,7 @@ uint32_t Q10fGimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &po * * @return The return value is the number of bytes written to the buffer. */ -uint32_t Q10fGimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed) +uint32_t Q10fGimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed) { Q10f::GIMBAL_SET_POS_MSG_T temp; temp.modeR = Q10f::GIMBAL_CMD_POS_MODE_SPEED; @@ -64,7 +64,7 @@ uint32_t Q10fGimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T & * * @return The return value is the number of bytes written to the buffer. */ -uint32_t Q10fGimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed) +uint32_t Q10fGimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed) { state.maxFollow.pitch = followSpeed.pitch / 0.1220740379f; state.maxFollow.roll = followSpeed.roll / 0.1220740379f; @@ -108,25 +108,25 @@ uint32_t Q10fGimbalDriver::takePic(void) * * @return The return value is the number of bytes written to the serial port. */ -uint32_t Q10fGimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState) +uint32_t Q10fGimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState) { uint8_t cmd[2] = {0X01, 0XFF}; - if (newState == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) + if (newState == AMOV_GIMBAL_VIDEO_TAKE) { cmd[0] = 0X02; - state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; + state.video = AMOV_GIMBAL_VIDEO_TAKE; } else { cmd[0] = 0X03; - state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; + state.video = AMOV_GIMBAL_VIDEO_OFF; } return pack(Q10f::GIMBAL_CMD_CAMERA, (uint8_t *)cmd, sizeof(cmd)); } -uint32_t Q10fGimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) +uint32_t Q10fGimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate) { uint8_t cmd[5] = {0X00, 0X00, 0X00, 0X00, 0XFF}; if (targetRate == 0.0f) @@ -134,13 +134,13 @@ uint32_t Q10fGimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl cmd[1] = 0XFF; switch (zoom) { - case amovGimbal::AMOV_GIMBAL_ZOOM_IN: + case AMOV_GIMBAL_ZOOM_IN: cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_OUT: + case AMOV_GIMBAL_ZOOM_OUT: cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_STOP: + case AMOV_GIMBAL_ZOOM_STOP: cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP; break; default: @@ -159,18 +159,18 @@ uint32_t Q10fGimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl } } -uint32_t Q10fGimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) +uint32_t Q10fGimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate) { uint8_t cmd[2] = {0X00, 0XFF}; switch (zoom) { - case amovGimbal::AMOV_GIMBAL_ZOOM_IN: + case AMOV_GIMBAL_ZOOM_IN: cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_OUT: + case AMOV_GIMBAL_ZOOM_OUT: cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT; break; - case amovGimbal::AMOV_GIMBAL_ZOOM_STOP: + case AMOV_GIMBAL_ZOOM_STOP: cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP; break; default: diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h index 9668b4b..b9e0e2d 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2022-10-27 18:10:07 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:45:46 + * @LastEditTime: 2023-12-05 16:27:27 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h */ #ifndef Q10F_GIMBAL_STRUCT_H diff --git a/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h b/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h new file mode 100755 index 0000000..ef0ac0d --- /dev/null +++ b/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h @@ -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 +#include +#include + +#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 \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_c.h b/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_c.h new file mode 100644 index 0000000..8926cfe --- /dev/null +++ b/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_c.h @@ -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 +#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 diff --git a/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_struct.h b/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_struct.h new file mode 100755 index 0000000..6171518 --- /dev/null +++ b/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_struct.h @@ -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 + +#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 diff --git a/gimbal_ctrl/driver/src/amov_gimbal.cpp b/gimbal_ctrl/driver/src/amov_gimbal.cpp deleted file mode 100755 index 36129df..0000000 --- a/gimbal_ctrl/driver/src/amov_gimbal.cpp +++ /dev/null @@ -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 -#include -#include -#include - -#define MAX_PACK_SIZE 280 - -namespace amovGimbal -{ - typedef amovGimbal::amovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO); - typedef std::map 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; -} diff --git a/gimbal_ctrl/driver/src/amov_gimbal.h b/gimbal_ctrl/driver/src/amov_gimbal.h deleted file mode 100755 index cb203c6..0000000 --- a/gimbal_ctrl/driver/src/amov_gimbal.h +++ /dev/null @@ -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 -#include -#include - -#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 \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/amov_gimbal_factory.cpp b/gimbal_ctrl/driver/src/amov_gimbal_factory.cpp new file mode 100644 index 0000000..d90c251 --- /dev/null +++ b/gimbal_ctrl/driver/src/amov_gimbal_factory.cpp @@ -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 +#include + +namespace amovGimbalFactory +{ + typedef amovGimbal::amovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO); + typedef std::map 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)); +} diff --git a/gimbal_ctrl/driver/src/amov_gimbal_interface.cpp b/gimbal_ctrl/driver/src/amov_gimbal_interface.cpp new file mode 100644 index 0000000..ffc74f5 --- /dev/null +++ b/gimbal_ctrl/driver/src/amov_gimbal_interface.cpp @@ -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; +} diff --git a/gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp b/gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp new file mode 100644 index 0000000..d0e6970 --- /dev/null +++ b/gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp @@ -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 + +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(); +} \ No newline at end of file diff --git a/gimbal_ctrl/driver/src/amov_gimbal_private.h b/gimbal_ctrl/driver/src/amov_gimbal_private.h index 1b97255..1380639 100644 --- a/gimbal_ctrl/driver/src/amov_gimbal_private.h +++ b/gimbal_ctrl/driver/src/amov_gimbal_private.h @@ -3,7 +3,7 @@ * @Author : Aiyangsky * @Date : 2023-05-13 10:39:20 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:45:18 + * @LastEditTime: 2023-12-05 17:18:06 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_private.h */ #ifndef __AMOV_GIMABL_PRIVATE_H @@ -12,12 +12,12 @@ #include #include #include - #include #include #include -#include "amov_gimbal.h" +#include "amovGimbal/amov_gimbal.h" +#include "amovGimbal/amov_gimbal_c.h" #include "Ring_Fifo.h" #include "amov_tool.h" @@ -28,9 +28,19 @@ namespace amovGimbal public: AMOV_GIMBAL_STATE_T state; std::mutex mState; - IOStreamBase *IO; - pStateInvoke updateGimbalStateCallback; - pMsgInvoke msgCustomCallback = idleMsgCallback; + + // IO类 + IOStreamBase *IO = nullptr; + // 适用于C的函数指针 + void *inBytesCaller = nullptr; + pAmovGimbalInputBytesInvoke inBytes = nullptr; + void *outBytesCaller = nullptr; + pAmovGimbalOutputBytesInvoke outBytes = nullptr; + + void *updataCaller = nullptr; + pAmovGimbalStateInvoke updateGimbalStateCallback; + void *msgCaller = nullptr; + pAmovGimbalMsgInvoke msgCustomCallback = idleMsgCallback; fifoRing *rxQueue; fifoRing *txQueue; @@ -39,7 +49,7 @@ namespace amovGimbal std::thread::native_handle_type sendThreadHanle = 0; std::thread::native_handle_type stackThreadHanle = 0; - PamovGimbalBase(SET IOStreamBase *_IO) + PamovGimbalBase(IOStreamBase *_IO) { IO = _IO; } @@ -61,6 +71,37 @@ namespace amovGimbal } }; + // Device interface + class IamovGimbalBase + { + public: + IamovGimbalBase() {} + virtual ~IamovGimbalBase() {} + + // non-block functions + virtual uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos); + virtual uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed); + virtual uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed); + + virtual uint32_t setGimabalHome(void); + virtual uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + virtual uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0); + virtual uint32_t setGimbalROI(const AMOV_GIMBAL_ROI_T area); + virtual uint32_t takePic(void); + virtual uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState); + virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, const AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData); + virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData); + virtual uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt); + virtual uint32_t extensionFuntions(void *cmd); + + // block functions + virtual bool setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos); + virtual bool setGimabalHomeBlock(void); + virtual bool setGimbalZoomBlock(float targetRate); + virtual bool takePicBlock(void); + virtual bool calibrationBlock(void); + }; + class amovGimbalBase : public IamovGimbalBase, public PamovGimbalBase { public: @@ -77,7 +118,7 @@ namespace amovGimbal virtual void mainLoop(void); virtual void stackStart(void); - virtual void parserStart(amovGimbal::pStateInvoke callback); + virtual void parserStart(pAmovGimbalStateInvoke callback, void *caller); virtual void nodeSet(SET uint32_t _self, SET uint32_t _remote); diff --git a/gimbal_ctrl/driver/src/amov_gimbal_realize.cpp b/gimbal_ctrl/driver/src/amov_gimbal_realize.cpp new file mode 100644 index 0000000..aa651f3 --- /dev/null +++ b/gimbal_ctrl/driver/src/amov_gimbal_realize.cpp @@ -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 + +#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]); + } +} diff --git a/gimbal_ctrl/driver/src/amov_gimbal_struct.h b/gimbal_ctrl/driver/src/amov_gimbal_struct.h deleted file mode 100755 index 991440a..0000000 --- a/gimbal_ctrl/driver/src/amov_gimbal_struct.h +++ /dev/null @@ -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 - -#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 diff --git a/gimbal_ctrl/driver/src/amov_tool.h b/gimbal_ctrl/driver/src/amov_tool.h index 0e7ae9d..027bf79 100644 --- a/gimbal_ctrl/driver/src/amov_tool.h +++ b/gimbal_ctrl/driver/src/amov_tool.h @@ -3,10 +3,12 @@ * @Author: L LC @amov * @Date: 2023-07-31 18:30:33 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:44:49 + * @LastEditTime: 2023-12-05 16:26:05 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_tool.h */ +#ifndef __AMOVGIMABL_TOOL_H +#define __AMOVGIMABL_TOOL_H namespace amovGimbalTools { static inline unsigned short conversionBigLittle(unsigned short value) @@ -27,3 +29,4 @@ namespace amovGimbalTools return temp; } } +#endif diff --git a/gimbal_ctrl/sv_gimbal.cpp b/gimbal_ctrl/sv_gimbal.cpp index 17edcd1..11f0123 100644 --- a/gimbal_ctrl/sv_gimbal.cpp +++ b/gimbal_ctrl/sv_gimbal.cpp @@ -3,11 +3,10 @@ * @Author: L LC @amov * @Date: 2023-04-12 09:12:52 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:43:39 + * @LastEditTime: 2023-12-05 17:33:29 * @FilePath: /SpireCV/gimbal_ctrl/sv_gimbal.cpp */ -#include "amov_gimbal.h" -#include "amov_gimbal_struct.h" +#include "amovGimbal/amov_gimbal.h" #include "sv_gimbal.h" #include "sv_gimbal_io.hpp" @@ -20,19 +19,42 @@ namespace sv std::map Gimbal::IOList; std::mutex Gimbal::IOListMutex; - std::map gimbaltypeList = - { - {GimbalType::G1, "G1"}, - {GimbalType::Q10f, "Q10f"}, - {GimbalType::AT10, "AT10"}, - {GimbalType::GX40, "GX40"}}; - - std::string &cvGimbalType2Str(const GimbalType &type) + typedef struct { - std::map::iterator temp = gimbaltypeList.find(type); + std::string name; + GimbalLink supportLink; + } gimbalTrait; + + std::map gimbaltypeList = + { + {GimbalType::G1, {"G1", GimbalLink::SERIAL}}, + {GimbalType::Q10f, {"Q10f", GimbalLink::SERIAL}}, + {GimbalType::AT10, {"AT10", GimbalLink::SERIAL | GimbalLink::ETHERNET_TCP}}, + {GimbalType::GX40, {"GX40", GimbalLink::SERIAL | GimbalLink::ETHERNET_TCP | GimbalLink::ETHERNET_UDP}}}; + + /** + * The function `svGimbalType2Str` converts a `GimbalType` enum value to its corresponding string + * representation. + * + * @return a reference to a string. + */ + std::string &svGimbalType2Str(const GimbalType &type) + { + std::map::iterator temp = gimbaltypeList.find(type); if (temp != gimbaltypeList.end()) { - return (temp->second); + return (temp->second).name; + } + throw "Error: Unsupported gimbal device type!!!!"; + exit(-1); + } + + GimbalLink &svGimbalTypeFindLinkType(const GimbalType &type) + { + std::map::iterator temp = gimbaltypeList.find(type); + if (temp != gimbaltypeList.end()) + { + return (temp->second).supportLink; } throw "Error: Unsupported gimbal device type!!!!"; exit(-1); @@ -101,17 +123,29 @@ void sv::Gimbal::setNetIp(const std::string &ip) } /** - * This function sets the network port for a Gimbal object in C++. + * The function sets the TCP network port for the Gimbal object. * - * @param port The "port" parameter is an integer value that represents the network port number that - * the Gimbal object will use for communication. This function sets the value of the "m_net_port" - * member variable of the Gimbal object to the value passed in as the "port" parameter. + * @param port The parameter "port" is an integer that represents the TCP network port number. */ -void sv::Gimbal::setNetPort(const int &port) +void sv::Gimbal::setTcpNetPort(const int &port) { this->m_net_port = port; } +/** + * The function sets the UDP network ports for receiving and sending data. + * + * @param recvPort The recvPort parameter is the port number that the Gimbal object will use to receive + * UDP packets. + * @param sendPort The sendPort parameter is the port number used for sending data over UDP (User + * Datagram Protocol) network communication. + */ +void sv::Gimbal::setUdpNetPort(const int &recvPort, const int &sendPort) +{ + this->m_net_recv_port = recvPort; + this->m_net_send_port = sendPort; +} + /** * The function sets a parser callback for a gimbal device. * @@ -122,25 +156,31 @@ void sv::Gimbal::setNetPort(const int &port) void sv::Gimbal::setStateCallback(sv::PStateInvoke callback) { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - pdevTemp->setParserCallback(callback); + m_callback = callback; + pdevTemp->setParserCallback(sv::Gimbal::gimbalUpdataCallback, this); } /** - * The function `creatIO` creates an IO object for a gimbal device and adds it to a list of IO objects - * if it doesn't already exist. + * The function `sv::Gimbal::creatIO` creates an IO object based on the specified gimbal type and link + * type, and returns a pointer to the created object. * - * @param dev A pointer to an instance of the `Gimbal` class. - * @param IO The parameter "IO" is a pointer to an object that represents the input/output (IO) device - * for the gimbal. It is used to establish a connection with the gimbal and perform communication - * operations. + * @param dev The "dev" parameter is a pointer to an object of type "sv::Gimbal". It is used to access + * the member variables of the Gimbal object, such as "m_serial_port", "m_serial_baud_rate", + * "m_serial_timeout", etc. These variables store information about * - * @return a boolean value. + * @return a void pointer. */ void *sv::Gimbal::creatIO(sv::Gimbal *dev) { IOListMutex.lock(); std::map::iterator list = IOList.find(dev->m_serial_port); std::pair key("NULL", nullptr); + GimbalLink link = svGimbalTypeFindLinkType(dev->m_gimbal_type); + + if ((dev->m_gimbal_link & svGimbalTypeFindLinkType(dev->m_gimbal_type)) == GimbalLink::NONE) + { + throw std::runtime_error("gimbal Unsupported linktype !!!"); + } if (list == IOList.end()) { @@ -160,20 +200,35 @@ void *sv::Gimbal::creatIO(sv::Gimbal *dev) } else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP) { + TCPClient *tcp; + tcp = new TCPClient(dev->m_net_ip, dev->m_net_port); + key.first = dev->m_net_ip; + key.second = (void *)tcp; + IOList.insert(key); } else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP) { + UDP *udp; + udp = new UDP(dev->m_net_ip, dev->m_net_recv_port, dev->m_net_send_port); + key.first = dev->m_net_ip; + key.second = (void *)udp; + IOList.insert(key); } - else - { - std::cout << "Error: gimbal IO has opened!!!" << std::endl; - } + } + else + { + std::cout << "Error: gimbal IO has opened!!!" << std::endl; } IOListMutex.unlock(); return key.second; } +/** + * The function removes a Gimbal device from the IOList. + * + * @param dev dev is a pointer to an object of type sv::Gimbal. + */ void sv::Gimbal::removeIO(sv::Gimbal *dev) { IOListMutex.lock(); @@ -181,6 +236,7 @@ void sv::Gimbal::removeIO(sv::Gimbal *dev) IOListMutex.unlock(); } + /** * The function opens a communication interface with a gimbal device and sets up a parser to handle * incoming data. @@ -192,7 +248,6 @@ void sv::Gimbal::removeIO(sv::Gimbal *dev) * * @return A boolean value is being returned. */ -#include "stdlib.h" bool sv::Gimbal::open(PStateInvoke callback) { bool ret = false; @@ -202,11 +257,12 @@ bool sv::Gimbal::open(PStateInvoke callback) if (this->IO != nullptr) { std::string driverName; - driverName = sv::cvGimbalType2Str(this->m_gimbal_type); + driverName = sv::svGimbalType2Str(this->m_gimbal_type); this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO); amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; pdevTemp->startStack(); - pdevTemp->parserAuto(callback); + m_callback = callback; + pdevTemp->parserAuto(sv::Gimbal::gimbalUpdataCallback, this); ret = true; } @@ -224,7 +280,7 @@ bool sv::Gimbal::open(PStateInvoke callback) bool sv::Gimbal::setHome() { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - if (pdevTemp->dev->setGimabalHome() > 0) + if (pdevTemp->setGimabalHome() > 0) { return true; } @@ -253,7 +309,7 @@ bool sv::Gimbal::setZoom(double x) return false; } - if (pdevTemp->dev->setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_STOP, x) > 0) + if (pdevTemp->setGimbalZoom(AMOV_GIMBAL_ZOOM_STOP, x) > 0) { return true; } @@ -277,7 +333,7 @@ bool sv::Gimbal::setAutoZoom(int state) { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - if (pdevTemp->dev->setGimbalZoom((amovGimbal::AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0) + if (pdevTemp->setGimbalZoom((AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0) { return true; } @@ -303,7 +359,7 @@ bool sv::Gimbal::setAutoFocus(int state) { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - if (pdevTemp->dev->setGimbalFocus((amovGimbal::AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0) + if (pdevTemp->setGimbalFocus((AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0) { return true; } @@ -323,7 +379,7 @@ bool sv::Gimbal::takePhoto() { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - if (pdevTemp->dev->takePic() > 0) + if (pdevTemp->takePic() > 0) { return true; } @@ -347,21 +403,21 @@ bool sv::Gimbal::takeVideo(int state) { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - amovGimbal::AMOV_GIMBAL_VIDEO_T newState; + AMOV_GIMBAL_VIDEO_T newState; switch (state) { case 0: - newState = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; + newState = AMOV_GIMBAL_VIDEO_OFF; break; case 1: - newState = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE; + newState = AMOV_GIMBAL_VIDEO_TAKE; break; default: - newState = amovGimbal::AMOV_GIMBAL_VIDEO_OFF; + newState = AMOV_GIMBAL_VIDEO_OFF; break; } - if (pdevTemp->dev->setVideo(newState) > 0) + if (pdevTemp->setVideo(newState) > 0) { return true; } @@ -382,13 +438,13 @@ int sv::Gimbal::getVideoState() { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; int ret; - amovGimbal::AMOV_GIMBAL_STATE_T state; + AMOV_GIMBAL_STATE_T state; state = pdevTemp->getGimabalState(); - if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE) + if (state.video == AMOV_GIMBAL_VIDEO_TAKE) { ret = 1; } - else if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_OFF) + else if (state.video == AMOV_GIMBAL_VIDEO_OFF) { ret = 0; } @@ -416,7 +472,7 @@ void sv::Gimbal::setAngleEuler(double roll, double pitch, double yaw, { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - amovGimbal::AMOV_GIMBAL_POS_T temp; + AMOV_GIMBAL_POS_T temp; if (pitch_rate == 0.0f) pitch_rate = 360; @@ -428,11 +484,11 @@ void sv::Gimbal::setAngleEuler(double roll, double pitch, double yaw, temp.pitch = pitch_rate; temp.roll = roll_rate; temp.yaw = yaw_rate; - pdevTemp->dev->setGimabalFollowSpeed(temp); + pdevTemp->setGimabalFollowSpeed(temp); temp.pitch = pitch; temp.roll = roll; temp.yaw = yaw; - pdevTemp->dev->setGimabalPos(temp); + pdevTemp->setGimabalPos(temp); } /** @@ -447,11 +503,11 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y { amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev; - amovGimbal::AMOV_GIMBAL_POS_T temp; + AMOV_GIMBAL_POS_T temp; temp.pitch = pitch_rate; temp.roll = roll_rate; temp.yaw = yaw_rate; - pdevTemp->dev->setGimabalSpeed(temp); + pdevTemp->setGimabalSpeed(temp); } sv::Gimbal::~Gimbal() diff --git a/gimbal_ctrl/sv_gimbal_io.hpp b/gimbal_ctrl/sv_gimbal_io.hpp index e74d1bc..5a0fc4a 100644 --- a/gimbal_ctrl/sv_gimbal_io.hpp +++ b/gimbal_ctrl/sv_gimbal_io.hpp @@ -3,65 +3,236 @@ * @Author: L LC @amov * @Date: 2023-04-12 12:22:09 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:43:49 + * @LastEditTime: 2023-12-05 17:38:59 * @FilePath: /SpireCV/gimbal_ctrl/sv_gimbal_io.hpp */ #ifndef __SV_GIMABL_IO_H #define __SV_GIMABL_IO_H -#include "amov_gimbal.h" +#include "amovGimbal/amov_gimbal.h" #include "serial/serial.h" -class UART : public amovGimbal::IOStreamBase +#include +#include +#if defined(_WIN32) +#include +#include +#endif + +#if defined(__linux__) +#include +#endif + +#include +namespace sv { -private: - serial::Serial *ser; + class UART : public amovGimbal::IOStreamBase + { + private: + serial::Serial *ser; -public: - virtual bool open() - { - ser->open(); - return true; - } - virtual bool close() - { - ser->close(); - return true; - } - virtual bool isOpen() - { - return ser->isOpen(); - } - virtual bool isBusy() - { - return false; - } + public: + virtual bool open() + { + ser->open(); + return true; + } + virtual bool close() + { + ser->close(); + return true; + } + virtual bool isOpen() + { + return ser->isOpen(); + } + virtual bool isBusy() + { + return false; + } + virtual uint32_t inPutBytes(IN uint8_t *byte) + { + if (ser->read(byte, 1)) + { + return 1; + } + return 0; + } + virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) + { + return ser->write(byte, lenght); + } + UART(const std::string &port, uint32_t baudrate, serial::Timeout timeout, + serial::bytesize_t bytesize, serial::parity_t parity, serial::stopbits_t stopbits, + serial::flowcontrol_t flowcontrol) + { + ser = new serial::Serial(port, baudrate, timeout, bytesize, parity, stopbits, flowcontrol); + } + ~UART() + { + ser->close(); + } + }; - virtual bool inPutByte(IN uint8_t *byte) + int scoketClose(int scoket) { - if (ser->read(byte, 1)) +#if defined(_WIN32) + return closesocket(scoket); +#endif +#if defined(__linux__) + return close(scoket); +#endif + } + class TCPClient : public amovGimbal::IOStreamBase + { + private: + int scoketFd; + sockaddr_in scoketAddr; + + public: + virtual bool open() { return true; } - return false; - } + virtual bool close() + { + sv::scoketClose(scoketFd); + return true; + } + virtual bool isOpen() + { + return true; + } + virtual bool isBusy() + { + return false; + } + virtual uint32_t inPutBytes(IN uint8_t *byte) + { + return recv(scoketFd, (char *)byte, 65536, 0); + } + virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) + { + return send(scoketFd, (const char *)byte, lenght, 0); + } - virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) - { - return ser->write(byte, lenght); - } + TCPClient(const std::string &addr, const uint16_t port) + { + if ((scoketFd = socket(AF_INET, SOCK_STREAM, 0)) == -1) + { + throw std::runtime_error("scoket creat failed"); + } + memset(&scoketAddr, 0, sizeof(scoketAddr)); + scoketAddr.sin_family = AF_INET; + scoketAddr.sin_addr.s_addr = inet_addr(addr.c_str()); - UART(const std::string &port, uint32_t baudrate, serial::Timeout timeout, - serial::bytesize_t bytesize, serial::parity_t parity, serial::stopbits_t stopbits, - serial::flowcontrol_t flowcontrol) + if (scoketAddr.sin_addr.s_addr == INADDR_NONE || + scoketAddr.sin_addr.s_addr == INADDR_ANY) + { + sv::scoketClose(scoketFd); + + throw std::runtime_error("scoket addr errr"); + } + scoketAddr.sin_port = htons(port); + + if (connect(scoketFd, (struct sockaddr *)&scoketAddr, sizeof(scoketAddr)) != 0) + { + sv::scoketClose(scoketFd); + + throw std::runtime_error("scoket connect failed !"); + } + } + ~TCPClient() + { + close(); + } + }; + + class UDP : public amovGimbal::IOStreamBase { - ser = new serial::Serial(port, baudrate, timeout, bytesize, parity, stopbits, flowcontrol); - } - ~UART() - { - ser->close(); - delete ser; - } -}; + private: + int rcvScoketFd, sendScoketFd; + sockaddr_in rcvScoketAddr, sendScoketAddr; + + public: + virtual bool open() + { + return true; + } + virtual bool close() + { + sv::scoketClose(rcvScoketFd); + sv::scoketClose(sendScoketFd); + return true; + } + virtual bool isOpen() + { + return true; + } + virtual bool isBusy() + { + return false; + } + virtual uint32_t inPutBytes(IN uint8_t *byte) + { + sockaddr_in remoteAddr; + int len = sizeof(remoteAddr); + + return recvfrom(rcvScoketFd, (char *)byte, 65536, 0, + (struct sockaddr *)&remoteAddr, reinterpret_cast(&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 \ No newline at end of file diff --git a/include/sv_gimbal.h b/include/sv_gimbal.h index 31cd858..0df5dcd 100644 --- a/include/sv_gimbal.h +++ b/include/sv_gimbal.h @@ -3,8 +3,8 @@ * @Author: jario-jin @amov * @Date: 2023-04-12 09:12:52 * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-18 11:49:27 - * @FilePath: /spirecv-gimbal-sdk/include/sv_gimbal.h + * @LastEditTime: 2023-12-05 17:25:40 + * @FilePath: /SpireCV/include/sv_gimbal.h */ #ifndef __SV_GIMBAL__ #define __SV_GIMBAL__ @@ -29,13 +29,24 @@ namespace sv AT10, GX40, }; - enum class GimbalLink + enum class GimbalLink : int { - SERIAL, - ETHERNET_TCP, - ETHERNET_UDP + NONE = 0x00, + SERIAL = 0x01, + ETHERNET_TCP = 0x02, + ETHERNET_UDP = 0x04, }; + constexpr GimbalLink operator|(GimbalLink a, GimbalLink b) + { + return static_cast(static_cast(a) | static_cast(b)); + } + + constexpr GimbalLink operator&(GimbalLink a, GimbalLink b) + { + return static_cast(static_cast(a) & static_cast(b)); + } + enum class GimablSerialByteSize { FIVE_BYTES = 5, @@ -84,6 +95,7 @@ namespace sv // Device pointers void *dev; void *IO; + PStateInvoke m_callback; // Generic serial interface parameters list & default parameters std::string m_serial_port = "/dev/ttyUSB0"; @@ -95,8 +107,10 @@ namespace sv int m_serial_timeout = 500; // Ethernet interface parameters list & default parameters - std::string m_net_ip = "192.168.2.64"; - int m_net_port = 9090; + std::string m_net_ip = "192.168.144.121"; + int m_net_port = 2332; + int m_net_recv_port = 2338; + int m_net_send_port = 2337; GimbalType m_gimbal_type; GimbalLink m_gimbal_link; @@ -105,6 +119,14 @@ namespace sv static std::mutex IOListMutex; static void *creatIO(Gimbal *dev); static void removeIO(Gimbal *dev); + + static void gimbalUpdataCallback(double frameAngleRoll, double frameAnglePitch, double frameAngleYaw, + double imuAngleRoll, double imuAnglePitch, double imuAngleYaw, + double fovX, double fovY, void *handle) + { + ((Gimbal *)(handle))->m_callback(frameAngleRoll, frameAnglePitch, frameAngleYaw, imuAngleRoll, imuAnglePitch, imuAngleYaw, fovX, fovY); + } + public: //! Constructor /*! @@ -126,7 +148,10 @@ namespace sv // set Ethernet interface parameters void setNetIp(const std::string &ip); - void setNetPort(const int &port); + // set tcp port + void setTcpNetPort(const int &port); + // set udp port + void setUdpNetPort(const int &recvPort, const int &sendPort); // Create a device instance void setStateCallback(PStateInvoke callback);