balance-board/src/calibration.c

#include "calibration.h"
#include "ads1256.h"
#include "comm_protocol.h"

#include <errno.h>
#include <string.h>
#include <zephyr/logging/log.h>
#include <zephyr/settings/settings.h>
#include <zephyr/sys/atomic.h>

LOG_MODULE_REGISTER(calibration, LOG_LEVEL_INF);

#define CAL_SETTINGS_ROOT    "cal"
#define CAL_SETTINGS_VER     CAL_SETTINGS_ROOT "/ver"
#define CAL_SETTINGS_ZERO    CAL_SETTINGS_ROOT "/zero"
#define CAL_SETTINGS_GAIN    CAL_SETTINGS_ROOT "/gain"
#define CAL_SETTINGS_GRID_X  CAL_SETTINGS_ROOT "/grid_x"
#define CAL_SETTINGS_GRID_Y  CAL_SETTINGS_ROOT "/grid_y"
#define CAL_VERSION_L1_VALID BIT(0)
#define CAL_VERSION_L2_VALID BIT(1)
#define CAL_FLAG_PENDING     0
#define CAL_FLAG_DIRTY       0
#define CAL_AVG_COUNT        17
#define CAL_GRID_X_LEFT      (-28.33f)
#define CAL_GRID_X_MID       (0.0f)
#define CAL_GRID_X_RIGHT     (28.33f)
#define CAL_GRID_Y_BOTTOM    (-10.67f)
#define CAL_GRID_Y_MID       (0.0f)
#define CAL_GRID_Y_TOP       (10.67f)

enum cal_state {
    CAL_STATE_IDLE = 0,
    CAL_STATE_L1_IN_PROGRESS,
    CAL_STATE_L1_READY,
    CAL_STATE_L2_IN_PROGRESS,
    CAL_STATE_L2_READY,
};

struct cal_pending_cmd {
    uint8_t subcmd;
    uint8_t target;
    float param;
};

static struct cal_runtime committed;
static struct cal_runtime working;
static struct cal_pending_cmd pending_cmd;
static atomic_t pending_flags;
static atomic_t cal_flags;
static enum cal_state state                         = CAL_STATE_IDLE;
static const uint8_t mux_channels[CAL_NUM_CHANNELS] = { 0x01, 0x23, 0x45, 0x67 };

/**
 * @brief 对采样数组做原地升序排序。
 *
 * @param arr 待排序数组。
 * @param n 元素个数。
 *
 * @return 无返回值。
 */
static void sort_array(int32_t *arr, int n) {
    for (int i = 0; i < n - 1; i++) {
        for (int j = 0; j < n - i - 1; j++) {
            if (arr[j] > arr[j + 1]) {
                int32_t tmp = arr[j];
                arr[j]      = arr[j + 1];
                arr[j + 1]  = tmp;
            }
        }
    }
}

/**
 * @brief 将一份运行时标定数据重置为默认值。
 *
 * 默认值只保留“未标定但可运行”的最小语义：零点为 0，增益回落到
 * `ADC_TO_FORCE_SCALE`。这样擦除或首次上电后，系统仍能继续输出基础 CoP。
 *
 * @param[out] runtime 待重置的标定数据。
 *
 * @return 无返回值。
 */
static void cal_reset_runtime(struct cal_runtime *runtime) {
    memset(runtime, 0, sizeof(*runtime));
    for (int i = 0; i < CAL_NUM_CHANNELS; i++) {
        runtime->gain[i] = ADC_TO_FORCE_SCALE;
    }
}

/**
 * @brief 计算当前 committed 数据对应的版本位图。
 *
 * @return 版本位图。
 */
static uint32_t cal_make_version(void) {
    uint32_t version = 0U;

    if (committed.l1_zero_valid && committed.l1_gain_valid) {
        version |= CAL_VERSION_L1_VALID;
    }
    if (committed.l2_valid) {
        version |= CAL_VERSION_L2_VALID;
    }

    return version;
}

