用atan模拟atan2以解决逐飞AI8051u开源库中的双精度浮点运算与atan2运算冲突问题
由于全国大学生智能汽车竞赛中越野组需要使用GNSS模块进行定位和解算，但单精度浮点运算会导致精度丢失，所以解决方案为勾选“double precision floating point”开启双精度浮点运算，以避免精度丢失问题。但与之带来的是atan2函数计算异常的情况，由于惯性导航在智能汽车上的普及，IMU模块的姿态解算尤为重要，其中四元数解算欧拉角时可能会涉及到atan2函数，所以必须用atan函数来表示出atan2函数，而不能直接调用标准数学库中的atan2函数。故分享解决原理与示例代码如下，将my_atan2函数定义并声明，在需要使用atan2的地方替换为my_atan2即可。https://pica.zhimg.com/v2-5b9ba5ca035a4fd0ad0a001d57c3cef2_1440w.jpg
#define M_PI 3.1415926535
double my_atan2(double y, double x) {
// 处理x = 0的情况
if (x == 0) {
if (y > 0) {
return M_PI / 2; // y轴正半轴
} else if (y < 0) {
return -M_PI / 2; // y轴负半轴
} else {
return 0; // 原点
}
}
else{
// 计算基础角度
double theta = atan(y / x);
// 处理x ≠ 0的情况
if (x > 0) {
return theta; // 第一象限和第四象限
// 处理x < 0的情况
} else {
if (y >= 0) {
return theta + M_PI; // 第二象限或x轴负半轴
} else {
return theta - M_PI; // 第三象限
}
}
}
}
仅供参考，特别鸣谢：陈工、左工、王昱顺、yan、大w！
页:
[1]