refactor(protocol): 板面几何与 CoP 计算统一为 cm 单位，force 改 int16
compute_cop 直接输出 int16_t (cm/kg)，消除打包时的浮点转换； BOARD_HALF_WIDTH/LENGTH 从 mm 改 cm，传感器坐标表同步更新。
This commit is contained in:
@@ -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 要求。
|
||||
|
||||
+4
-4
@@ -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));
|
||||
|
||||
|
||||
+24
-24
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user