/**
 * @brief 发送标定响应帧。
 *
 * `data[0]` 固定回显 `target`，`data[1]` 固定回显当前状态机状态，其余 6 字节
 * 由命令处理逻辑按上下文填充。这样上位机至少总能知道“哪条命令作用在谁身上，
 * 执行后落在什么状态”，不需要依赖日志猜测固件内部阶段。
 *
 * @param status 响应状态码。
 * @param subcmd 子命令码。
 * @param target 命令目标。
 * @param value4 4 字节上下文值。
 * @param extra2 2 字节补充值。
 *
 * @return 无返回值。
 */
static void cal_send_resp(uint8_t status, uint8_t subcmd, uint8_t target, uint32_t value4, uint16_t extra2) {
    uint8_t data[8] = { 0 };

    data[0] = target;
    data[1] = (uint8_t)state;
    memcpy(&data[2], &value4, sizeof(value4));
    memcpy(&data[6], &extra2, sizeof(extra2));
    comm_protocol_send_cal_resp(status, subcmd, data);
}

/**
 * @brief 读取指定通道的中值 ADC 原始计数。
 *
 * 标定读数必须和正式采样走同一条 MUX/DRDY/中值链路，否则即便数学公式正确，
 * 也会因为采样路径不一致而把系统误差带进标定结果。
 *
 * @param ch 通道号，范围 0-3。
 *
 * @return 中值 ADC 计数。
 */
static int32_t cal_read_channel_median(uint8_t ch) {
    int32_t samples[CAL_AVG_COUNT];

    ads1256_write_reg(ADS1256_REG_MUX, mux_channels[ch]);
    ads1256_sync_wakeup();

    for (int i = 0; i < CAL_AVG_COUNT; i++) {
        ads1256_wait_drdy(50);
        samples[i] = ads1256_read_data();
    }

    sort_array(samples, CAL_AVG_COUNT);
    return samples[CAL_AVG_COUNT / 2];
}

/**
 * @brief 将网格点编号映射到 3x3 目标坐标。
 *
 * 编号采用行优先顺序：`0..2` 为上排，`3..5` 为中排，`6..8` 为下排。
 * 这样坐标定义集中在一个函数里，后续若手机侧编号不同，只需要改这一处映射。
 *
 * @param target 网格点编号。
 * @param[out] x 目标 X 坐标。
 * @param[out] y 目标 Y 坐标。
 *
 * @return 无返回值。
 */
static void cal_grid_target_to_xy(uint8_t target, float *x, float *y) {
    uint8_t row = target / 3U;
    uint8_t col = target % 3U;

    *x = (col == 0U) ? CAL_GRID_X_LEFT : ((col == 1U) ? CAL_GRID_X_MID : CAL_GRID_X_RIGHT);
    *y = (row == 0U) ? CAL_GRID_Y_TOP : ((row == 1U) ? CAL_GRID_Y_MID : CAL_GRID_Y_BOTTOM);
}

/**
 * @brief 用当前 committed 的 L1 参数测一次 CoP。
 *
 * L2 标定记录的是“当前测得的 CoP 与已知目标点之间的误差”，所以这里必须
 * 显式使用 committed 里的零点/增益，而不是 working 里旧版本的数据。
 *
 * @param[out] cop_x 测得的 CoP X。
 * @param[out] cop_y 测得的 CoP Y。
 *
 * @retval 0 测量成功。
 * @retval -ERANGE 总力过低，当前 CoP 无意义。
 */
