标签：NavEKF3 ardupilot off 上电 偏移 bcnPosOffsetNED UWB cpp position
解决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=bcnPosOffsetNED.x;
off_y=bcnPosOffsetNED.y;
注意，需在.cpp中定义，
然后在要减去的地方，
写上
extern float off_x,off_y;
减去即可。
为了不破坏ardupilot原本程序的封装，重构了一下。在AP_NavEKF3_core.cpp中，增加一个函数，把偏移量返回：
Vector3f NavEKF3_core::getBcnPosOffsetNED(){ // add by huang canming in 2019.4.17
return bcnPosOffsetNED;
}
同理，在AP_NavEKF3.cpp中，也需要增加对应代码：
Vector3f NavEKF3::getBcnPosOffsetNED(){ // add by Huangcanming in 2019.4.17
return core[primary].getBcnPosOffsetNED();
}
在各自相应的头文件中，应将函数声明为public
由此，在get_relative_position_NED_origin函数中，就可以调用这些函数来获取偏移，如：
case EKF_TYPE3: {
Vector3f off; // add by huang canming in 2019.4.17
Vector2f posNE;
float posD;
if (EKF3.getPosNE(-1,posNE) && EKF3.getPosD(-1,posD)) {
// position is valid
off=EKF3.getBcnPosOffsetNED(); // add by huang canming in 2019.4.17
vec.x = posNE.x-off.x;
vec.y = posNE.y-off.y;
vec.z = posD;
return true;
}
return false;
}
qqNCer
发布了15 篇原创文章 · 获赞 4 · 访问量 539
私信
关注
标签：NavEKF3,ardupilot,off,上电,偏移,bcnPosOffsetNED,UWB,cpp,position 来源： https://blog.csdn.net/qqNCer/article/details/104515459
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。