解决ardupilot中使用UWB每次上电的时候坐标不一样的问题
ctime:2019-08-02 22:41:07 +0800|1564756867
标签(空格分隔): 技术 飞控
在GCS_Common.cpp中,找到send_local_position函数,这个函数用来发送local_position给树莓派
将get_relative_position_NED_home
修改为 get_relative_position_NED_origin
后者作用为获取当前的相对坐标,前者中调用了后者,并且减去了_home
的偏移坐标,照理来说使用前者应该没问题,但实际使用中发现,他这个_home
的偏移似乎有问题,因此干脆不用他这个,自己在后面减去偏移就行了
偏移可从\libraries\AP_NavEKF3\AP_NavEKF3_RngBcnFusion.cpp
中获得,在SelectRngBcnFusion
函数中,当beacon采集了100个点做了UWB系统中心位置估测后,估测出来的位置就是偏移,即
bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x;
bcnPosOffsetNED.y = receiverPos.y - stateStruct.position.y;
将这个偏移保存下来,然后减去就是了。这里用的办法比较粗暴,直接设置全局变量off_x,off_y;
off_x=bcnP