static int cal_measure_cop(float *cop_x, float *cop_y) {
    static const float sensor_x[CAL_NUM_CHANNELS] = {
        +BOARD_HALF_WIDTH_CM,
        +BOARD_HALF_WIDTH_CM,
        -BOARD_HALF_WIDTH_CM,
        -BOARD_HALF_WIDTH_CM,
    };
    static const float sensor_y[CAL_NUM_CHANNELS] = {
        +BOARD_HALF_LENGTH_CM,
        -BOARD_HALF_LENGTH_CM,
        -BOARD_HALF_LENGTH_CM,
        +BOARD_HALF_LENGTH_CM,
    };
    float total = 0.0f;
    float wx    = 0.0f;
    float wy    = 0.0f;

    for (int i = 0; i < CAL_NUM_CHANNELS; i++) {
        int32_t raw = cal_read_channel_median((uint8_t)i);
        float force = (float)(raw - committed.zero[i]) * committed.gain[i];
        if (force < 0.0f) force = 0.0f;

        total += force;
        wx += force * sensor_x[i];
        wy += force * sensor_y[i];
    }

    if (total < COP_FORCE_ENTER_THRESHOLD) {
        *cop_x = 0.0f;
        *cop_y = 0.0f;
        return -ERANGE;
    }

    *cop_x = wx / total;
    *cop_y = wy / total;
    return 0;
}

/**
 * @brief 将当前 committed 数据持久化到 NVS。
 *
 * @retval 0 保存成功。
 * @retval 负值 settings 子系统返回的具体错误码。
 */
static int cal_save_all(void) {
    uint32_t version = cal_make_version();
    int err          = settings_save_one(CAL_SETTINGS_VER, &version, sizeof(version));

    if (err) return err;
    err = settings_save_one(CAL_SETTINGS_ZERO, committed.zero, sizeof(committed.zero));
    if (err) return err;
    err = settings_save_one(CAL_SETTINGS_GAIN, committed.gain, sizeof(committed.gain));
    if (err) return err;
    err = settings_save_one(CAL_SETTINGS_GRID_X, committed.grid_err_x, sizeof(committed.grid_err_x));
    if (err) return err;
    return settings_save_one(CAL_SETTINGS_GRID_Y, committed.grid_err_y, sizeof(committed.grid_err_y));
}

/**
 * @brief 擦除所有标定键。
 *
 * @retval 0 擦除成功。
 * @retval 负值 settings 子系统返回的具体错误码。
 */
static int cal_delete_all(void) {
    int err = settings_delete(CAL_SETTINGS_VER);

    if (err) return err;
    err = settings_delete(CAL_SETTINGS_ZERO);
    if (err) return err;
    err = settings_delete(CAL_SETTINGS_GAIN);
    if (err) return err;
    err = settings_delete(CAL_SETTINGS_GRID_X);
    if (err) return err;
    return settings_delete(CAL_SETTINGS_GRID_Y);
}

/**
 * @brief settings 子树加载回调。
 *
 * @param key `cal/` 后的子键名。
 * @param len 数据长度。
 * @param read_cb backend 读回调。
 * @param cb_arg backend 私有参数。
 *
 * @retval 0 处理成功。
 * @retval 负值 读失败。
 */
static int cal_settings_set(const char *key, size_t len, settings_read_cb read_cb, void *cb_arg) {
    const char *next;
    int rc;

    if (settings_name_steq(key, "ver", &next) && !next && len == sizeof(uint32_t)) {
        uint32_t version = 0U;
        rc               = read_cb(cb_arg, &version, sizeof(version));
        if (rc >= 0) {
            committed.l1_zero_valid = (version & CAL_VERSION_L1_VALID) != 0U;
            committed.l1_gain_valid = (version & CAL_VERSION_L1_VALID) != 0U;
            committed.l2_valid      = (version & CAL_VERSION_L2_VALID) != 0U;
            return 0;
        }
        return rc;
    }

    if (settings_name_steq(key, "zero", &next) && !next && len == sizeof(committed.zero)) {
        rc = read_cb(cb_arg, committed.zero, sizeof(committed.zero));
        return (rc < 0) ? rc : 0;
    }

    if (settings_name_steq(key, "gain", &next) && !next && len == sizeof(committed.gain)) {
        rc = read_cb(cb_arg, committed.gain, sizeof(committed.gain));
        return (rc < 0) ? rc : 0;
    }

    if (settings_name_steq(key, "grid_x", &next) && !next && len == sizeof(committed.grid_err_x)) {
        rc = read_cb(cb_arg, committed.grid_err_x, sizeof(committed.grid_err_x));
        return (rc < 0) ? rc : 0;
    }

    if (settings_name_steq(key, "grid_y", &next) && !next && len == sizeof(committed.grid_err_y)) {
        rc = read_cb(cb_arg, committed.grid_err_y, sizeof(committed.grid_err_y));
        return (rc < 0) ? rc : 0;
    }

    return 0;
}

