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

Fixed a bug caused by abnormal timestamps in the ROS environment 

Signed-off-by: AiYangSky <1732570904@qq.com>
This commit is contained in:
AiYangSky 2024-04-11 07:52:39 +00:00 committed by Gitee
parent 4a9adf7f98
commit f9c32a3754
No known key found for this signature in database
GPG Key ID: 173E9B9CA92EEF8F
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -28,6 +28,7 @@ GX40GimbalDriver::GX40GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::
targetPos[2] = 0; targetPos[2] = 0;
parserState = GX40::GIMBAL_FRAME_PARSER_STATE_IDLE; parserState = GX40::GIMBAL_FRAME_PARSER_STATE_IDLE;
upDataTs = std::chrono::duration_cast<std::chrono::milliseconds>(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::milliseconds>(std::chrono::system_clock::now().time_since_epoch()); std::chrono::milliseconds nowTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
// over 1s GNSS has losed // 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->selfRoll = (int16_t)(-(carrierPos.roll / 0.01f));
primary->selfPitch = (int16_t)(-(carrierPos.pitch / 0.01f)); primary->selfPitch = (int16_t)(-(carrierPos.pitch / 0.01f));