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
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 */
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);
}
}