SETTINGS_STATIC_HANDLER_DEFINE(calibration, CAL_SETTINGS_ROOT, NULL, cal_settings_set, NULL, NULL);

/**
 * @brief 初始化标定模块，从 NVS 加载持久化数据或使用默认值。
 *
 * @retval 0 成功。
 */
int cal_init(void) {
    cal_reset_runtime(&committed);
    cal_reset_runtime(&working);
    atomic_clear(&pending_flags);
    atomic_clear(&cal_flags);
    state = CAL_STATE_IDLE;

    (void)settings_load_subtree(CAL_SETTINGS_ROOT);

    working = committed;
    if (working.l2_valid) {
        state = CAL_STATE_L2_READY;
    } else if (working.l1_zero_valid && working.l1_gain_valid) {
        state = CAL_STATE_L1_READY;
    }

    LOG_INF("Calibration init: l1=%d l2=%d", working.l1_gain_valid, working.l2_valid);
    return 0;
}

/**
 * @brief 获取当前生效的标定数据指针。
 *
 * @return 指向内部 working copy 的只读指针。
 */
const struct cal_runtime *cal_get_working(void) {
    return &working;
}

/**
 * @brief 检查标定数据是否有更新，若有则刷新 working copy。
 *
 * @retval true 数据已刷新。
 * @retval false 无更新。
 */
bool cal_check_update(void) {
    if (!atomic_test_and_clear_bit(&cal_flags, CAL_FLAG_DIRTY)) {
        return false;
    }

    working = committed;
    return true;
}

/**
 * @brief 入队一条来自 BLE 的标定命令。
 *
 * @param subcmd 子命令码。
 * @param target 通道号或网格点号。
 * @param param 浮点参数。
 *
 * @retval 0 命令已入队。
 * @retval -EBUSY 上一条命令尚未被消费。
 * @retval -EINVAL 参数非法。
 */
int cal_enqueue_command(uint8_t subcmd, uint8_t target, float param) {
    switch (subcmd) {
    case CAL_SUBCMD_TARE_CH:
    case CAL_SUBCMD_GAIN_CH:
        if (target >= CAL_NUM_CHANNELS) return -EINVAL;
        break;
    case CAL_SUBCMD_RECORD_GRID:
        if (target >= CAL_NUM_GRID_PTS) return -EINVAL;
        break;
    default:
        break;
    }

    pending_cmd.subcmd = subcmd;
    pending_cmd.target = target;
    pending_cmd.param  = param;

    if (atomic_test_and_set_bit(&pending_flags, CAL_FLAG_PENDING)) {
        return -EBUSY;
    }

    return 0;
}

/**
 * @brief 在 sensor 线程中执行待处理的标定命令。
 *
 * @retval true 执行了标定命令。
 * @retval false 当前没有待处理命令。
 */
