137 lines
4.7 KiB
C
137 lines
4.7 KiB
C
#pragma once
|
||
#include <stdbool.h>
|
||
#include <stdint.h>
|
||
#include <zephyr/sys/crc.h>
|
||
|
||
/* ─── 帧常量 ─── */
|
||
/* 双帧头比单字节魔数更容易在连续字节流里完成包起始判定。 */
|
||
#define PROTO_SYNC0 0xAAU
|
||
#define PROTO_SYNC1 0x55U
|
||
#define PROTO_TYPE_COP 0x01U /* 上行: 压力中心 (CoP) 帧 */
|
||
#define PROTO_TYPE_ENCODER 0x02U /* 上行: 电机编码器帧（预留） */
|
||
#define PROTO_TYPE_RESISTANCE 0x10U /* 下行: 阻力参数 (K, B, Tau) */
|
||
#define PROTO_TYPE_SPOTTER 0x11U /* 下行: Spotter Mode 保护阈值 */
|
||
#define PROTO_TYPE_HEARTBEAT 0x20U /* 下行: 心跳包 */
|
||
|
||
/* CoP flags 位定义 */
|
||
#define COP_FLAG_FORCE_VALID 0x01U /* 总力 > 阈值，CoP 坐标有意义 */
|
||
|
||
/* ─── 板面几何 & 传感器标定 ─── */
|
||
/* 传感器到板面中心的半宽、半长 (cm)。 */
|
||
#define BOARD_HALF_WIDTH_CM 42.5f
|
||
#define BOARD_HALF_LENGTH_CM 16.0f
|
||
|
||
/*
|
||
* ADC → kg 换算推导：
|
||
* GML670 50kg: 灵敏度 1.75 mV/V, 激励 5V → 满量程输出 8.75 mV
|
||
* ADS1256: PGA=64, VREF=2.5V → 满量程 ±78.125 mV
|
||
* 50kg 对应 ADC counts = 8.75 / 78.125 × 8388607 ≈ 939524
|
||
* 1 LSB = 50.0 / 939524 ≈ 5.322e-5 kg
|
||
* 如实际激励电压或 VREF 不同，按比例修正此系数。
|
||
*/
|
||
#define SENSOR_EXCITATION_V 5.0f
|
||
#define SENSOR_SENSITIVITY_MVV 1.75f
|
||
#define SENSOR_RATED_LOAD_KG 50.0f
|
||
#define ADS1256_VREF_V 2.5f
|
||
#define ADS1256_PGA 64
|
||
|
||
#define ADC_COUNTS_AT_RATED \
|
||
((SENSOR_SENSITIVITY_MVV * SENSOR_EXCITATION_V) / (2.0f * ADS1256_VREF_V * 1000.0f / ADS1256_PGA) * 8388607.0f)
|
||
#define ADC_TO_FORCE_SCALE (SENSOR_RATED_LOAD_KG / ADC_COUNTS_AT_RATED)
|
||
|
||
/* 总重低于此值时 CoP 无意义（除零保护），单位 kg */
|
||
#define COP_MIN_FORCE_THRESHOLD 0.5f
|
||
|
||
/* ─── 上行: CoP 帧 (11 bytes, 50 Hz) ───
|
||
*
|
||
* [AA 55 01 flags cop_x(2) cop_y(2) force(2) crc]
|
||
*/
|
||
struct cop_frame_t {
|
||
uint8_t sync0; /* = PROTO_SYNC0 */
|
||
uint8_t sync1; /* = PROTO_SYNC1 */
|
||
uint8_t type; /* = PROTO_TYPE_COP */
|
||
uint8_t flags; /* bit0: force_valid */
|
||
int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */
|
||
int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */
|
||
int16_t force; /* 总重量 (kg) */
|
||
uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */
|
||
} __attribute__((packed));
|
||
|
||
union cop_pkt_t {
|
||
struct cop_frame_t frame;
|
||
uint8_t bytes[sizeof(struct cop_frame_t)];
|
||
};
|
||
|
||
/* ─── 上行: 电机编码器帧 (13 bytes, 预留) ───
|
||
*
|
||
* [AA 55 02 flags cable_length(4) velocity(4) crc]
|
||
*/
|
||
struct encoder_frame_t {
|
||
uint8_t sync0; /* = PROTO_SYNC0 */
|
||
uint8_t sync1; /* = PROTO_SYNC1 */
|
||
uint8_t type; /* = PROTO_TYPE_ENCODER */
|
||
uint8_t flags; /* 预留，填 0 */
|
||
float cable_length; /* 拉索长度 (mm) */
|
||
float velocity; /* 拉索速度 (mm/s) */
|
||
uint8_t crc; /* CRC-8/MAXIM */
|
||
} __attribute__((packed));
|
||
|
||
union encoder_pkt_t {
|
||
struct encoder_frame_t frame;
|
||
uint8_t bytes[sizeof(struct encoder_frame_t)];
|
||
};
|
||
|
||
/* ─── 下行: 阻力参数帧 (16 bytes) ───
|
||
*
|
||
* [AA 55 10 K(4) B(4) Tau(4) crc]
|
||
*/
|
||
struct resistance_frame_t {
|
||
uint8_t sync0; /* = PROTO_SYNC0 */
|
||
uint8_t sync1; /* = PROTO_SYNC1 */
|
||
uint8_t type; /* = PROTO_TYPE_RESISTANCE */
|
||
float K; /* 刚度 (N/m) */
|
||
float B; /* 阻尼 (Ns/m) */
|
||
float Tau; /* 时间常数 (s) */
|
||
uint8_t crc; /* CRC-8/MAXIM */
|
||
} __attribute__((packed));
|
||
|
||
union resistance_pkt_t {
|
||
struct resistance_frame_t frame;
|
||
uint8_t bytes[sizeof(struct resistance_frame_t)];
|
||
};
|
||
|
||
/* ─── 下行: Spotter Mode 帧 (9 bytes) ───
|
||
*
|
||
* [AA 55 11 threshold(4) enable crc]
|
||
*/
|
||
struct spotter_frame_t {
|
||
uint8_t sync0; /* = PROTO_SYNC0 */
|
||
uint8_t sync1; /* = PROTO_SYNC1 */
|
||
uint8_t type; /* = PROTO_TYPE_SPOTTER */
|
||
float threshold; /* 保护力阈值 (N) */
|
||
uint8_t enable; /* 0 = 关闭, 1 = 启用 */
|
||
uint8_t crc; /* CRC-8/MAXIM */
|
||
} __attribute__((packed));
|
||
|
||
union spotter_pkt_t {
|
||
struct spotter_frame_t frame;
|
||
uint8_t bytes[sizeof(struct spotter_frame_t)];
|
||
};
|
||
|
||
/* ─── 下行: 心跳帧 (5 bytes, ~1 Hz) ───
|
||
*
|
||
* [AA 55 20 counter crc]
|
||
*/
|
||
struct heartbeat_frame_t {
|
||
uint8_t sync0; /* = PROTO_SYNC0 */
|
||
uint8_t sync1; /* = PROTO_SYNC1 */
|
||
uint8_t type; /* = PROTO_TYPE_HEARTBEAT */
|
||
uint8_t counter; /* 滚动计数，用于活性检测 */
|
||
uint8_t crc; /* CRC-8/MAXIM */
|
||
} __attribute__((packed));
|
||
|
||
union heartbeat_pkt_t {
|
||
struct heartbeat_frame_t frame;
|
||
uint8_t bytes[sizeof(struct heartbeat_frame_t)];
|
||
};
|