From f9c32a375441ccf097cabb5b8ba4f2dfef13cf87 Mon Sep 17 00:00:00 2001 From: AiYangSky <1732570904@qq.com> Date: Thu, 11 Apr 2024 07:52:39 +0000 Subject: [PATCH] Fixed a bug caused by abnormal timestamps in the ROS environment Signed-off-by: AiYangSky <1732570904@qq.com> --- gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp index a284999..4da56d9 100644 --- a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp +++ b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp @@ -28,6 +28,7 @@ GX40GimbalDriver::GX40GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal:: targetPos[2] = 0; parserState = GX40::GIMBAL_FRAME_PARSER_STATE_IDLE; + upDataTs = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()) - std::chrono::milliseconds(2000); } /** @@ -107,7 +108,8 @@ uint32_t GX40GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payl // 惯导数据填充 std::chrono::milliseconds nowTs = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); // over 1s GNSS has losed - if ((nowTs.count() - upDataTs.count()) < std::chrono::milliseconds(1500).count()) + if ((nowTs.count() - upDataTs.count()) < std::chrono::milliseconds(1500).count() && + cmd == GX40::GIMBAL_CMD_NOP) { primary->selfRoll = (int16_t)(-(carrierPos.roll / 0.01f)); primary->selfPitch = (int16_t)(-(carrierPos.pitch / 0.01f));