From de87183021d6988b693756df444784af4324e2eb Mon Sep 17 00:00:00 2001 From: AiYangSky <1732570904@qq.com> Date: Mon, 19 Jun 2023 15:46:00 +0800 Subject: [PATCH] Fixed the problem that gimbal frame Angle is opposite to the attitude Angle --- .../driver/src/G1/g1_gimbal_driver.cpp | 4 +- .../driver/src/G2/g2_gimbal_driver.cpp | 4 +- gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h | 14 - .../driver/src/G2/g2_gimbal_iap_funtion.cpp | 357 ------------------ .../driver/src/Q10f/Q10f_gimbal_driver.cpp | 4 +- 5 files changed, 6 insertions(+), 377 deletions(-) delete mode 100755 gimbal_ctrl/driver/src/G2/g2_gimbal_iap_funtion.cpp diff --git a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp index 1ef00aa..5748722 100755 --- a/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp @@ -108,8 +108,8 @@ void g1GimbalDriver::convert(void *buf) state.rel.yaw = tempPos->HALL_yaw * G1_SCALE_FACTOR; state.rel.roll = tempPos->HALL_roll * G1_SCALE_FACTOR; state.rel.pitch = tempPos->HALL_pitch * G1_SCALE_FACTOR; - updateGimbalStateCallback(state.abs.roll, state.abs.pitch, state.abs.yaw, - state.rel.roll, state.rel.pitch, state.rel.yaw, + updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, + state.abs.roll, state.abs.pitch, state.abs.yaw, state.fov.x, state.fov.y); mState.unlock(); break; diff --git a/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp b/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp index cd43ab4..dda615e 100755 --- a/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp @@ -108,8 +108,8 @@ void g2GimbalDriver::convert(void *buf) { case G2::IAP_COMMAND_BLOCK_END: mState.lock(); - updateGimbalStateCallback(state.abs.roll, state.abs.pitch, state.abs.yaw, - state.rel.roll, state.rel.pitch, state.rel.yaw, + updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, + state.abs.roll, state.abs.pitch, state.abs.yaw, state.fov.x, state.fov.y); mState.unlock(); break; diff --git a/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h b/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h index 98cd2c3..d6ac388 100755 --- a/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h +++ b/gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h @@ -60,20 +60,6 @@ public: uint32_t takePic(void); uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState); -#ifdef AMOV_HOST - // iap funtion (内部源码模式提供功能 lib模式下不可见) - bool iapGetSoftInfo(std::string &info); - bool iapGetHardInfo(std::string &info); - bool iapJump(G2::GIMBAL_IAP_STATE_T &state); - bool iapFlashErase(G2::GIMBAL_IAP_STATE_T &state); - bool iapSendBlockInfo(uint32_t &startAddr, uint32_t &crc32); - bool iapSendBlockData(uint8_t offset, uint8_t *data); - bool iapFlashWrite(uint32_t &crc32, G2::GIMBAL_IAP_STATE_T &state); - - // 判断是否需要跳转 - bool iapJumpCheck(std::string &info) { return true; } -#endif - static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO) { return new g2GimbalDriver(_IO); diff --git a/gimbal_ctrl/driver/src/G2/g2_gimbal_iap_funtion.cpp b/gimbal_ctrl/driver/src/G2/g2_gimbal_iap_funtion.cpp deleted file mode 100755 index 8024c5f..0000000 --- a/gimbal_ctrl/driver/src/G2/g2_gimbal_iap_funtion.cpp +++ /dev/null @@ -1,357 +0,0 @@ -/* - * @Description: - * @Author: L LC @amov - * @Date: 2023-03-02 11:16:52 - * @LastEditors: L LC @amov - * @LastEditTime: 2023-04-18 10:13:08 - * @FilePath: /gimbal-sdk-multi-platform/src/G2/g2_gimbal_iap_funtion.cpp - */ - -#ifdef AMOV_HOST - -#include "g2_gimbal_driver.h" -#include "g2_gimbal_crc.h" -#include "string.h" - -#include - -#define MAX_WAIT_TIME_MS 2000 - -/** - * It gets the software information from the gimbal. - * - * @param info the string to store the information - * - * @return a boolean value. - */ -bool g2GimbalDriver::iapGetSoftInfo(std::string &info) -{ - uint8_t temp = 0; - bool ret = false; - G2::GIMBAL_FRAME_T ack; - - pack(G2::IAP_COMMAND_SOFT_INFO, &temp, 1); - - std::chrono::milliseconds startMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - - while (1) - { - if (getRxPack(&ack)) - { - if (ack.command == G2::IAP_COMMAND_SOFT_INFO && - ack.target == self && - ack.source == remote) - { - info = (char *)ack.data; - std::cout << info << std::endl; - ret = true; - break; - } - } - - std::chrono::milliseconds nowMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - if ((nowMs - startMs) > std::chrono::milliseconds(MAX_WAIT_TIME_MS)) - { - break; - } - } - return ret; -} - -/** - * It gets the hardware information of the gimbal. - * - * @param info the string to store the hardware information - * - * @return a boolean value. - */ -bool g2GimbalDriver::iapGetHardInfo(std::string &info) -{ - uint8_t temp = 0; - bool ret = false; - G2::GIMBAL_FRAME_T ack; - - pack(G2::IAP_COMMAND_HARDWARE_INFO, &temp, 1); - - std::chrono::milliseconds startMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - - while (1) - { - if (getRxPack(&ack)) - { - if (ack.command == G2::IAP_COMMAND_HARDWARE_INFO && - ack.target == self && - ack.source == remote) - { - info = (char *)ack.data; - std::cout << info << std::endl; - ret = true; - break; - } - } - - std::chrono::milliseconds nowMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - if ((nowMs - startMs) > std::chrono::milliseconds(MAX_WAIT_TIME_MS)) - { - break; - } - } - return ret; -} - -/** - * It sends a command to the gimbal to jump to the bootloader, and then waits for a response from the - * bootloader - * - * @param state the state of the gimbal, 0: normal, 1: iap - * - * @return The return value is a boolean. - */ -bool g2GimbalDriver::iapJump(G2::GIMBAL_IAP_STATE_T &state) -{ - uint8_t temp = 0; - bool ret = true; - G2::GIMBAL_FRAME_T ack; - - pack(G2::IAP_COMMAND_JUMP, &temp, 1); - - std::chrono::milliseconds startMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - - // It fails if the specified message is received. - while (1) - { - if (getRxPack(&ack)) - { - if (ack.command == G2::IAP_COMMAND_JUMP && - ack.target == self && - ack.source == remote) - { - state = (G2::GIMBAL_IAP_STATE_T)ack.data[1]; - ret = false; - break; - } - } - - std::chrono::milliseconds nowMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - if ((nowMs - startMs) > std::chrono::milliseconds(MAX_WAIT_TIME_MS)) - { - break; - } - } - return ret; -} - -/** - * The function sends a command to the gimbal to erase the flash memory - * - * @param state The state of the IAP process. - * - * @return The return value is a boolean. - */ -bool g2GimbalDriver::iapFlashErase(G2::GIMBAL_IAP_STATE_T &state) -{ - uint8_t temp = 0; - bool ret = false; - G2::GIMBAL_FRAME_T ack; - - pack(G2::IAP_COMMAND_FLASH_ERASE, &temp, 1); - - std::chrono::milliseconds startMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - - while (1) - { - if (getRxPack(&ack)) - { - if (ack.command == G2::IAP_COMMAND_FLASH_ERASE && - ack.target == self && - ack.source == remote) - { - state = (G2::GIMBAL_IAP_STATE_T)ack.data[1]; - ret = true; - break; - } - } - - std::chrono::milliseconds nowMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - if ((nowMs - startMs) > std::chrono::milliseconds(MAX_WAIT_TIME_MS)) - { - break; - } - } - return ret; -} - -/** - * It sends a block of data to the gimbal, and waits for an acknowledgement - * - * @param startAddr the start address of the block to be sent - * @param crc32 The CRC32 of the data to be sent. - * - * @return a boolean value. - */ -bool g2GimbalDriver::iapSendBlockInfo(uint32_t &startAddr, uint32_t &crc32) -{ - union - { - uint32_t f32; - uint8_t f8[4]; - } temp; - uint8_t buf[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - bool ret = false; - G2::GIMBAL_FRAME_T ack; - - temp.f32 = startAddr; - memcpy(buf, temp.f8, sizeof(uint32_t)); - temp.f32 = crc32; - memcpy(buf, temp.f8, sizeof(uint32_t)); - - pack(G2::IAP_COMMAND_BOLCK_INFO, buf, 8); - - std::chrono::milliseconds startMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - - while (1) - { - if (getRxPack(&ack)) - { - if (ack.command == G2::IAP_COMMAND_BOLCK_INFO && - ack.target == self && - ack.source == remote) - { - ret = true; - for (uint8_t i = 0; i < 8; i++) - { - if (buf[i] != ack.data[i]) - { - ret = false; - } - } - break; - } - } - - std::chrono::milliseconds nowMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - if ((nowMs - startMs) > std::chrono::milliseconds(MAX_WAIT_TIME_MS)) - { - break; - } - } - return ret; -} - -/** - * It sends a block of data to the gimbal, and waits for an acknowledgement - * - * @param offset the offset of the data block in the file - * @param data pointer to the data to be sent - * - * @return The return value is a boolean. - */ -bool g2GimbalDriver::iapSendBlockData(uint8_t offset, uint8_t *data) -{ - bool ret = false; - G2::GIMBAL_FRAME_T ack; - - pack(G2::IAP_COMMAND_BLOCK_START + offset, data, 64); - - std::chrono::milliseconds startMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - - while (1) - { - if (getRxPack(&ack)) - { - if (ack.command == G2::IAP_COMMAND_BLOCK_START + offset && - ack.target == self && - ack.source == remote) - { - ret = true; - for (uint8_t i = 0; i < 64; i++) - { - if (data[i] != ack.data[i]) - { - ret = false; - } - } - break; - } - } - - std::chrono::milliseconds nowMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - if ((nowMs - startMs) > std::chrono::milliseconds(MAX_WAIT_TIME_MS)) - { - break; - } - } - return ret; -} - -/** - * The function sends a block of data to the gimbal, and waits for an acknowledgement - * - * @param crc32 The CRC32 of the data block - * @param state The state of the IAP process. - * - * @return The return value is a boolean. - */ -bool g2GimbalDriver::iapFlashWrite(uint32_t &crc32, G2::GIMBAL_IAP_STATE_T &state) -{ - bool ret = false; - G2::GIMBAL_FRAME_T ack; - - union - { - uint32_t f32; - uint8_t f8[4]; - } temp; - - temp.f32 = crc32; - - pack(G2::IAP_COMMAND_BLOCK_WRITE, temp.f8, sizeof(uint32_t)); - - std::chrono::milliseconds startMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - - while (1) - { - if (getRxPack(&ack)) - { - if (ack.command == G2::IAP_COMMAND_BLOCK_WRITE && - ack.target == self && - ack.source == remote) - { - state = (G2::GIMBAL_IAP_STATE_T)ack.data[4]; - ret = true; - for (uint8_t i = 0; i < 4; i++) - { - if (temp.f8[i] != ack.data[i]) - { - ret = false; - } - } - break; - } - } - - std::chrono::milliseconds nowMs = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - if ((nowMs - startMs) > std::chrono::milliseconds(MAX_WAIT_TIME_MS)) - { - break; - } - } - return ret; -} - -#endif - diff --git a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp index 2b098d1..fc1045d 100755 --- a/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp @@ -119,8 +119,8 @@ void Q10fGimbalDriver::convert(void *buf) state.rel.yaw = tempPos->rollStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; state.rel.roll = tempPos->rollStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; state.rel.pitch = tempPos->pitchStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED; - updateGimbalStateCallback(state.abs.roll, state.abs.pitch, state.abs.yaw, - state.rel.roll, state.rel.pitch, state.rel.yaw, + updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw, + state.abs.roll, state.abs.pitch, state.abs.yaw, state.fov.x, state.fov.y); mState.unlock();