202608051132_《theta & freguency measure distance _algorithmn》

#include <math.h>
#include <stddef.h>
#include <stdint.h>

#define SPEED_OF_LIGHT_M_S 299792458.0

typedef struct {
    double freq_hz;  /* tone frequency for this channel */
    double i;        /* in-phase component  */
    double q;        /* quadrature component */
} cs_tone_t;


/**********************************************************
* 相位斜率测距。
*
* 往返相位随 phi = -4*pi*f*d/c 增加，因此对展开相位与频率进行最小二乘拟合
* 可以通过斜率得到距离：
* d = -斜率 * c / (4*pi)
*
* 音调必须按升序排列并且频率间隔足够小，这样每步的相位变化保持在 ±pi 以内
* （这限定了最大测距：d_max = c / (4 * delta_f)）。
*
* 成功返回 0 并将距离写入 *distance_m，输入错误返回 -1。
**********************************************************/

 
int cs_estimate_distance(const cs_tone_t *tones, size_t n, double *distance_m)
{
    if (tones == NULL || distance_m == NULL || n < 2U) {
        return -1;
    }

    double prev_raw = atan2(tones[0].q, tones[0].i);
    double unwrapped = prev_raw;

    /* Accumulators for the least-squares fit. */
    double sum_f = 0.0, sum_p = 0.0, sum_ff = 0.0, sum_fp = 0.0;

    for (size_t k = 0U; k < n; ++k) {
        if (k > 0U) {
            double raw = atan2(tones[k].q, tones[k].i);
            double delta = raw - prev_raw;

            /* Wrap the increment into (-pi, pi] before accumulating. */
            while (delta > M_PI)  { delta -= 2.0 * M_PI; }
            while (delta <= -M_PI) { delta += 2.0 * M_PI; }

            unwrapped += delta;
            prev_raw = raw;
        }

        const double f = tones[k].freq_hz;
        sum_f  += f;
        sum_p  += unwrapped;
        sum_ff += f * f;
        sum_fp += f * unwrapped;
    }

    const double nd = (double)n;
    const double denom = (nd * sum_ff) - (sum_f * sum_f);

    /* Degenerate: all tones at the same frequency, no slope to fit. */
    if (fabs(denom) < 1e-9) {
        return -1;
    }

    const double slope = ((nd * sum_fp) - (sum_f * sum_p)) / denom;
    double d = -slope * SPEED_OF_LIGHT_M_S / (4.0 * M_PI);

    /* Noise and multipath can push the fit slightly negative at close range. */
    if (d < 0.0) {
        d = 0.0;
    }

    *distance_m = d;
    return 0;
}