diff --git a/doc/BALANCE_BOARD_PROTOCOL.md b/doc/BALANCE_BOARD_PROTOCOL.md index b7fe22c..333210e 100644 --- a/doc/BALANCE_BOARD_PROTOCOL.md +++ b/doc/BALANCE_BOARD_PROTOCOL.md @@ -51,7 +51,7 @@ CoP 帧固定 11 字节，结构如下： | `3` | `flags` | `uint8` | 见下表 | 状态标志位 | | `4-5` | `cop_x` | `int16 LE` | - | 压力中心 X 坐标 (cm) | | `6-7` | `cop_y` | `int16 LE` | - | 压力中心 Y 坐标 (cm) | -| `8-9` | `force` | `uint16 LE` | - | 总重量 (kg) | +| `8-9` | `force` | `int16 LE` | - | 总重量 (kg) | | `10` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | #### flags 位定义 @@ -71,14 +71,14 @@ CoP 帧固定 11 字节，结构如下： S3 (BL) -------- S2 (BR) ``` -传感器坐标（单位：mm，板面中心为原点）： +传感器坐标（单位：cm，板面中心为原点）： | 传感器 | 位置 | X | Y | |--------|------|---|---| -| S1 | 右前 (FR) | `+200` | `+200` | -| S2 | 右后 (BR) | `+200` | `-200` | -| S3 | 左后 (BL) | `-200` | `-200` | -| S4 | 左前 (FL) | `-200` | `+200` | +| S1 | 右前 (FR) | `+10` | `+10` | +| S2 | 右后 (BR) | `+10` | `-10` | +| S3 | 左后 (BL) | `-10` | `-10` | +| S4 | 左前 (FL) | `-10` | `+10` | 计算公式： @@ -89,7 +89,7 @@ CoP_X = (F0 * x0 + F1 * x1 + F2 * x2 + F3 * x3) / F_total CoP_Y = (F0 * y0 + F1 * y1 + F2 * y2 + F3 * y3) / F_total ``` -当 `F_total < COP_MIN_FORCE_THRESHOLD` 时，CoP 坐标无意义，`flags.bit0 = 0`，`cop_x` 和 `cop_y` 填 `0.0f`。 +当 `F_total < COP_MIN_FORCE_THRESHOLD` 时，CoP 坐标无意义，`flags.bit0 = 0`，`cop_x` 和 `cop_y` 填 `0`。 #### 示例 @@ -199,6 +199,6 @@ AA 55 20 05 XX - CoP 帧固定 11 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 - 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。 - 传感器通道到物理位置的映射：MUX `{0x01, 0x23, 0x45, 0x67}` → FL/FR/BR/BL。 -- `BOARD_HALF_WIDTH_MM = 200`，`BOARD_HALF_LENGTH_MM = 200`（后续可能调整）。 +- `BOARD_HALF_WIDTH_CM = 10`，`BOARD_HALF_LENGTH_CM = 10`（后续可能调整）。 - `ADC_TO_FORCE_SCALE` 当前为占位值 `1.0f`，待 10 kg 砝码标定后修正。 - ARM Cortex-M33 为小端架构，`float32` 在 packed struct 中天然符合 Little Endian 要求。 diff --git a/inc/protocol.h b/inc/protocol.h index 79e7dd9..8496b6f 100644 --- a/inc/protocol.h +++ b/inc/protocol.h @@ -17,9 +17,9 @@ #define COP_FLAG_FORCE_VALID 0x01U /* 总力 > 阈值，CoP 坐标有意义 */ /* ─── 板面几何 & 传感器标定 ─── */ -/* 传感器到板面中心的半宽、半长 (mm)。 */ -#define BOARD_HALF_WIDTH_MM 100.0f -#define BOARD_HALF_LENGTH_MM 100.0f +/* 传感器到板面中心的半宽、半长 (cm)。 */ +#define BOARD_HALF_WIDTH_CM 10.0f +#define BOARD_HALF_LENGTH_CM 10.0f /* * ADC → kg 换算推导： @@ -53,7 +53,7 @@ struct cop_frame_t { uint8_t flags; /* bit0: force_valid */ int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */ int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */ - uint16_t force; /* 总重量 (kg) */ + int16_t force; /* 总重量 (kg) */ uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ } __attribute__((packed)); diff --git a/src/sensor.c b/src/sensor.c index 4dce60e..e2b1fe8 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -24,16 +24,16 @@ static const uint8_t mux_channels[4] = { 0x01, 0x23, 0x45, 0x67 }; /* 传感器坐标：S0=FR, S1=BR, S2=BL, S3=FL */ static const float sensor_x[4] = { - +BOARD_HALF_WIDTH_MM, /* S0: FR */ - +BOARD_HALF_WIDTH_MM, /* S1: BR */ - -BOARD_HALF_WIDTH_MM, /* S2: BL */ - -BOARD_HALF_WIDTH_MM, /* S3: FL */ + +BOARD_HALF_WIDTH_CM, /* S0: FR */ + +BOARD_HALF_WIDTH_CM, /* S1: BR */ + -BOARD_HALF_WIDTH_CM, /* S2: BL */ + -BOARD_HALF_WIDTH_CM, /* S3: FL */ }; static const float sensor_y[4] = { - +BOARD_HALF_LENGTH_MM, /* S0: FR */ - -BOARD_HALF_LENGTH_MM, /* S1: BR */ - -BOARD_HALF_LENGTH_MM, /* S2: BL */ - +BOARD_HALF_LENGTH_MM, /* S3: FL */ + +BOARD_HALF_LENGTH_CM, /* S0: FR */ + -BOARD_HALF_LENGTH_CM, /* S1: BR */ + -BOARD_HALF_LENGTH_CM, /* S2: BL */ + +BOARD_HALF_LENGTH_CM, /* S3: FL */ }; /* --- 内部状态 --- */ @@ -126,14 +126,14 @@ static void acquire_cycle(void) { /** * @brief 根据四路受力结果计算压力中心（CoP）和总力。 * - * @param[out] out_x 输出 CoP 的 X 坐标，单位由板级几何参数决定； - * @param[out] out_y 输出 CoP 的 Y 坐标；与 @p out_x 配套写回， - * @param[out] out_force 输出总力值； + * @param[out] out_x 输出 CoP 的 X 坐标 (cm)； + * @param[out] out_y 输出 CoP 的 Y 坐标 (cm)； + * @param[out] out_force 输出总力值 (kg)； * * @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。 * @retval false 总力不足，CoP 被强制归零以避免在几乎无载荷时放大数值噪声。 */ -static bool compute_cop(float *out_x, float *out_y, float *out_force) { +static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force) { float forces[4]; float total = 0.0f; @@ -142,11 +142,11 @@ static bool compute_cop(float *out_x, float *out_y, float *out_force) { total += forces[i]; } - *out_force = total; + *out_force = (int16_t)total; if (total < COP_MIN_FORCE_THRESHOLD) { - *out_x = 0.0f; - *out_y = 0.0f; + *out_x = 0; + *out_y = 0; return false; } @@ -155,8 +155,8 @@ static bool compute_cop(float *out_x, float *out_y, float *out_force) { wx += forces[i] * sensor_x[i]; wy += forces[i] * sensor_y[i]; } - *out_x = wx / total; - *out_y = wy / total; + *out_x = (int16_t)(wx / total); + *out_y = (int16_t)(wy / total); return true; } @@ -193,7 +193,7 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { int64_t t1 = k_uptime_get(); /* CoP + 打包 */ - float cop_x, cop_y, force; + int16_t cop_x, cop_y, force; bool valid = compute_cop(&cop_x, &cop_y, &force); union cop_pkt_t pkt; @@ -201,9 +201,9 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { pkt.frame.sync1 = PROTO_SYNC1; pkt.frame.type = PROTO_TYPE_COP; pkt.frame.flags = valid ? COP_FLAG_FORCE_VALID : 0; - pkt.frame.cop_x = (int16_t)(cop_x * 0.1f); - pkt.frame.cop_y = (int16_t)(cop_y * 0.1f); - pkt.frame.force = (uint16_t)force; + pkt.frame.cop_x = cop_x; + pkt.frame.cop_y = cop_y; + pkt.frame.force = force; pkt.frame.crc = crc8(pkt.bytes + 2, sizeof(pkt.bytes) - 3, 0x31, 0x00, true); /* 发送 */ @@ -213,9 +213,9 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { if (++debug_log_divider >= 10) { debug_log_divider = 0; LOG_INF( - "FR=%.2f BR=%.2f BL=%.2f FL=%.2f | total=%.2f kg | cop=(%.1f,%.1f) cm | %s", (double)filtered[0], - (double)filtered[1], (double)filtered[2], (double)filtered[3], (double)force, - (double)(cop_x * 0.1f), (double)(cop_y * 0.1f), valid ? "VALID" : "low"); + "FR=%.2f\t BR=%.2f\t BL=%.2f\t FL=%.2f\t | total=%d kg | cop=(%d,%d) cm | %s", (double)filtered[0], + (double)filtered[1], (double)filtered[2], (double)filtered[3], force, cop_x, cop_y, + valid ? "VALID" : "low"); LOG_INF("timing: acq=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)frame_ms); } }