用atan模拟atan2以解决逐飞AI8051u开源库中的双精度浮点运算与atan2运算冲突问题

万能青***

  由于全国大学生智能汽车竞赛中越野组需要使用GNSS模块进行定位和解算，但单精度浮点运算会导致精度丢失，所以解决方案为勾选“double precision floating point”开启双精度浮点运算，以避免精度丢失问题。但与之带来的是atan2函数计算异常的情况，由于惯性导航在智能汽车上的普及，IMU模块的姿态解算尤为重要，其中四元数解算欧拉角时可能会涉及到atan2函数，所以必须用atan函数来表示出atan2函数，而不能直接调用标准数学库中的atan2函数。故分享解决原理与示例代码如下，将my_atan2函数定义并声明，在需要使用atan2的地方替换为my_atan2即可。




#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！