计算翻转角度

计算翻转角度

#include <stdio.h>
#include <math.h>

// 定义一个宏来将弧度转换为度数
#define RAD_TO_DEG(rad) ((rad) * 180.0 / M_PI)

// 计算俯仰角的函数
double calculatePitch(double ax, double ay, double az) {
    // 假设Z轴向下为正方向（这是许多加速度传感器的默认设置）
    // 注意：在只有重力作用的情况下，ax应该接近0（除了噪声和校准误差）
    // 但是，为了健壮性，我们仍然使用ay和az来计算pitch角
    // 需要避免除以零的情况，但由于az是重力加速度的分量，它通常不会为零
    // 然而，为了代码的健壮性，我们可以添加一个小的检查值来避免潜在的除以零错误
    double epsilon = 1e-6; // 一个非常小的数，用于避免除以零
    double pitch = RAD_TO_DEG(atan2(ay, -sqrt(ax * ax + az * az + epsilon)));
    // 注意：上面的公式中，sqrt(ax * ax + az * az + epsilon) 是为了计算重力向量的水平分量
    // 但是，由于我们假设ax很小（主要由噪声引起），我们可以近似地只使用az来计算分母
    // 这将简化为：pitch = RAD_TO_DEG(atan2(ay, -az));
    // 然而，上面的完整公式在ax不为零时提供了更准确的俯仰角计算
    // 在实际应用中，可能需要根据具体情况选择使用哪个公式

    // 为了简化，并且因为ax通常很小，我们可以使用下面的近似公式：
    // pitch = RAD_TO_DEG(-atan(ay / (-az + epsilon)));
    // 注意负号是因为当设备向前倾斜时，ay为正，而我们需要的是向下的角度（即正俯仰角）
    // 但是，上面的近似公式在ay或az接近零时可能不准确
    // 因此，在实际应用中，建议使用上面的完整公式或添加额外的逻辑来处理这种情况

    // 在这里，我们为了示例的简洁性，使用近似公式（但注释掉了），并展示完整公式
    // pitch = RAD_TO_DEG(-atan(ay / (-az + epsilon))); // 近似公式，可能不准确

    // 使用完整公式
    pitch = RAD_TO_DEG(atan2(ay, -sqrt(az * az + epsilon))); // 简化版，假设ax很小可以忽略
    // 注意：这里我们假设ax很小，因此没有将其包含在sqrt中。如果ax可能很大（例如，在动态环境中），
    // 则应该使用完整的sqrt(ax * ax + ay * ay + az * az)来计算总加速度，并从中提取俯仰角。
    // 但是，在只有重力作用的情况下，且设备相对静止时，上面的简化公式是有效的。

    // 最终的简化，考虑到ay是俯仰角的主要贡献者，且az代表重力（向下为正）：
    pitch = RAD_TO_DEG(atan2(ay, -az)); // 最常用的简化公式

    return pitch;
}

int main() {
    // 示例加速度值（这些值应该来自你的G-sensor，并且已经过校准）
    double ax = 0.0;  // X轴加速度（在绕X轴旋转时，这个值通常很小，但由于噪声可能不为零）
    double ay = 0.707; // Y轴加速度（例如，45度倾斜时的值）
    double az = -0.707; // Z轴加速度（负值表示向下，这里假设重力加速度为1g）

    // 计算俯仰角
    double pitch = calculatePitch(ax, ay, az);

    // 输出结果
    printf("Pitch: %f degrees\n", pitch);

    return 0;
}