pNexus/balance-board
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

refactor(protocol): 板面几何与 CoP 计算统一为 cm 单位，force 改 int16

Browse Source 
compute_cop 直接输出 int16_t (cm/kg)，消除打包时的浮点转换；
BOARD_HALF_WIDTH/LENGTH 从 mm 改 cm，传感器坐标表同步更新。
This commit is contained in:
pNexus
2026-05-12 16:20:47 +08:00
parent f0072c3bd0
commit ffafbc4c6a
3 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/BALANCE_BOARD_PROTOCOL.md
+8 -8
View File
@@ -51,7 +51,7 @@ CoP 帧固定 11 字节，结构如下：
| `3` | `flags` | `uint8` | 见下表 | 状态标志位 | | `3` | `flags` | `uint8` | 见下表 | 状态标志位 |
| `4-5` | `cop_x` | `int16 LE` | - | 压力中心 X 坐标 (cm) | | `4-5` | `cop_x` | `int16 LE` | - | 压力中心 X 坐标 (cm) |
| `6-7` | `cop_y` | `int16 LE` | - | 压力中心 Y 坐标 (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 校验 | | `10` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 |
#### flags 位定义 #### flags 位定义
@@ -71,14 +71,14 @@ CoP 帧固定 11 字节，结构如下：
S3 (BL) -------- S2 (BR) S3 (BL) -------- S2 (BR)
``` ```
传感器坐标（单位：mm，板面中心为原点）： 传感器坐标（单位：cm，板面中心为原点）：
| 传感器 | 位置 | X | Y | | 传感器 | 位置 | X | Y |
|--------|------|---|---| |--------|------|---|---|
| S1 | 右前 (FR) | `+200` | `+200` | | S1 | 右前 (FR) | `+10` | `+10` |
| S2 | 右后 (BR) | `+200` | `-200` | | S2 | 右后 (BR) | `+10` | `-10` |
| S3 | 左后 (BL) | `-200` | `-200` | | S3 | 左后 (BL) | `-10` | `-10` |
| S4 | 左前 (FL) | `-200` | `+200` | | 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 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 字节）下传输。 - CoP 帧固定 11 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。
- 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。 - 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。
- 传感器通道到物理位置的映射：MUX `{0x01, 0x23, 0x45, 0x67}` → FL/FR/BR/BL。 - 传感器通道到物理位置的映射：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 砝码标定后修正。 - `ADC_TO_FORCE_SCALE` 当前为占位值 `1.0f`，待 10 kg 砝码标定后修正。
- ARM Cortex-M33 为小端架构，`float32` 在 packed struct 中天然符合 Little Endian 要求。 - ARM Cortex-M33 为小端架构，`float32` 在 packed struct 中天然符合 Little Endian 要求。
inc/protocol.h
+4 -4
View File
@@ -17,9 +17,9 @@
#define COP_FLAG_FORCE_VALID 0x01U /* 总力 > 阈值，CoP 坐标有意义 */ #define COP_FLAG_FORCE_VALID 0x01U /* 总力 > 阈值，CoP 坐标有意义 */
/* ─── 板面几何 & 传感器标定 ─── */ /* ─── 板面几何 & 传感器标定 ─── */
/* 传感器到板面中心的半宽、半长 (mm)。 */ /* 传感器到板面中心的半宽、半长 (cm)。 */
#define BOARD_HALF_WIDTH_MM 100.0f #define BOARD_HALF_WIDTH_CM 10.0f
#define BOARD_HALF_LENGTH_MM 100.0f #define BOARD_HALF_LENGTH_CM 10.0f
/* /*
* ADC → kg 换算推导： * ADC → kg 换算推导：
@@ -53,7 +53,7 @@ struct cop_frame_t {
uint8_t flags; /* bit0: force_valid */ uint8_t flags; /* bit0: force_valid */
int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */ int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */
int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */ int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */
uint16_t force; /* 总重量 (kg) */ int16_t force; /* 总重量 (kg) */
uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */
} __attribute__((packed)); } __attribute__((packed));
src/sensor.c
+24 -24
View File
@@ -24,16 +24,16 @@ static const uint8_t mux_channels[4] = { 0x01, 0x23, 0x45, 0x67 };
/* 传感器坐标：S0=FR, S1=BR, S2=BL, S3=FL */ /* 传感器坐标：S0=FR, S1=BR, S2=BL, S3=FL */
static const float sensor_x[4] = { static const float sensor_x[4] = {
+BOARD_HALF_WIDTH_MM, /* S0: FR */ +BOARD_HALF_WIDTH_CM, /* S0: FR */
+BOARD_HALF_WIDTH_MM, /* S1: BR */ +BOARD_HALF_WIDTH_CM, /* S1: BR */
-BOARD_HALF_WIDTH_MM, /* S2: BL */ -BOARD_HALF_WIDTH_CM, /* S2: BL */
-BOARD_HALF_WIDTH_MM, /* S3: FL */ -BOARD_HALF_WIDTH_CM, /* S3: FL */
}; };
static const float sensor_y[4] = { static const float sensor_y[4] = {
+BOARD_HALF_LENGTH_MM, /* S0: FR */ +BOARD_HALF_LENGTH_CM, /* S0: FR */
-BOARD_HALF_LENGTH_MM, /* S1: BR */ -BOARD_HALF_LENGTH_CM, /* S1: BR */
-BOARD_HALF_LENGTH_MM, /* S2: BL */ -BOARD_HALF_LENGTH_CM, /* S2: BL */
+BOARD_HALF_LENGTH_MM, /* S3: FL */ +BOARD_HALF_LENGTH_CM, /* S3: FL */
}; };
/* --- 内部状态 --- */ /* --- 内部状态 --- */
@@ -126,14 +126,14 @@ static void acquire_cycle(void) {
/** /**
* @brief 根据四路受力结果计算压力中心（CoP）和总力。 * @brief 根据四路受力结果计算压力中心（CoP）和总力。
* *
* @param[out] out_x 输出 CoP 的 X 坐标，单位由板级几何参数决定； * @param[out] out_x 输出 CoP 的 X 坐标 (cm)；
* @param[out] out_y 输出 CoP 的 Y 坐标；与 @p out_x 配套写回， * @param[out] out_y 输出 CoP 的 Y 坐标 (cm)；
* @param[out] out_force 输出总力值； * @param[out] out_force 输出总力值 (kg)；
* *
* @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。 * @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。
* @retval false 总力不足，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 forces[4];
float total = 0.0f; float total = 0.0f;
@@ -142,11 +142,11 @@ static bool compute_cop(float *out_x, float *out_y, float *out_force) {
total += forces[i]; total += forces[i];
} }
*out_force = total; *out_force = (int16_t)total;
if (total < COP_MIN_FORCE_THRESHOLD) { if (total < COP_MIN_FORCE_THRESHOLD) {
*out_x = 0.0f; *out_x = 0;
*out_y = 0.0f; *out_y = 0;
return false; 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]; wx += forces[i] * sensor_x[i];
wy += forces[i] * sensor_y[i]; wy += forces[i] * sensor_y[i];
} }
*out_x = wx / total; *out_x = (int16_t)(wx / total);
*out_y = wy / total; *out_y = (int16_t)(wy / total);
return true; return true;
} }
@@ -193,7 +193,7 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) {
int64_t t1 = k_uptime_get(); int64_t t1 = k_uptime_get();
/* CoP + 打包 */ /* CoP + 打包 */
float cop_x, cop_y, force; int16_t cop_x, cop_y, force;
bool valid = compute_cop(&cop_x, &cop_y, &force); bool valid = compute_cop(&cop_x, &cop_y, &force);
union cop_pkt_t pkt; 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.sync1 = PROTO_SYNC1;
pkt.frame.type = PROTO_TYPE_COP; pkt.frame.type = PROTO_TYPE_COP;
pkt.frame.flags = valid ? COP_FLAG_FORCE_VALID : 0; pkt.frame.flags = valid ? COP_FLAG_FORCE_VALID : 0;
pkt.frame.cop_x = (int16_t)(cop_x * 0.1f); pkt.frame.cop_x = cop_x;
pkt.frame.cop_y = (int16_t)(cop_y * 0.1f); pkt.frame.cop_y = cop_y;
pkt.frame.force = (uint16_t)force; pkt.frame.force = force;
pkt.frame.crc = crc8(pkt.bytes + 2, sizeof(pkt.bytes) - 3, 0x31, 0x00, true); 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) { if (++debug_log_divider >= 10) {
debug_log_divider = 0; debug_log_divider = 0;
LOG_INF( LOG_INF(
"FR=%.2f BR=%.2f BL=%.2f FL=%.2f | total=%.2f kg | cop=(%.1f,%.1f) cm | %s", (double)filtered[0], "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], (double)force, (double)filtered[1], (double)filtered[2], (double)filtered[3], force, cop_x, cop_y,
(double)(cop_x * 0.1f), (double)(cop_y * 0.1f), valid ? "VALID" : "low"); valid ? "VALID" : "low");
LOG_INF("timing: acq=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)frame_ms); LOG_INF("timing: acq=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)frame_ms);
} }
} }