bool cal_execute_pending(void) {
    struct cal_pending_cmd cmd;
    uint8_t status  = CAL_STATUS_OK;
    uint32_t value4 = 0U;
    uint16_t extra2 = 0U;

    if (!atomic_test_and_clear_bit(&pending_flags, CAL_FLAG_PENDING)) {
        return false;
    }

    cmd = pending_cmd;

    switch (cmd.subcmd) {
    case CAL_SUBCMD_START_L1:
        committed               = working;
        committed.l1_zero_valid = false;
        committed.l1_gain_valid = false;
        committed.l2_valid      = false;
        memset(committed.grid_err_x, 0, sizeof(committed.grid_err_x));
        memset(committed.grid_err_y, 0, sizeof(committed.grid_err_y));
        state = CAL_STATE_L1_IN_PROGRESS;
        break;

    case CAL_SUBCMD_TARE_CH:
        if (state != CAL_STATE_L1_IN_PROGRESS) {
            status = CAL_STATUS_ERR_STATE;
            break;
        }
        committed.zero[cmd.target] = cal_read_channel_median(cmd.target);
        memcpy(&value4, &committed.zero[cmd.target], sizeof(committed.zero[cmd.target]));
        break;

    case CAL_SUBCMD_GAIN_CH:
        if (state != CAL_STATE_L1_IN_PROGRESS) {
            status = CAL_STATUS_ERR_STATE;
            break;
        }
        if (cmd.param <= 0.0f) {
            status = CAL_STATUS_ERR_PARAM;
            break;
        }
        {
            int32_t median = cal_read_channel_median(cmd.target);
            int32_t delta  = median - committed.zero[cmd.target];
            if (delta == 0) {
                status = CAL_STATUS_ERR_PARAM;
                break;
            }
            committed.gain[cmd.target] = cmd.param / (float)delta;
            memcpy(&value4, &committed.gain[cmd.target], sizeof(committed.gain[cmd.target]));
        }
        break;

    case CAL_SUBCMD_COMMIT_L1:
        if (state != CAL_STATE_L1_IN_PROGRESS) {
            status = CAL_STATUS_ERR_STATE;
            break;
        }
        committed.l1_zero_valid = true;
        committed.l1_gain_valid = true;
        committed.l2_valid      = false;
        if (cal_save_all() != 0) {
            status = CAL_STATUS_ERR_NVS;
            break;
        }
        state  = CAL_STATE_L1_READY;
        value4 = cal_make_version();
        atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY);
        break;

    case CAL_SUBCMD_ABORT:
        committed = working;
        state     = working.l2_valid
                        ? CAL_STATE_L2_READY
                        : ((working.l1_zero_valid && working.l1_gain_valid) ? CAL_STATE_L1_READY : CAL_STATE_IDLE);
        break;

    case CAL_SUBCMD_START_L2:
        if (!(working.l1_zero_valid && working.l1_gain_valid)) {
            status = CAL_STATUS_ERR_STATE;
            break;
        }
        committed = working;
        memset(committed.grid_err_x, 0, sizeof(committed.grid_err_x));
        memset(committed.grid_err_y, 0, sizeof(committed.grid_err_y));
        committed.l2_valid = false;
        state              = CAL_STATE_L2_IN_PROGRESS;
        break;

    case CAL_SUBCMD_RECORD_GRID:
        if (state != CAL_STATE_L2_IN_PROGRESS) {
            status = CAL_STATUS_ERR_STATE;
            break;
        }
        {
            float measured_x;
            float measured_y;
            float target_x;
            float target_y;

            if (cal_measure_cop(&measured_x, &measured_y) != 0) {
                status = CAL_STATUS_ERR_PARAM;
                break;
            }

            cal_grid_target_to_xy(cmd.target, &target_x, &target_y);
            committed.grid_err_x[cmd.target] = measured_x - target_x;
            committed.grid_err_y[cmd.target] = measured_y - target_y;
            memcpy(&value4, &committed.grid_err_x[cmd.target], sizeof(committed.grid_err_x[cmd.target]));
            memcpy(&extra2, &committed.grid_err_y[cmd.target], sizeof(extra2));
        }
        break;

    case CAL_SUBCMD_COMMIT_L2:
        if (state != CAL_STATE_L2_IN_PROGRESS) {
            status = CAL_STATUS_ERR_STATE;
            break;
        }
        committed.l2_valid = true;
        if (cal_save_all() != 0) {
            committed.l2_valid = false;
            status             = CAL_STATUS_ERR_NVS;
            break;
        }
        state  = CAL_STATE_L2_READY;
        value4 = cal_make_version();
        atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY);
        break;

    case CAL_SUBCMD_ERASE:
        if (cal_delete_all() != 0) {
            status = CAL_STATUS_ERR_NVS;
            break;
        }
        cal_reset_runtime(&committed);
        state = CAL_STATE_IDLE;
        atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY);
        break;

    case CAL_SUBCMD_QUERY:
        value4 = cal_make_version();
        extra2 = (uint16_t)((working.l1_zero_valid ? BIT(0) : 0U) | (working.l1_gain_valid ? BIT(1) : 0U) |
                            (working.l2_valid ? BIT(2) : 0U));
        break;

    default:
        status = CAL_STATUS_ERR_PARAM;
        break;
    }

    cal_send_resp(status, cmd.subcmd, cmd.target, value4, extra2);
    return true;
}

/**
 * @brief 对计算出的 CoP 坐标施加 L2 网格补偿。
 *
 * @param[in,out] cop_x CoP X 坐标 (cm)。
 * @param[in,out] cop_y CoP Y 坐标 (cm)。
 *
 * @return 无返回值。
 */
void cal_apply_l2_correction(float *cop_x, float *cop_y) {
    float x;
    float y;
    int ix;
    int iy;
    float x0;
    float x1;
    float y0;
    float y1;
    float tx;
    float ty;
    int idx00;
    int idx10;
    int idx01;
    int idx11;
    float ex0;
    float ex1;
    float ey0;
    float ey1;
    float err_x;
    float err_y;

    if (!working.l2_valid || !cop_x || !cop_y) {
        return;
    }

    x = *cop_x;
    y = *cop_y;

    if (x < CAL_GRID_X_LEFT) x = CAL_GRID_X_LEFT;
    if (x > CAL_GRID_X_RIGHT) x = CAL_GRID_X_RIGHT;
    if (y < CAL_GRID_Y_BOTTOM) y = CAL_GRID_Y_BOTTOM;
    if (y > CAL_GRID_Y_TOP) y = CAL_GRID_Y_TOP;

    ix = (x <= CAL_GRID_X_MID) ? 0 : 1;
    iy = (y <= CAL_GRID_Y_MID) ? 0 : 1;

    x0 = (ix == 0) ? CAL_GRID_X_LEFT : CAL_GRID_X_MID;
    x1 = (ix == 0) ? CAL_GRID_X_MID : CAL_GRID_X_RIGHT;
    y0 = (iy == 0) ? CAL_GRID_Y_BOTTOM : CAL_GRID_Y_MID;
    y1 = (iy == 0) ? CAL_GRID_Y_MID : CAL_GRID_Y_TOP;

    tx = (x1 - x0) == 0.0f ? 0.0f : (x - x0) / (x1 - x0);
    ty = (y1 - y0) == 0.0f ? 0.0f : (y - y0) / (y1 - y0);

    idx00 = (2 - iy) * 3 + ix;
    idx10 = idx00 + 1;
    idx01 = (1 - iy) * 3 + ix;
    idx11 = idx01 + 1;

    ex0   = working.grid_err_x[idx00] + tx * (working.grid_err_x[idx10] - working.grid_err_x[idx00]);
    ex1   = working.grid_err_x[idx01] + tx * (working.grid_err_x[idx11] - working.grid_err_x[idx01]);
    ey0   = working.grid_err_y[idx00] + tx * (working.grid_err_y[idx10] - working.grid_err_y[idx00]);
    ey1   = working.grid_err_y[idx01] + tx * (working.grid_err_y[idx11] - working.grid_err_y[idx01]);
    err_x = ex0 + ty * (ex1 - ex0);
    err_y = ey0 + ty * (ey1 - ey0);

    *cop_x = x - err_x;
    *cop_y = y - err_y;
}