Compare commits
1 Commits
master
..
1a8b4471e7
| Author | SHA1 | Date | |
|---|---|---|---|
| 1a8b4471e7 |
-320
@@ -1,320 +0,0 @@
|
||||
---
|
||||
Language: Cpp
|
||||
AlignAfterOpenBracket: true
|
||||
AccessModifierOffset: -2
|
||||
AlignArrayOfStructures: None
|
||||
AlignConsecutiveAssignments:
|
||||
Enabled: true
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionDeclarations: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: true
|
||||
AlignConsecutiveBitFields:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionDeclarations: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignConsecutiveDeclarations:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionDeclarations: true
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignConsecutiveMacros:
|
||||
Enabled: true
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionDeclarations: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignConsecutiveShortCaseStatements:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCaseArrows: false
|
||||
AlignCaseColons: false
|
||||
AlignConsecutiveTableGenBreakingDAGArgColons:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionDeclarations: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignConsecutiveTableGenCondOperatorColons:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionDeclarations: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignConsecutiveTableGenDefinitionColons:
|
||||
Enabled: false
|
||||
AcrossEmptyLines: false
|
||||
AcrossComments: false
|
||||
AlignCompound: false
|
||||
AlignFunctionDeclarations: false
|
||||
AlignFunctionPointers: false
|
||||
PadOperators: false
|
||||
AlignEscapedNewlines: Right
|
||||
AlignOperands: Align
|
||||
AlignTrailingComments:
|
||||
AlignPPAndNotPP: true
|
||||
Kind: Always
|
||||
OverEmptyLines: 0
|
||||
AllowAllArgumentsOnNextLine: true
|
||||
AllowAllParametersOfDeclarationOnNextLine: false
|
||||
AllowBreakBeforeNoexceptSpecifier: Never
|
||||
AllowBreakBeforeQtProperty: false
|
||||
AllowShortBlocksOnASingleLine: Never
|
||||
AllowShortCaseExpressionOnASingleLine: true
|
||||
AllowShortCaseLabelsOnASingleLine: false
|
||||
AllowShortCompoundRequirementOnASingleLine: true
|
||||
AllowShortEnumsOnASingleLine: false
|
||||
AllowShortFunctionsOnASingleLine: None
|
||||
AllowShortIfStatementsOnASingleLine: WithoutElse
|
||||
AllowShortLambdasOnASingleLine: All
|
||||
AllowShortLoopsOnASingleLine: false
|
||||
AllowShortNamespacesOnASingleLine: false
|
||||
AlwaysBreakAfterDefinitionReturnType: None
|
||||
AlwaysBreakBeforeMultilineStrings: false
|
||||
AttributeMacros:
|
||||
- __capability
|
||||
BinPackArguments: true
|
||||
BinPackLongBracedList: true
|
||||
BinPackParameters: OnePerLine
|
||||
BitFieldColonSpacing: Both
|
||||
BracedInitializerIndentWidth: -1
|
||||
BraceWrapping:
|
||||
AfterCaseLabel: false
|
||||
AfterClass: true
|
||||
AfterControlStatement: Never
|
||||
AfterEnum: false
|
||||
AfterExternBlock: true
|
||||
AfterFunction: false
|
||||
AfterNamespace: true
|
||||
AfterObjCDeclaration: true
|
||||
AfterStruct: false
|
||||
AfterUnion: false
|
||||
BeforeCatch: true
|
||||
BeforeElse: false
|
||||
BeforeLambdaBody: false
|
||||
BeforeWhile: false
|
||||
IndentBraces: false
|
||||
SplitEmptyFunction: true
|
||||
SplitEmptyRecord: true
|
||||
SplitEmptyNamespace: true
|
||||
BreakAdjacentStringLiterals: true
|
||||
BreakAfterAttributes: Leave
|
||||
BreakAfterJavaFieldAnnotations: false
|
||||
BreakAfterOpenBracketBracedList: false
|
||||
BreakAfterOpenBracketFunction: true
|
||||
BreakAfterOpenBracketIf: false
|
||||
BreakAfterOpenBracketLoop: false
|
||||
BreakAfterOpenBracketSwitch: false
|
||||
BreakAfterReturnType: None
|
||||
BreakArrays: true
|
||||
BreakBeforeBinaryOperators: None
|
||||
BreakBeforeCloseBracketBracedList: false
|
||||
BreakBeforeCloseBracketFunction: false
|
||||
BreakBeforeCloseBracketIf: false
|
||||
BreakBeforeCloseBracketLoop: false
|
||||
BreakBeforeCloseBracketSwitch: false
|
||||
BreakBeforeConceptDeclarations: Always
|
||||
BreakBeforeBraces: Custom
|
||||
BreakBeforeInlineASMColon: OnlyMultiline
|
||||
BreakBeforeTemplateCloser: false
|
||||
BreakBeforeTernaryOperators: true
|
||||
BreakBinaryOperations: Never
|
||||
BreakConstructorInitializers: BeforeColon
|
||||
BreakFunctionDefinitionParameters: false
|
||||
BreakInheritanceList: BeforeColon
|
||||
BreakStringLiterals: true
|
||||
BreakTemplateDeclarations: MultiLine
|
||||
ColumnLimit: 120
|
||||
CommentPragmas: '^ IWYU pragma:'
|
||||
CompactNamespaces: false
|
||||
ConstructorInitializerIndentWidth: 4
|
||||
ContinuationIndentWidth: 4
|
||||
Cpp11BracedListStyle: Block
|
||||
DerivePointerAlignment: false
|
||||
DisableFormat: false
|
||||
EmptyLineAfterAccessModifier: Never
|
||||
EmptyLineBeforeAccessModifier: LogicalBlock
|
||||
EnumTrailingComma: Leave
|
||||
ExperimentalAutoDetectBinPacking: false
|
||||
FixNamespaceComments: true
|
||||
ForEachMacros:
|
||||
- foreach
|
||||
- Q_FOREACH
|
||||
- BOOST_FOREACH
|
||||
IfMacros:
|
||||
- KJ_IF_MAYBE
|
||||
IncludeBlocks: Preserve
|
||||
IncludeCategories:
|
||||
- Regex: '^"(llvm|llvm-c|clang|clang-c)/'
|
||||
Priority: 2
|
||||
SortPriority: 0
|
||||
CaseSensitive: false
|
||||
- Regex: '^(<|"(gtest|gmock|isl|json)/)'
|
||||
Priority: 3
|
||||
SortPriority: 0
|
||||
CaseSensitive: false
|
||||
- Regex: '.*'
|
||||
Priority: 1
|
||||
SortPriority: 0
|
||||
CaseSensitive: false
|
||||
IncludeIsMainRegex: '(Test)?$'
|
||||
IncludeIsMainSourceRegex: ''
|
||||
IndentAccessModifiers: false
|
||||
IndentCaseBlocks: false
|
||||
IndentCaseLabels: false
|
||||
IndentExportBlock: true
|
||||
IndentExternBlock: AfterExternBlock
|
||||
IndentGotoLabels: true
|
||||
IndentPPDirectives: None
|
||||
IndentRequiresClause: true
|
||||
IndentWidth: 4
|
||||
IndentWrappedFunctionNames: false
|
||||
InsertBraces: false
|
||||
InsertNewlineAtEOF: false
|
||||
InsertTrailingCommas: None
|
||||
IntegerLiteralSeparator:
|
||||
Binary: 0
|
||||
BinaryMinDigitsInsert: 0
|
||||
BinaryMaxDigitsRemove: 0
|
||||
Decimal: 0
|
||||
DecimalMinDigitsInsert: 0
|
||||
DecimalMaxDigitsRemove: 0
|
||||
Hex: 0
|
||||
HexMinDigitsInsert: 0
|
||||
HexMaxDigitsRemove: 0
|
||||
BinaryMinDigits: 0
|
||||
DecimalMinDigits: 0
|
||||
HexMinDigits: 0
|
||||
JavaScriptQuotes: Leave
|
||||
JavaScriptWrapImports: true
|
||||
KeepEmptyLines:
|
||||
AtEndOfFile: false
|
||||
AtStartOfBlock: true
|
||||
AtStartOfFile: true
|
||||
KeepFormFeed: false
|
||||
LambdaBodyIndentation: Signature
|
||||
LineEnding: DeriveLF
|
||||
MacroBlockBegin: ''
|
||||
MacroBlockEnd: ''
|
||||
MainIncludeChar: Quote
|
||||
MaxEmptyLinesToKeep: 1
|
||||
NamespaceIndentation: None
|
||||
NumericLiteralCase:
|
||||
ExponentLetter: Leave
|
||||
HexDigit: Leave
|
||||
Prefix: Leave
|
||||
Suffix: Leave
|
||||
ObjCBinPackProtocolList: Auto
|
||||
ObjCBlockIndentWidth: 2
|
||||
ObjCBreakBeforeNestedBlockParam: true
|
||||
ObjCSpaceAfterProperty: false
|
||||
ObjCSpaceBeforeProtocolList: true
|
||||
OneLineFormatOffRegex: ''
|
||||
PackConstructorInitializers: BinPack
|
||||
PenaltyBreakAssignment: 2
|
||||
PenaltyBreakBeforeFirstCallParameter: 19
|
||||
PenaltyBreakBeforeMemberAccess: 150
|
||||
PenaltyBreakComment: 300
|
||||
PenaltyBreakFirstLessLess: 120
|
||||
PenaltyBreakOpenParenthesis: 0
|
||||
PenaltyBreakScopeResolution: 500
|
||||
PenaltyBreakString: 1000
|
||||
PenaltyBreakTemplateDeclaration: 10
|
||||
PenaltyExcessCharacter: 1000000
|
||||
PenaltyIndentedWhitespace: 0
|
||||
PenaltyReturnTypeOnItsOwnLine: 1000
|
||||
PointerAlignment: Right
|
||||
PPIndentWidth: -1
|
||||
QualifierAlignment: Leave
|
||||
ReferenceAlignment: Pointer
|
||||
ReflowComments: Always
|
||||
RemoveBracesLLVM: false
|
||||
RemoveEmptyLinesInUnwrappedLines: false
|
||||
RemoveParentheses: Leave
|
||||
RemoveSemicolon: false
|
||||
RequiresClausePosition: OwnLine
|
||||
RequiresExpressionIndentation: OuterScope
|
||||
SeparateDefinitionBlocks: Leave
|
||||
ShortNamespaceLines: 1
|
||||
SkipMacroDefinitionBody: false
|
||||
SortIncludes:
|
||||
Enabled: true
|
||||
IgnoreCase: false
|
||||
IgnoreExtension: false
|
||||
SortJavaStaticImport: Before
|
||||
SortUsingDeclarations: LexicographicNumeric
|
||||
SpaceAfterCStyleCast: false
|
||||
SpaceAfterLogicalNot: false
|
||||
SpaceAfterOperatorKeyword: false
|
||||
SpaceAfterTemplateKeyword: true
|
||||
SpaceAroundPointerQualifiers: Default
|
||||
SpaceBeforeAssignmentOperators: true
|
||||
SpaceBeforeCaseColon: false
|
||||
SpaceBeforeCpp11BracedList: false
|
||||
SpaceBeforeCtorInitializerColon: true
|
||||
SpaceBeforeInheritanceColon: true
|
||||
SpaceBeforeJsonColon: false
|
||||
SpaceBeforeParens: ControlStatements
|
||||
SpaceBeforeParensOptions:
|
||||
AfterControlStatements: true
|
||||
AfterForeachMacros: true
|
||||
AfterFunctionDefinitionName: false
|
||||
AfterFunctionDeclarationName: false
|
||||
AfterIfMacros: true
|
||||
AfterNot: false
|
||||
AfterOverloadedOperator: false
|
||||
AfterPlacementOperator: true
|
||||
AfterRequiresInClause: false
|
||||
AfterRequiresInExpression: false
|
||||
BeforeNonEmptyParentheses: false
|
||||
SpaceBeforeRangeBasedForLoopColon: true
|
||||
SpaceBeforeSquareBrackets: false
|
||||
SpaceInEmptyBraces: Never
|
||||
SpacesBeforeTrailingComments: 1
|
||||
SpacesInAngles: Never
|
||||
SpacesInContainerLiterals: true
|
||||
SpacesInLineCommentPrefix:
|
||||
Minimum: 1
|
||||
Maximum: -1
|
||||
SpacesInParens: Never
|
||||
SpacesInParensOptions:
|
||||
ExceptDoubleParentheses: false
|
||||
InCStyleCasts: false
|
||||
InConditionalStatements: false
|
||||
InEmptyParentheses: false
|
||||
Other: false
|
||||
SpacesInSquareBrackets: false
|
||||
Standard: Latest
|
||||
StatementAttributeLikeMacros:
|
||||
- Q_EMIT
|
||||
StatementMacros:
|
||||
- Q_UNUSED
|
||||
- QT_REQUIRE_VERSION
|
||||
TableGenBreakInsideDAGArg: DontBreak
|
||||
TabWidth: 4
|
||||
UseTab: Never
|
||||
VerilogBreakBetweenInstancePorts: true
|
||||
WhitespaceSensitiveMacros:
|
||||
- BOOST_PP_STRINGIZE
|
||||
- CF_SWIFT_NAME
|
||||
- NS_SWIFT_NAME
|
||||
- PP_STRINGIZE
|
||||
- STRINGIZE
|
||||
WrapNamespaceBodyWithEmptyLines: Leave
|
||||
...
|
||||
-38
@@ -1,38 +0,0 @@
|
||||
# =========================================================
|
||||
# Editor swap / backup
|
||||
# =========================================================
|
||||
*.swp
|
||||
*~
|
||||
|
||||
# =========================================================
|
||||
# IDE 本地配置
|
||||
# =========================================================
|
||||
.idea/
|
||||
*.iml
|
||||
.vscode/
|
||||
.codex
|
||||
.claude
|
||||
|
||||
# =========================================================
|
||||
# Embedded / CMake / Zephyr 构建产物
|
||||
# =========================================================
|
||||
/build*/
|
||||
build/
|
||||
**/build/
|
||||
**/zephyr/
|
||||
**/output/
|
||||
*.o
|
||||
*.obj
|
||||
*.elf
|
||||
*.bin
|
||||
*.hex
|
||||
*.map
|
||||
*.ninja
|
||||
.ninja_deps
|
||||
.ninja_log
|
||||
|
||||
# =========================================================
|
||||
# OS 临时文件
|
||||
# =========================================================
|
||||
.DS_Store
|
||||
Thumbs.db
|
||||
@@ -1,31 +0,0 @@
|
||||
#
|
||||
# Copyright (c) 2018 Nordic Semiconductor
|
||||
#
|
||||
# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
|
||||
#
|
||||
cmake_minimum_required(VERSION 3.20.0)
|
||||
|
||||
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
||||
project(GML670_V2)
|
||||
|
||||
if(SYSBUILD)
|
||||
get_filename_component(SYSBUILD_TOP_BINARY_DIR "${CMAKE_BINARY_DIR}" DIRECTORY)
|
||||
add_custom_target(sync_compile_commands ALL
|
||||
COMMAND ${CMAKE_COMMAND} -E copy_if_different
|
||||
${CMAKE_BINARY_DIR}/compile_commands.json
|
||||
${SYSBUILD_TOP_BINARY_DIR}/compile_commands.json
|
||||
COMMENT "Sync ${PROJECT_NAME} compile_commands.json to sysbuild top-level build/"
|
||||
VERBATIM
|
||||
)
|
||||
endif()
|
||||
|
||||
# NORDIC SDK APP START
|
||||
file(GLOB_RECURSE app_sources src/*.c)
|
||||
target_sources(app PRIVATE
|
||||
${app_sources}
|
||||
)
|
||||
|
||||
target_include_directories(app PRIVATE
|
||||
inc
|
||||
)
|
||||
# NORDIC SDK APP END
|
||||
@@ -1,45 +0,0 @@
|
||||
#
|
||||
# Copyright (c) 2018 Nordic Semiconductor
|
||||
#
|
||||
# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
|
||||
#
|
||||
|
||||
source "Kconfig.zephyr"
|
||||
|
||||
menu "Nordic UART BLE GATT service sample"
|
||||
|
||||
config BT_NUS_THREAD_STACK_SIZE
|
||||
int "Thread stack size"
|
||||
default 1024
|
||||
help
|
||||
Stack size used in each of the two threads
|
||||
|
||||
config BT_NUS_UART_BUFFER_SIZE
|
||||
int "UART payload buffer element size"
|
||||
default 40
|
||||
help
|
||||
Size of the payload buffer in each RX and TX FIFO element
|
||||
|
||||
config BT_NUS_SECURITY_ENABLED
|
||||
bool "Security"
|
||||
default y
|
||||
select BT_SMP
|
||||
help
|
||||
Enable Bluetooth LE security for the UART service
|
||||
|
||||
config BT_NUS_UART_RX_WAIT_TIME
|
||||
int "Timeout for UART RX complete event"
|
||||
default 50000
|
||||
help
|
||||
Wait for RX complete event time in microseconds
|
||||
|
||||
config SETTINGS
|
||||
default y
|
||||
|
||||
config ZMS
|
||||
default y if (SOC_FLASH_NRF_RRAM || SOC_FLASH_NRF_MRAM)
|
||||
|
||||
config NVS
|
||||
default y if !(SOC_FLASH_NRF_RRAM || SOC_FLASH_NRF_MRAM)
|
||||
|
||||
endmenu
|
||||
@@ -1,377 +1,2 @@
|
||||
# GML670 Balance Board
|
||||
# balance-board
|
||||
|
||||
> 当前目录是 `GML670x4_V2`。代码内的 CMake 项目名是 `GML670_V2`,BLE 设备名是 `GML670_System`。
|
||||
>
|
||||
> 这份 README 只描述当前 `V2` 工程已经落地的实现。更细的协议字段和示例帧见 [BALANCE_BOARD_PROTOCOL.md](./doc/BALANCE_BOARD_PROTOCOL.md)。
|
||||
|
||||
---
|
||||
|
||||
## 目录
|
||||
|
||||
1. [产品概述](#1-产品概述)
|
||||
2. [硬件组成](#2-硬件组成)
|
||||
3. [系统架构](#3-系统架构)
|
||||
4. [功能需求与当前状态](#4-功能需求与当前状态)
|
||||
5. [BLE 协议规格](#5-ble-协议规格)
|
||||
6. [ADS1256 / CoP 规格](#6-ads1256--cop-规格)
|
||||
7. [开发环境与板级配置](#7-开发环境与板级配置)
|
||||
8. [项目结构](#8-项目结构)
|
||||
9. [构建与烧录](#9-构建与烧录)
|
||||
10. [日志与当前约束](#10-日志与当前约束)
|
||||
|
||||
---
|
||||
|
||||
## 1. 产品概述
|
||||
|
||||
本工程是平衡板的 nRF5340 App Core 固件,当前版本的主链路很简单:
|
||||
|
||||
- 上电后等待 2 秒,初始化 BLE NUS 传输层并启动广播
|
||||
- 初始化 ADS1256,完成一次 tare 去皮
|
||||
- 创建独立的传感器线程,循环读取 4 路差分输入
|
||||
- 在板端完成重量换算和 CoP(Center of Pressure,压力中心)解算
|
||||
- 通过 BLE NUS 持续上报 17 字节 CoP 二进制帧
|
||||
- 接收基站下发的阻力参数、Spotter 参数和心跳帧,并保存在内存里
|
||||
|
||||
当前实现已经把采样、解算和上行链路拆成了独立模块:`main.c` 只负责启动,BLE 在 `src/ble_transport.c`,ADS1256 在 `src/ads1256.c`,采样与 CoP 在 `src/sensor.c`。
|
||||
|
||||
## 2. 硬件组成
|
||||
|
||||
当前仓库里能直接确认的硬件如下:
|
||||
|
||||
| 器件 | 型号 / 对象 | 接口 | 作用 | 状态 |
|
||||
|------|-------------|------|------|------|
|
||||
| 主控 | nRF5340 DK App Core | - | 运行 BLE、SPI 采样和 CoP 解算 | ✅ |
|
||||
| ADC | ADS1256 | SPI1 | 4 路多通道采样 | ✅ |
|
||||
| 传感器输入 | 4 路差分输入 | ADS1256 MUX | 用于四角重量采样 | ✅ |
|
||||
| 基站链路 | Nordic UART Service | BLE | 上报 CoP、接收控制参数 | ✅ |
|
||||
| 编码器链路 | 预留 | BLE 协议 | 编码器上行帧已定义,当前固件不发送 | 🔲 |
|
||||
|
||||
### ADS1256 相关引脚
|
||||
|
||||
`app.overlay` 当前把 ADS1256 接到 `spi1`,连线如下:
|
||||
|
||||
| 信号 | nRF5340 引脚 | 说明 |
|
||||
|------|--------------|------|
|
||||
| SCK | `P1.15` | `spi1` 时钟 |
|
||||
| MOSI | `P1.13` | `spi1` 主发从收 |
|
||||
| MISO | `P1.14` | `spi1` 主收从发 |
|
||||
| CS | `P1.12` | 片选 |
|
||||
| DRDY | `P1.11` | 数据就绪输入 |
|
||||
| RESET | `P1.10` | ADS1256 复位控制 |
|
||||
|
||||
虽然 `app.overlay` 里有 `ads1256@0` 节点,但当前实现没有走单独的 Zephyr ADS1256 驱动,实际访问路径还是 `src/ads1256.c` 里直接操作 `spi1` 和 GPIO alias。
|
||||
|
||||
## 3. 系统架构
|
||||
|
||||
```text
|
||||
4 路差分传感器
|
||||
|
|
||||
v
|
||||
ADS1256 + SPI1
|
||||
|
|
||||
v
|
||||
src/ads1256.c
|
||||
|
|
||||
v
|
||||
src/sensor.c (业务层)
|
||||
|
|
||||
+--> 去皮 / 17 次均值 / 死区抑制 / 重量换算 / CoP 解算
|
||||
|
|
||||
v
|
||||
src/comm_protocol.c (协议层)
|
||||
|
|
||||
+--> 上行:组包 CoP/标定响应帧,调用 ble_transport_send()
|
||||
+--> 下行:解析收到的帧,推入 msgq 供业务层消费
|
||||
|
|
||||
v
|
||||
src/ble_transport.c (传输层)
|
||||
|
|
||||
+--> BLE NUS Notify / Write,纯字节收发
|
||||
```
|
||||
|
||||
`src/main.c` 本身不承载采样逻辑,它只按顺序调用 `ble_transport_init()`、`comm_protocol_init()`、`ble_transport_adv_start()` 和 `sensor_init()`。
|
||||
|
||||
## 4. 功能需求与当前状态
|
||||
|
||||
### 4.1 采样与解算
|
||||
|
||||
| 项目 | 说明 | 状态 |
|
||||
|------|------|------|
|
||||
| 4 路轮询采样 | 通过 ADS1256 MUX 依次读取 `0x01 / 0x23 / 0x45 / 0x67` 四组差分通道 | ✅ |
|
||||
| tare 去皮 | 每路读取 17 个样本,排序后取中位数作为零点偏移 | ✅ |
|
||||
| 运行期滤波 | 每帧每通道采 17 次并取均值 | ✅ |
|
||||
| 死区抑制 | 去皮后的均值落在 `-250 ~ 250` counts 时压到 `0` | ✅ |
|
||||
| 重量换算 | `ADC_TO_FORCE_SCALE` 已按 GML670 50 kg 传感器参数推导 | ✅ |
|
||||
| CoP 解算 | 根据四角重量计算 `cop_x / cop_y / force` | ✅ |
|
||||
| 有效力阈值 | 总重量低于 `0.5 kg` 时,CoP 坐标置零并清除 `force_valid` | ✅ |
|
||||
|
||||
### 4.2 BLE 通信
|
||||
|
||||
| 项目 | 说明 | 状态 |
|
||||
|------|------|------|
|
||||
| BLE 角色 | `Peripheral`,使用 NUS 私有服务 | ✅ |
|
||||
| 广播内容 | 广播包带 NUS UUID,扫描响应带设备名 | ✅ |
|
||||
| 连接参数请求 | 连接后请求 `7.5-15 ms` 区间 | ✅ |
|
||||
| CoP 上行 | 固定 17 字节二进制帧,目标频率约 `50 Hz` | ✅ |
|
||||
| NUS 发送保护 | 仅在连接存在且通知已使能时发送,失败时短间隔重试 3 次 | ✅ |
|
||||
| 断线重广播 | 断开 1 秒后重新广播 | ✅ |
|
||||
| NUS 下行解析 | Resistance / Spotter / Heartbeat 三类帧已接收解析 | ✅ |
|
||||
| 编码器上行 | 协议已预留,主循环未发送 | 🔲 |
|
||||
|
||||
### 4.3 控制参数与保护逻辑
|
||||
|
||||
| 项目 | 说明 | 状态 |
|
||||
|------|------|------|
|
||||
| Resistance 参数接收 | 接收 `K / B / Tau` 并保存到传输层静态变量 | ✅ |
|
||||
| Spotter 参数接收 | 接收 `threshold / enable` 并保存到传输层静态变量 | ✅ |
|
||||
| Heartbeat 接收 | 接收后刷新最近心跳时间戳 | ✅ |
|
||||
| 阻力闭环控制 | 参数已接入协议层,控制逻辑未落地 | 🔲 |
|
||||
| Spotter 保护动作 | 参数已接入协议层,保护动作未落地 | 🔲 |
|
||||
|
||||
### 4.4 传感器标定
|
||||
|
||||
标定分两级,通过 BLE 下行标定命令帧触发,平衡板执行后回复标定响应帧。帧格式详见 [BALANCE_BOARD_PROTOCOL.md](./doc/BALANCE_BOARD_PROTOCOL.md)。
|
||||
|
||||
#### L1 标定(单通道零点 + 增益)
|
||||
|
||||
```text
|
||||
基站 平衡板
|
||||
│ │
|
||||
│── START_L1 ─────────────────>│ 进入 L1 模式
|
||||
│<──────────────── OK ─────────│
|
||||
│ │
|
||||
│── TARE_CH (ch=0) ──────────>│ 空载,记录零点
|
||||
│<──────────────── OK ─────────│
|
||||
│ ...对 ch=1,2,3 重复... │
|
||||
│ │
|
||||
│── GAIN_CH (ch=0, 10kg) ────>│ 加载砝码,计算增益
|
||||
│<──────────────── OK ─────────│
|
||||
│ ...对 ch=1,2,3 重复... │
|
||||
│ │
|
||||
│── COMMIT_L1 ────────────────>│ 写入 NVS
|
||||
│<──────────────── OK ─────────│
|
||||
```
|
||||
|
||||
#### L2 标定(9 点网格空间修正)
|
||||
|
||||
L2 需要 L1 已完成。在 9 个已知位置分别放置标定负载,记录 CoP 误差用于空间修正。
|
||||
|
||||
```text
|
||||
基站 平衡板
|
||||
│ │
|
||||
│── START_L2 ─────────────────>│ 进入 L2 模式
|
||||
│<──────────────── OK ─────────│
|
||||
│ │
|
||||
│── RECORD_GRID (pt=0) ──────>│ 在网格点 0 放置已知负载
|
||||
│<──────────────── OK ─────────│
|
||||
│ ...对 pt=1..8 重复... │
|
||||
│ │
|
||||
│── COMMIT_L2 ────────────────>│ 写入 NVS
|
||||
│<──────────────── OK ─────────│
|
||||
```
|
||||
|
||||
网格点编号(3×3,俯视):
|
||||
|
||||
```text
|
||||
6 ── 7 ── 8
|
||||
│ │
|
||||
3 ── 4 ── 5
|
||||
│ │
|
||||
0 ── 1 ── 2
|
||||
```
|
||||
|
||||
点 4 为板面中心。
|
||||
|
||||
#### 状态与持久化
|
||||
|
||||
- 标定数据通过 NVS 持久化,上电后自动加载
|
||||
- `ERASE` 命令可清除全部标定数据,回退到出厂默认
|
||||
- `QUERY` 命令返回当前各级标定的有效性标志
|
||||
- `ABORT` 可在任意阶段中止标定流程,已测量数据不写入 NVS
|
||||
|
||||
## 5. BLE 协议规格
|
||||
|
||||
平衡板工作在 BLE `Peripheral` 角色,通过 Nordic UART Service 与基站交换二进制帧。
|
||||
|
||||
- 上行方向:平衡板 -> 基站,使用 NUS `Notify`
|
||||
- 下行方向:基站 -> 平衡板,使用 NUS `Write`
|
||||
- 广播内容:广播包带 NUS UUID,扫描响应带设备名 `GML670_System`
|
||||
|
||||
### 5.1 NUS UUID
|
||||
|
||||
| 项目 | UUID | 方向 | 说明 |
|
||||
|------|------|------|------|
|
||||
| Service | `6E400001-B5A3-F393-E0A9-E50E24DCCA9E` | - | NUS 主服务 |
|
||||
| TX Characteristic | `6E400003-B5A3-F393-E0A9-E50E24DCCA9E` | 平衡板 -> 基站 | Notify,上报 CoP 数据 |
|
||||
| RX Characteristic | `6E400002-B5A3-F393-E0A9-E50E24DCCA9E` | 基站 -> 平衡板 | Write,下发控制参数 |
|
||||
|
||||
### 5.2 连接参数
|
||||
|
||||
| 参数 | 当前值 | 说明 |
|
||||
|------|--------|------|
|
||||
| 广播模式 | `BT_LE_ADV_CONN_FAST_2` | 上电和断线后走同一入口启动 |
|
||||
| 连接间隔请求 | `7.5-15 ms` | `interval_min = 6`,`interval_max = 12` |
|
||||
| 从站延迟 | `0` | 不跳过连接事件 |
|
||||
| 监督超时 | `4000 ms` | `timeout = 400` |
|
||||
|
||||
### 5.3 帧类型总览
|
||||
|
||||
| 类型名 | 值 | 方向 | 长度 | 频率 | 说明 |
|
||||
|--------|----|------|------|------|------|
|
||||
| `PROTO_TYPE_COP` | `0x01` | 平衡板 -> 基站 | `17` | 约 `50 Hz` | CoP 上行帧 |
|
||||
| `PROTO_TYPE_ENCODER` | `0x02` | 平衡板 -> 基站 | `13` | TBD | 编码器上行帧,预留 |
|
||||
| `PROTO_TYPE_RESISTANCE` | `0x10` | 基站 -> 平衡板 | `15` | 按需 | 阻力参数帧 |
|
||||
| `PROTO_TYPE_SPOTTER` | `0x11` | 基站 -> 平衡板 | `8` | 按需 | Spotter 参数帧 |
|
||||
| `PROTO_TYPE_HEARTBEAT` | `0x20` | 基站 -> 平衡板 | `4` | `~1 Hz` | 心跳帧 |
|
||||
|
||||
### 5.4 当前上行主帧
|
||||
|
||||
CoP 帧固定 17 字节,结构如下:
|
||||
|
||||
| 字节偏移 | 字段 | 类型 | 说明 |
|
||||
|----------|------|------|------|
|
||||
| `0` | `sync0` | `uint8` | 固定 `0xAA` |
|
||||
| `1` | `sync1` | `uint8` | 固定 `0x55` |
|
||||
| `2` | `type` | `uint8` | 固定 `0x01` |
|
||||
| `3` | `seq` | `uint8` | 包序号,循环递增 |
|
||||
| `4` | `flags` | `uint8` | `bit0` 表示 `force_valid` |
|
||||
| `5-8` | `cop_x` | `float32 LE` | 压力中心 X 坐标,单位 mm |
|
||||
| `9-12` | `cop_y` | `float32 LE` | 压力中心 Y 坐标,单位 mm |
|
||||
| `13-16` | `force` | `float32 LE` | 总重量,单位 kg |
|
||||
|
||||
更完整的字段说明和示例帧见 [BALANCE_BOARD_PROTOCOL.md](./BALANCE_BOARD_PROTOCOL.md)。
|
||||
|
||||
## 6. ADS1256 / CoP 规格
|
||||
|
||||
### 6.1 初始化与采样流程
|
||||
|
||||
当前固件的 ADS1256 侧流程如下:
|
||||
|
||||
```text
|
||||
上电
|
||||
|
|
||||
+-- 配置 CS / DRDY / RESET GPIO
|
||||
+-- 绑定 SPI1,频率 500 kHz
|
||||
+-- 硬件复位 ADS1256
|
||||
+-- 发送 CMD_SDATAC,关闭连续输出模式
|
||||
+-- 写寄存器: STATUS=0x06, MUX=0x01, ADCON=0x07, DRATE=0xC0
|
||||
+-- 发送 CMD_SELFCAL
|
||||
+-- 回读关键寄存器做校验
|
||||
+-- tare 去皮
|
||||
|
|
||||
+-- 传感器线程循环
|
||||
|
|
||||
+-- 依次切换 4 路 MUX
|
||||
+-- 每路读 17 次并求均值
|
||||
+-- 扣除零点偏移
|
||||
+-- 死区抑制
|
||||
+-- 重量换算
|
||||
+-- CoP 解算
|
||||
+-- 打包 CoP 帧并发送
|
||||
```
|
||||
|
||||
### 6.2 关键参数
|
||||
|
||||
| 项目 | 当前值 | 说明 |
|
||||
|------|--------|------|
|
||||
| SPI 频率 | `500 kHz` | `spi_cfg.frequency = 500000` |
|
||||
| STATUS | `0x06` | `ACAL=1`,`BUFEN=1` |
|
||||
| PGA | `64` | `ADCON = 0x07` |
|
||||
| 采样率 | `3750 SPS` | `DRATE = 0xC0` |
|
||||
| 每帧均值次数 | `17` | `AVG_COUNT = 17` |
|
||||
| 去皮窗口 | `17` | 每路取 17 点,中位数为零点 |
|
||||
| 死区 | `±250 counts` | `DEADZONE_THRESHOLD = 250` |
|
||||
| 板面半宽 | `100 mm` | `BOARD_HALF_WIDTH_MM = 100.0f` |
|
||||
| 板面半长 | `100 mm` | `BOARD_HALF_LENGTH_MM = 100.0f` |
|
||||
| CoP 最小有效总重 | `0.5 kg` | `COP_MIN_FORCE_THRESHOLD = 0.5f` |
|
||||
| 调试日志频率 | 每 10 帧一次 | 目标运行状态下约 `5 Hz` |
|
||||
|
||||
### 6.3 CoP 判定规则
|
||||
|
||||
- 四路去皮后的 ADC 值先乘 `ADC_TO_FORCE_SCALE`,换算成重量
|
||||
- 总重量低于 `0.5 kg` 时,`cop_x` 和 `cop_y` 置 `0.0f`
|
||||
- 此时 `flags.bit0 = 0`,表示 CoP 坐标没有物理意义
|
||||
- 总重量达到阈值后,再根据四角坐标做加权平均
|
||||
|
||||
当前四路传感器与板面坐标的映射如下:
|
||||
|
||||
| 通道 | 位置 | X | Y |
|
||||
|------|------|---|---|
|
||||
| `S0` | 右前 `FR` | `+100` | `+100` |
|
||||
| `S1` | 右后 `BR` | `+100` | `-100` |
|
||||
| `S2` | 左后 `BL` | `-100` | `-100` |
|
||||
| `S3` | 左前 `FL` | `-100` | `+100` |
|
||||
|
||||
## 7. 开发环境与板级配置
|
||||
|
||||
当前工程采用标准 Zephyr / NCS 应用结构,目标板为 `nrf5340dk/nrf5340/cpuapp`。
|
||||
|
||||
`prj.conf` 当前可以直接确认的配置点包括:
|
||||
|
||||
- `CONFIG_BT=y`,启用 BLE
|
||||
- `CONFIG_BT_PERIPHERAL=y`,工作在外设角色
|
||||
- `CONFIG_BT_NUS=y`,启用 Nordic UART Service
|
||||
- `CONFIG_BT_DEVICE_NAME="GML670_System"`,设置广播设备名
|
||||
- `CONFIG_BT_NUS_SECURITY_ENABLED=n`,关闭 NUS 安全限制
|
||||
- 日志走 RTT:`CONFIG_USE_SEGGER_RTT=y`
|
||||
- 关闭 UART Console:`CONFIG_CONSOLE=n`、`CONFIG_UART_CONSOLE=n`
|
||||
- 关闭 UART 日志后端:`CONFIG_LOG_BACKEND_UART=n`
|
||||
- 关闭串口驱动:`CONFIG_SERIAL=n`
|
||||
|
||||
`app.overlay` 当前只保留了 SPI1、ADS1256 设备节点和三根控制 GPIO 的定义,没有把 UART Console 复用回来。
|
||||
|
||||
## 8. 项目结构
|
||||
|
||||
当前阅读和维护时最关键的文件如下:
|
||||
|
||||
| 路径 | 说明 |
|
||||
|------|------|
|
||||
| `src/main.c` | 启动入口,负责初始化 BLE、协议层和传感器子系统 |
|
||||
| `src/ads1256.c` | ADS1256 SPI 读写、复位、寄存器配置和校验 |
|
||||
| `src/sensor.c` | 采样线程、tare、均值滤波、重量换算和 CoP 计算 |
|
||||
| `src/comm_protocol.c` | 协议收发解析层:上行组包、下行解析、msgq 分发 |
|
||||
| `src/ble_transport.c` | BLE 广播、NUS 字节收发、断线重广播 |
|
||||
| `inc/comm_protocol.h` | 帧格式定义、标定子命令码、下行消息队列类型和协议 API |
|
||||
| `app.overlay` | SPI1、CS、DRDY、RESET 的板级连线 |
|
||||
| `prj.conf` | BLE、日志和控制台相关配置 |
|
||||
| `BALANCE_BOARD_PROTOCOL.md` | 协议专项说明、字段表和示例帧 |
|
||||
|
||||
## 9. 构建与烧录
|
||||
|
||||
在已经配置好 Zephyr / NCS 环境的前提下,可以直接在工程根目录执行:
|
||||
|
||||
```bash
|
||||
west build -b nrf5340dk/nrf5340/cpuapp -p always .
|
||||
west flash
|
||||
```
|
||||
|
||||
如果只是重新编译当前应用,默认目标仍然是 `nrf5340dk/nrf5340/cpuapp`。
|
||||
|
||||
## 10. 日志与当前约束
|
||||
|
||||
### 10.1 日志
|
||||
|
||||
当前日志走 RTT,不占用 UART Console。启动后可以看到如下类型的信息:
|
||||
|
||||
- 启动 banner
|
||||
- BLE transport 初始化结果
|
||||
- ADS1256 关键寄存器回读结果
|
||||
- tare 开始和结束
|
||||
- Resistance / Spotter 下行参数日志
|
||||
- 周期性的四角重量、总重、CoP 和帧耗时日志
|
||||
|
||||
Heartbeat 接收路径已经接入,但默认 `DBG` 级别下才单独打印。
|
||||
|
||||
### 10.2 当前约束
|
||||
|
||||
- 阻力参数、Spotter 参数和 Heartbeat 目前只做到协议接收、内存保存和日志留痕
|
||||
- 编码器上行帧只在协议头里预留,当前没有实际数据源
|
||||
- 传感器线程没有额外 `k_msleep()` 节流,实际帧率由 ADS1256 采样和 BLE 发送耗时共同决定
|
||||
- README 中的频率描述按当前 `3750 SPS + 4 通道 * 17 次均值` 这套实现估算,目标运行频率约 `50 Hz`
|
||||
- 当前实现默认四角传感器是固定几何布局,半宽和半长都是 `100 mm`
|
||||
|
||||
### 10.3 TODO
|
||||
|
||||
- [ ] L1 增益标定:当前使用理论推导值 `ADC_TO_FORCE_SCALE`,实测 10 kg 砝码显示 9 kg(约 10% 误差),需用已知砝码通过标定命令校准每通道增益
|
||||
- [ ] BR 通道硬件排查:静止无马达状态下 BR 通道有 ±4600 counts 异常抖动(其余通道仅 ±500),疑似接触不良或安装松动
|
||||
- [ ] 马达振动频率确认:当前 IIR 截止 1.2 Hz,若实际振动低于帧率一半(23.5 Hz)存在混叠,可能需要在 ADC 级做抗混叠或调整采样策略
|
||||
|
||||
-66
@@ -1,66 +0,0 @@
|
||||
/*
|
||||
* Copyright (c) 2022 Nordic Semiconductor ASA
|
||||
*
|
||||
* SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
|
||||
*/
|
||||
|
||||
/*
|
||||
* nRF5340 DK (App Core) Overlay for ADS1256
|
||||
* 仅保留 SPI 和 GPIO 定义
|
||||
*/
|
||||
|
||||
&spi1 {
|
||||
compatible = "nordic,nrf-spim";
|
||||
status = "okay";
|
||||
pinctrl-0 = <&spi1_default>;
|
||||
pinctrl-1 = <&spi1_sleep>;
|
||||
pinctrl-names = "default", "sleep";
|
||||
|
||||
/* CS 引脚配置 */
|
||||
cs-gpios = <&gpio1 12 GPIO_ACTIVE_LOW>;
|
||||
|
||||
ads1256: ads1256@0 {
|
||||
compatible = "ti,ads1256";
|
||||
reg = <0>;
|
||||
spi-max-frequency = <1000000>;
|
||||
};
|
||||
};
|
||||
|
||||
&pinctrl {
|
||||
spi1_default: spi1_default {
|
||||
group1 {
|
||||
psels = <NRF_PSEL(SPIM_SCK, 1, 15)>,
|
||||
<NRF_PSEL(SPIM_MOSI, 1, 13)>,
|
||||
<NRF_PSEL(SPIM_MISO, 1, 14)>;
|
||||
};
|
||||
};
|
||||
spi1_sleep: spi1_sleep {
|
||||
group1 {
|
||||
psels = <NRF_PSEL(SPIM_SCK, 1, 15)>,
|
||||
<NRF_PSEL(SPIM_MOSI, 1, 13)>,
|
||||
<NRF_PSEL(SPIM_MISO, 1, 14)>;
|
||||
low-power-enable;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
/ {
|
||||
aliases {
|
||||
ads-drdy = &ads_drdy_pin;
|
||||
ads-reset = &ads_reset_pin;
|
||||
ads-pdwn = &ads_pdwn_pin;
|
||||
};
|
||||
|
||||
ads1256_control {
|
||||
compatible = "gpio-keys";
|
||||
ads_drdy_pin: ads_drdy {
|
||||
gpios = <&gpio1 11 GPIO_ACTIVE_LOW>;
|
||||
};
|
||||
ads_reset_pin: ads_reset {
|
||||
gpios = <&gpio1 10 GPIO_ACTIVE_LOW>;
|
||||
};
|
||||
ads_pdwn_pin: ads_pdwn {
|
||||
gpios = <&gpio1 9 GPIO_ACTIVE_LOW>;
|
||||
};
|
||||
};
|
||||
};
|
||||
@@ -1,302 +0,0 @@
|
||||
# 平衡板协议说明
|
||||
|
||||
本文说明当前固件里的数据包格式和 BLE 通讯方式。
|
||||
|
||||
## 1. BLE 通讯方式
|
||||
|
||||
平衡板工作在 BLE `Peripheral` 角色,通过 Nordic UART Service(NUS)与基站通信。
|
||||
|
||||
- 上行方向:平衡板 -> 基站,使用 NUS `Notify`
|
||||
- 下行方向:基站 -> 平衡板,使用 NUS `Write`
|
||||
- 广播内容:包含设备名和 NUS 128-bit UUID
|
||||
|
||||
### 1.1 NUS UUID
|
||||
|
||||
| 项目 | UUID | 方向 | 说明 |
|
||||
|------|------|------|------|
|
||||
| Service | `6E400001-B5A3-F393-E0A9-E50E24DCCA9E` | - | NUS 主服务 |
|
||||
| TX Characteristic | `6E400003-B5A3-F393-E0A9-E50E24DCCA9E` | 平衡板 -> 基站 | Notify,上报 CoP 和编码器数据 |
|
||||
| RX Characteristic | `6E400002-B5A3-F393-E0A9-E50E24DCCA9E` | 基站 -> 平衡板 | Write,下发阻力参数和控制命令 |
|
||||
|
||||
### 1.2 当前连接行为
|
||||
|
||||
- 连接建立后,平衡板会请求 `7.5-15 ms` 连接区间(`min=6, max=12`)。
|
||||
- 所有数据通过 NUS 二进制帧发送,不走文本协议。
|
||||
|
||||
## 2. 数据包总览
|
||||
|
||||
当前协议使用双帧头 `0xAA 0x55` 作为包起始标记。所有多字节字段按 Little Endian 编码。
|
||||
|
||||
每帧末尾附加 1 字节 CRC-8/MAXIM 校验(poly=`0x31`, init=`0x00`, refin=true, refout=true, xorout=`0x00`)。校验范围为帧头之后、CRC 字节之前的所有字段(即跳过 `sync0`/`sync1`)。调用方式:`crc8(data + 2, len - 3, 0x31, 0x00, true)`。
|
||||
|
||||
| 类型名 | 值 | 方向 | 长度 | 频率 | 说明 |
|
||||
|--------|----|------|------|------|------|
|
||||
| `PROTO_TYPE_COP` | `0x01` | 平衡板 -> 基站 | `11` | 50 Hz | 压力中心帧 |
|
||||
| `PROTO_TYPE_ENCODER` | `0x02` | 平衡板 -> 基站 | `13` | TBD | 电机编码器帧(预留) |
|
||||
| `PROTO_TYPE_RESISTANCE` | `0x10` | 基站 -> 平衡板 | `16` | 按需 | 阻力参数帧 |
|
||||
| `PROTO_TYPE_SPOTTER` | `0x11` | 基站 -> 平衡板 | `9` | 按需 | 保护模式帧 |
|
||||
| `PROTO_TYPE_HEARTBEAT` | `0x20` | 基站 -> 平衡板 | `5` | ~1 Hz | 心跳帧 |
|
||||
| `PROTO_TYPE_CAL_CMD` | `0x30` | 基站 -> 平衡板 | `10` | 按需 | 标定命令帧 |
|
||||
| `PROTO_TYPE_CAL_RESP` | `0x31` | 平衡板 -> 基站 | `14` | 按需 | 标定响应帧 |
|
||||
|
||||
## 3. 上行数据包
|
||||
|
||||
### 3.1 压力中心帧 (CoP)
|
||||
|
||||
CoP 帧固定 11 字节,结构如下:
|
||||
|
||||
| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 |
|
||||
|----------|------|------|---------|------|
|
||||
| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 |
|
||||
| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 |
|
||||
| `2` | `type` | `uint8` | `0x01` | CoP 包类型 |
|
||||
| `3` | `flags` | `uint8` | 见下表 | 状态标志位 |
|
||||
| `4-5` | `cop_x` | `int16 LE` | - | 压力中心 X 坐标 (cm) |
|
||||
| `6-7` | `cop_y` | `int16 LE` | - | 压力中心 Y 坐标 (cm) |
|
||||
| `8-9` | `force` | `int16 LE` | - | 总重量 (kg) |
|
||||
| `10` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 |
|
||||
|
||||
#### flags 位定义
|
||||
|
||||
| 位 | 名称 | 说明 |
|
||||
|----|------|------|
|
||||
| bit0 | `COP_FLAG_FORCE_VALID` | `1` 表示总力超过阈值,CoP 坐标有意义 |
|
||||
| bit1-7 | 预留 | 填 `0` |
|
||||
|
||||
#### CoP 解算公式
|
||||
|
||||
传感器布局(俯视):
|
||||
|
||||
```text
|
||||
S4 (FL) -------- S1 (FR)
|
||||
| center |
|
||||
S3 (BL) -------- S2 (BR)
|
||||
```
|
||||
|
||||
传感器坐标(单位:cm,板面中心为原点):
|
||||
|
||||
| 传感器 | 位置 | X | Y |
|
||||
|--------|------|---|---|
|
||||
| S1 | 右前 (FR) | `+10` | `+10` |
|
||||
| S2 | 右后 (BR) | `+10` | `-10` |
|
||||
| S3 | 左后 (BL) | `-10` | `-10` |
|
||||
| S4 | 左前 (FL) | `-10` | `+10` |
|
||||
|
||||
计算公式:
|
||||
|
||||
```text
|
||||
F_total = F0 + F1 + F2 + F3
|
||||
|
||||
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`。
|
||||
|
||||
#### 示例
|
||||
|
||||
下面是一个 CoP 帧示例(CoP 有效,cop_x=3 cm, cop_y=5 cm, force=75 kg):
|
||||
|
||||
```text
|
||||
AA 55 01 01 03 00 05 00 4B 00 XX
|
||||
```
|
||||
|
||||
含义如下:
|
||||
|
||||
- `AA 55`:双帧头
|
||||
- `01`:CoP 包
|
||||
- `01`:flags = `force_valid`
|
||||
- `03 00`:`cop_x = 3` cm(LE int16)
|
||||
- `05 00`:`cop_y = 5` cm(LE int16)
|
||||
- `4B 00`:`force = 75` kg(LE uint16)
|
||||
- `XX`:CRC-8/MAXIM(对字节 `[2..9]` 计算)
|
||||
|
||||
### 3.2 电机编码器帧(预留)
|
||||
|
||||
编码器帧固定 13 字节,结构如下。该帧目前仅在协议中预留定义,固件暂不实现。
|
||||
|
||||
| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 |
|
||||
|----------|------|------|---------|------|
|
||||
| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 |
|
||||
| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 |
|
||||
| `2` | `type` | `uint8` | `0x02` | 编码器包类型 |
|
||||
| `3` | `flags` | `uint8` | `0` | 预留 |
|
||||
| `4-7` | `cable_length` | `float32 LE` | - | 拉索长度 (mm) |
|
||||
| `8-11` | `velocity` | `float32 LE` | - | 拉索速度 (mm/s) |
|
||||
| `12` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 |
|
||||
|
||||
## 4. 下行数据包
|
||||
|
||||
### 4.1 阻力参数帧
|
||||
|
||||
阻力参数帧固定 16 字节,用于基站向平衡板下发电机阻力控制参数。
|
||||
|
||||
| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 |
|
||||
|----------|------|------|---------|------|
|
||||
| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 |
|
||||
| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 |
|
||||
| `2` | `type` | `uint8` | `0x10` | 阻力参数包类型 |
|
||||
| `3-6` | `K` | `float32 LE` | - | 刚度 (N/m) |
|
||||
| `7-10` | `B` | `float32 LE` | - | 阻尼 (Ns/m) |
|
||||
| `11-14` | `Tau` | `float32 LE` | - | 时间常数 (s) |
|
||||
| `15` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 |
|
||||
|
||||
#### 示例
|
||||
|
||||
```text
|
||||
AA 55 10 00 00 C8 42 00 00 48 41 CD CC 4C 3E XX
|
||||
```
|
||||
|
||||
含义如下:
|
||||
|
||||
- `AA 55 10`:阻力参数帧头
|
||||
- `00 00 C8 42`:`K = 100.0` N/m
|
||||
- `00 00 48 41`:`B = 12.5` Ns/m
|
||||
- `CD CC 4C 3E`:`Tau = 0.2` s
|
||||
- `XX`:CRC-8/MAXIM
|
||||
|
||||
### 4.2 Spotter Mode 帧
|
||||
|
||||
Spotter 帧固定 9 字节,用于启用或关闭保护模式。
|
||||
|
||||
| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 |
|
||||
|----------|------|------|---------|------|
|
||||
| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 |
|
||||
| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 |
|
||||
| `2` | `type` | `uint8` | `0x11` | Spotter 包类型 |
|
||||
| `3-6` | `threshold` | `float32 LE` | - | 保护力阈值 (N) |
|
||||
| `7` | `enable` | `uint8` | `0` 或 `1` | `0` = 关闭, `1` = 启用 |
|
||||
| `8` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 |
|
||||
|
||||
#### 示例
|
||||
|
||||
启用 Spotter Mode,阈值 500 N:
|
||||
|
||||
```text
|
||||
AA 55 11 00 00 FA 43 01 XX
|
||||
```
|
||||
|
||||
### 4.3 心跳帧
|
||||
|
||||
心跳帧固定 5 字节,基站以约 1 Hz 频率发送,用于活性检测。
|
||||
|
||||
| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 |
|
||||
|----------|------|------|---------|------|
|
||||
| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 |
|
||||
| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 |
|
||||
| `2` | `type` | `uint8` | `0x20` | 心跳包类型 |
|
||||
| `3` | `counter` | `uint8` | `0-255` | 滚动计数 |
|
||||
| `4` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 |
|
||||
|
||||
#### 示例
|
||||
|
||||
```text
|
||||
AA 55 20 05 XX
|
||||
```
|
||||
|
||||
含义:心跳包,计数 `5`,`XX` 为 CRC。
|
||||
|
||||
## 5. 标定协议
|
||||
|
||||
标定协议通过下行标定命令帧(`0x30`)和上行标定响应帧(`0x31`)完成传感器在线标定。标定分两级:L1 单通道零点/增益标定,L2 多点网格空间修正。
|
||||
|
||||
### 5.1 标定命令帧(下行)
|
||||
|
||||
标定命令帧固定 10 字节,由基站发送给平衡板:
|
||||
|
||||
| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 |
|
||||
|----------|------|------|---------|------|
|
||||
| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 |
|
||||
| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 |
|
||||
| `2` | `type` | `uint8` | `0x30` | 标定命令包类型 |
|
||||
| `3` | `subcmd` | `uint8` | 见下表 | 子命令码 |
|
||||
| `4` | `target` | `uint8` | `0-3` / `0-8` | 通道号或网格点号 |
|
||||
| `5-8` | `param` | `float32 LE` | - | 参数(如已知质量 kg) |
|
||||
| `9` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 |
|
||||
|
||||
#### `subcmd` 子命令码定义
|
||||
|
||||
| 子命令码 | 名称 | target 含义 | param 含义 | 说明 |
|
||||
|----------|------|-------------|------------|------|
|
||||
| `0x01` | `START_L1` | - | - | 进入 L1 标定模式 |
|
||||
| `0x02` | `TARE_CH` | 通道号 (0-3) | - | 对指定通道执行零点标定 |
|
||||
| `0x03` | `GAIN_CH` | 通道号 (0-3) | 已知质量 (kg) | 对指定通道执行增益标定 |
|
||||
| `0x04` | `COMMIT_L1` | - | - | 提交 L1 标定数据到 NVS |
|
||||
| `0x05` | `ABORT` | - | - | 中止当前标定流程 |
|
||||
| `0x10` | `START_L2` | - | - | 进入 L2 网格标定模式 |
|
||||
| `0x11` | `RECORD_GRID` | 网格点号 (0-8) | - | 记录当前网格点 CoP 误差 |
|
||||
| `0x12` | `COMMIT_L2` | - | - | 提交 L2 标定数据到 NVS |
|
||||
| `0x20` | `ERASE` | - | - | 擦除所有标定数据 |
|
||||
| `0x21` | `QUERY` | - | - | 查询当前标定状态 |
|
||||
|
||||
#### 示例
|
||||
|
||||
对通道 2 执行增益标定,已知质量 10.0 kg:
|
||||
|
||||
```text
|
||||
AA 55 30 03 02 00 00 20 41 XX
|
||||
```
|
||||
|
||||
- `AA 55 30`:标定命令帧头
|
||||
- `03`:subcmd = `GAIN_CH`
|
||||
- `02`:target = 通道 2
|
||||
- `00 00 20 41`:param = `10.0` kg(float32 LE)
|
||||
- `XX`:CRC-8/MAXIM
|
||||
|
||||
### 5.2 标定响应帧(上行)
|
||||
|
||||
标定响应帧固定 14 字节,平衡板在执行标定命令后回复:
|
||||
|
||||
| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 |
|
||||
|----------|------|------|---------|------|
|
||||
| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 |
|
||||
| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 |
|
||||
| `2` | `type` | `uint8` | `0x31` | 标定响应包类型 |
|
||||
| `3` | `status` | `uint8` | 见下表 | 结果状态码 |
|
||||
| `4` | `subcmd` | `uint8` | - | 对应的子命令回显 |
|
||||
| `5-12` | `data` | `uint8[8]` | - | 响应数据(上下文相关) |
|
||||
| `13` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 |
|
||||
|
||||
#### 状态码定义
|
||||
|
||||
| 状态码 | 名称 | 说明 |
|
||||
|--------|------|------|
|
||||
| `0x00` | `OK` | 命令执行成功 |
|
||||
| `0x01` | `ERR_STATE` | 状态机不允许此操作 |
|
||||
| `0x02` | `ERR_NVS` | NVS 读写失败 |
|
||||
| `0x03` | `ERR_PARAM` | 参数非法 |
|
||||
|
||||
#### data 字段含义
|
||||
|
||||
`data[8]` 内容取决于子命令:
|
||||
|
||||
| 子命令 | data 内容 | 说明 |
|
||||
|--------|-----------|------|
|
||||
| `TARE_CH` | `data[0..3]` = 零点 ADC 值 (int32 LE) | 通道去皮后的零点偏移 |
|
||||
| `GAIN_CH` | `data[0..3]` = 增益 (float32 LE) | kg/count 换算系数 |
|
||||
| `QUERY` | `data[0]` = L1 zero valid, `data[1]` = L1 gain valid, `data[2]` = L2 valid | 各级标定有效性标志 |
|
||||
| 其他 | 全零 | 无附加数据 |
|
||||
|
||||
#### 示例
|
||||
|
||||
通道 2 增益标定成功,增益 = 5.322e-5 kg/count:
|
||||
|
||||
```text
|
||||
AA 55 31 00 03 XX XX XX XX 00 00 00 00 XX
|
||||
```
|
||||
|
||||
- `AA 55 31`:标定响应帧头
|
||||
- `00`:status = `OK`
|
||||
- `03`:subcmd = `GAIN_CH`(回显)
|
||||
- 前 4 字节 data:增益值 float32 LE
|
||||
- 后 4 字节 data:填 0
|
||||
- 末字节:CRC-8/MAXIM
|
||||
|
||||
## 6. 当前实现约束
|
||||
|
||||
- CoP 帧固定 11 字节,可在最小 ATT MTU 23(NUS 有效负载 20 字节)下传输。
|
||||
- 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测,因此心跳仅为下行。
|
||||
- 传感器通道到物理位置的映射:MUX `{0x01, 0x23, 0x45, 0x67}` → FL/FR/BR/BL。
|
||||
- `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 要求。
|
||||
Binary file not shown.
Binary file not shown.
@@ -1,191 +0,0 @@
|
||||
# BLE 接入说明
|
||||
|
||||
本文档只说明客户实现“连接蓝牙”和“设置力控参数”所需的最小协议。
|
||||
|
||||
## 1. BLE 连接
|
||||
|
||||
设备使用 Nordic UART Service 风格的 BLE 服务。
|
||||
|
||||
| 用途 | UUID | 方向 |
|
||||
| --- | --- | --- |
|
||||
| Service | `6e400001-b5a3-f393-e0a9-e50e24dcca9e` | - |
|
||||
| TX characteristic | `6e400002-b5a3-f393-e0a9-e50e24dcca9e` | 客户 App 写入设备 |
|
||||
| RX characteristic | `6e400003-b5a3-f393-e0a9-e50e24dcca9e` | 设备 notify 给客户 App |
|
||||
|
||||
连接流程:
|
||||
|
||||
1. 扫描 BLE 设备,设备名通常包含 `YRobot`。
|
||||
2. 连接目标设备。
|
||||
3. 对 RX characteristic 开启 notify。
|
||||
4. 向 TX characteristic 写入力控参数帧。
|
||||
|
||||
单包最大长度建议不超过 `250 bytes`。
|
||||
|
||||
## 2. 左右侧设备
|
||||
|
||||
力控参数帧的 `key` 需要区分左右侧。
|
||||
|
||||
| 侧别 | 写入 key |
|
||||
| --- | --- |
|
||||
| 左侧 | `0x6F` |
|
||||
| 右侧 | `0xAF` |
|
||||
| 不区分左右侧 | `0x2F` |
|
||||
|
||||
现有工具通过设备名判断左右侧:
|
||||
|
||||
- 右侧:设备名包含 `ZDR`, `ZDB`, `ZCR`, `ZCB`, `ARR`, `ARB`, `ASR`, `ASB`
|
||||
- 左侧:设备名包含 `ZDL`, `ZCL`, `ARL`, `ASL`
|
||||
|
||||
如果设备名无法判断,客户 App 需要让用户手动选择左/右侧。
|
||||
|
||||
## 3. 力控参数帧格式
|
||||
|
||||
设置力控参数使用 JSON 字符串,通过 TX characteristic 写入。
|
||||
|
||||
帧格式:
|
||||
|
||||
```text
|
||||
[0] key
|
||||
[1] length
|
||||
[2] crc
|
||||
[3] command
|
||||
[4...] utf8(json)
|
||||
```
|
||||
|
||||
字段说明:
|
||||
|
||||
| 字段 | 说明 |
|
||||
| --- | --- |
|
||||
| `key` | 左侧 `0x6F`,右侧 `0xAF` |
|
||||
| `length` | 整帧长度,即 `json_payload_len + 4` |
|
||||
| `crc` | 固定填 `0x00` |
|
||||
| `command` | 设置参数固定填 `0x01` |
|
||||
| `json` | UTF-8 编码的 JSON 参数 |
|
||||
|
||||
也就是:
|
||||
|
||||
```text
|
||||
payload = [0x01] + UTF8(json_string)
|
||||
frame = [key, len(payload) + 3, 0x00] + payload
|
||||
```
|
||||
|
||||
## 4. 设置力控模式和参数
|
||||
|
||||
### 力控参数表
|
||||
|
||||
所有 key 都定义在 `TuningParams::RegisterForceModeCtrl()` 中。
|
||||
|
||||
| JSON key | 默认值 | 单位 | 说明 |
|
||||
| --- | ---: | --- | --- |
|
||||
| `fm_mode` | `0.0` | - | 力控模式选择,四舍五入后映射到模式枚举 |
|
||||
| `fm_mset` | `0.1` | kg | 用户设定重量 |
|
||||
| `fm_kin` | `0.0` | - | 惯性比例系数,虚拟质量 = `fm_kin * fm_mset` |
|
||||
| `fm_bfr` | `0.0` | N/(m/s) | 线性摩擦/阻尼系数 |
|
||||
| `fm_kecc` | `0.1` | - | 离心倍率,离心力 = `fm_kecc * 向心力` |
|
||||
| `fm_vth` | `0.3` | m/s | 离心/向心平滑切换速度阈值 |
|
||||
| `fm_cdrv` | `0.1` | N/(m/s)^2 | 粘滞模式拉出方向平方阻尼系数 |
|
||||
| `fm_crec` | `0.1` | N/(m/s) | 粘滞模式回收方向线性阻尼系数 |
|
||||
| `fm_k` | `0.0` | N/m | 弹性刚度 |
|
||||
| `fm_x0` | `0.0` | m | 弹性零点位置 |
|
||||
| `fm_vmax` | `1.0` | m/s | 等速模式最大速度 |
|
||||
| `fm_gwall` | `0.0` | N/(m/s) | 等速速度墙增益 |
|
||||
| `fm_sp_v` | `0.1` | m/s | Spotter 触发速度阈值 |
|
||||
| `fm_sp_t` | `5.0` | s | Spotter 触发时间阈值 |
|
||||
| `fm_sp_d` | `5.0` | s | Spotter 减重衰减时长 |
|
||||
| `fm_sp_home` | `0.0` | m | Spotter 参考初始位置 |
|
||||
| `fm_sp_rng` | `-0.10` | m | Spotter 负重判定位置阈值 |
|
||||
| `fm_sp_rec` | `1.0` | s | Spotter 恢复时长;当前实现中会被同步为 `fm_sp_t` |
|
||||
| `fm_rad` | `0.2` | m | 滑轮半径,输出扭矩 = 输出力 * `fm_rad` |
|
||||
|
||||
### 力控模式
|
||||
|
||||
`fm_mode` 会先四舍五入,再限幅到 `0...6`。
|
||||
|
||||
| `fm_mode` | 模式 | 主要参数 | 说明 |
|
||||
| ---: | --- | --- | --- |
|
||||
| `0` | None | - | 输出力为 0 |
|
||||
| `1` | FreeWeight | `fm_mset`, `fm_kin`, `fm_bfr`, `fm_rad` | 自由重量/惯性补偿 |
|
||||
| `2` | EccentricOverload | `fm_mset`, `fm_kin`, `fm_bfr`, `fm_kecc`, `fm_vth`, Spotter 参数 | 离心超负荷 |
|
||||
| `3` | Viscous | `fm_cdrv`, `fm_crec`, `fm_rad` | 粘滞/流体阻尼 |
|
||||
| `4` | Elastic | `fm_k`, `fm_x0`, `fm_rad` | 弹性/弹簧 |
|
||||
| `5` | IsokineticSpotting | `fm_mset`, `fm_vmax`, `fm_gwall`, Spotter 参数 | 等速速度墙 |
|
||||
| `6` | Spotter | `fm_mset`, `fm_kin`, `fm_bfr`, Spotter 参数 | 保护模式 |
|
||||
|
||||
## 5. 示例
|
||||
|
||||
设置右侧设备为离心超负荷模式:
|
||||
|
||||
```json
|
||||
{
|
||||
"fm_mode": 2,
|
||||
"fm_mset": 10.0,
|
||||
"fm_vth": 0.3,
|
||||
"fm_kecc": 1.5,
|
||||
"fm_kin": 1.0,
|
||||
"fm_bfr": 0.05,
|
||||
"fm_rad": 0.04
|
||||
}
|
||||
```
|
||||
|
||||
对应写入帧:
|
||||
|
||||
```text
|
||||
key = 0xAF
|
||||
command = 0x01
|
||||
json_string = '{"fm_mode":2,"fm_mset":10.0,"fm_vth":0.3,"fm_kecc":1.5,"fm_kin":1.0,"fm_bfr":0.05,"fm_rad":0.04}'
|
||||
|
||||
payload = [0x01] + UTF8(json_string)
|
||||
frame = [0xAF, len(payload) + 3, 0x00] + payload
|
||||
```
|
||||
|
||||
写入 TX characteristic UUID:
|
||||
|
||||
```text
|
||||
6e400002-b5a3-f393-e0a9-e50e24dcca9e
|
||||
```
|
||||
|
||||
## 6. 查询当前力控参数
|
||||
|
||||
查询参数同样写 TX characteristic。
|
||||
|
||||
查询当前模式参数:
|
||||
|
||||
```text
|
||||
frame = [key, 0x04, 0x00, 0x02]
|
||||
```
|
||||
|
||||
查询指定模式参数,例如查询 `fm_mode = 2`:
|
||||
|
||||
```text
|
||||
payload = [0x02] + UTF8('{"fm_mode":2}')
|
||||
frame = [key, len(payload) + 3, 0x00] + payload
|
||||
```
|
||||
|
||||
设备会通过 RX characteristic notify 返回 JSON 参数。返回可能分包,需要按包序号拼接:
|
||||
|
||||
```text
|
||||
[0] key
|
||||
[1] length
|
||||
[2] crc
|
||||
[3] type = 0x02
|
||||
[4] packet_index
|
||||
[5] packet_count
|
||||
[6...] utf8(json_fragment)
|
||||
```
|
||||
|
||||
当 `packet_index == packet_count` 时,说明最后一包已收到,可以拼接所有 `json_fragment` 后解析 JSON。
|
||||
|
||||
## 7. 心跳包
|
||||
当连接上设备之后,需要每隔2s发送心跳包给设备,维持和设备的连接
|
||||
```text
|
||||
[0] key
|
||||
[1] length
|
||||
[2] crc
|
||||
[3] command = 0x00
|
||||
```
|
||||
## 8. 注意事项
|
||||
|
||||
- 写入前必须先连接设备并开启 RX notify。
|
||||
- 写入 key 必须和设备侧别一致:左侧 `0x6F`,右侧 `0xAF`。
|
||||
- `crc` 当前固定填 `0x00`。
|
||||
- JSON 使用 UTF-8 编码。
|
||||
@@ -1,191 +0,0 @@
|
||||
# BLE 接入说明
|
||||
|
||||
本文档只说明客户实现“连接蓝牙”和“设置力控参数”所需的最小协议。
|
||||
|
||||
## 1. BLE 连接
|
||||
|
||||
设备使用 Nordic UART Service 风格的 BLE 服务。
|
||||
|
||||
| 用途 | UUID | 方向 |
|
||||
| --- | --- | --- |
|
||||
| Service | `6e400001-b5a3-f393-e0a9-e50e24dcca9e` | - |
|
||||
| TX characteristic | `6e400002-b5a3-f393-e0a9-e50e24dcca9e` | 客户 App 写入设备 |
|
||||
| RX characteristic | `6e400003-b5a3-f393-e0a9-e50e24dcca9e` | 设备 notify 给客户 App |
|
||||
|
||||
连接流程:
|
||||
|
||||
1. 扫描 BLE 设备,设备名通常包含 `YRobot`。
|
||||
2. 连接目标设备。
|
||||
3. 对 RX characteristic 开启 notify。
|
||||
4. 向 TX characteristic 写入力控参数帧。
|
||||
|
||||
单包最大长度建议不超过 `250 bytes`。
|
||||
|
||||
## 2. 左右侧设备
|
||||
|
||||
力控参数帧的 `key` 需要区分左右侧。
|
||||
|
||||
| 侧别 | 写入 key |
|
||||
| --- | --- |
|
||||
| 左侧 | `0x6F` |
|
||||
| 右侧 | `0xAF` |
|
||||
| 不区分左右侧 | `0x2F` |
|
||||
|
||||
现有工具通过设备名判断左右侧:
|
||||
|
||||
- 右侧:设备名包含 `ZDR`, `ZDB`, `ZCR`, `ZCB`, `ARR`, `ARB`, `ASR`, `ASB`
|
||||
- 左侧:设备名包含 `ZDL`, `ZCL`, `ARL`, `ASL`
|
||||
|
||||
如果设备名无法判断,客户 App 需要让用户手动选择左/右侧。
|
||||
|
||||
## 3. 力控参数帧格式
|
||||
|
||||
设置力控参数使用 JSON 字符串,通过 TX characteristic 写入。
|
||||
|
||||
帧格式:
|
||||
|
||||
```text
|
||||
[0] key
|
||||
[1] length
|
||||
[2] crc
|
||||
[3] command
|
||||
[4...] utf8(json)
|
||||
```
|
||||
|
||||
字段说明:
|
||||
|
||||
| 字段 | 说明 |
|
||||
| --- | --- |
|
||||
| `key` | 左侧 `0x6F`,右侧 `0xAF` |
|
||||
| `length` | 整帧长度,即 `json_payload_len + 4` |
|
||||
| `crc` | 固定填 `0x00` |
|
||||
| `command` | 设置参数固定填 `0x01` |
|
||||
| `json` | UTF-8 编码的 JSON 参数 |
|
||||
|
||||
也就是:
|
||||
|
||||
```text
|
||||
payload = [0x01] + UTF8(json_string)
|
||||
frame = [key, len(payload) + 3, 0x00] + payload
|
||||
```
|
||||
|
||||
## 4. 设置力控模式和参数
|
||||
|
||||
### 力控参数表
|
||||
|
||||
所有 key 都定义在 `TuningParams::RegisterForceModeCtrl()` 中。
|
||||
|
||||
| JSON key | 默认值 | 单位 | 说明 |
|
||||
| --- | ---: | --- | --- |
|
||||
| `fm_mode` | `0.0` | - | 力控模式选择,四舍五入后映射到模式枚举 |
|
||||
| `fm_mset` | `0.1` | kg | 用户设定重量 |
|
||||
| `fm_kin` | `0.0` | - | 惯性比例系数,虚拟质量 = `fm_kin * fm_mset` |
|
||||
| `fm_bfr` | `0.0` | N/(m/s) | 线性摩擦/阻尼系数 |
|
||||
| `fm_kecc` | `0.1` | - | 离心倍率,离心力 = `fm_kecc * 向心力` |
|
||||
| `fm_vth` | `0.3` | m/s | 离心/向心平滑切换速度阈值 |
|
||||
| `fm_cdrv` | `0.1` | N/(m/s)^2 | 粘滞模式拉出方向平方阻尼系数 |
|
||||
| `fm_crec` | `0.1` | N/(m/s) | 粘滞模式回收方向线性阻尼系数 |
|
||||
| `fm_k` | `0.0` | N/m | 弹性刚度 |
|
||||
| `fm_x0` | `0.0` | m | 弹性零点位置 |
|
||||
| `fm_vmax` | `1.0` | m/s | 等速模式最大速度 |
|
||||
| `fm_gwall` | `0.0` | N/(m/s) | 等速速度墙增益 |
|
||||
| `fm_sp_v` | `0.1` | m/s | Spotter 触发速度阈值 |
|
||||
| `fm_sp_t` | `5.0` | s | Spotter 触发时间阈值 |
|
||||
| `fm_sp_d` | `5.0` | s | Spotter 减重衰减时长 |
|
||||
| `fm_sp_home` | `0.0` | m | Spotter 参考初始位置 |
|
||||
| `fm_sp_rng` | `-0.10` | m | Spotter 负重判定位置阈值 |
|
||||
| `fm_sp_rec` | `1.0` | s | Spotter 恢复时长;当前实现中会被同步为 `fm_sp_t` |
|
||||
| `fm_rad` | `0.2` | m | 滑轮半径,输出扭矩 = 输出力 * `fm_rad` |
|
||||
|
||||
### 力控模式
|
||||
|
||||
`fm_mode` 会先四舍五入,再限幅到 `0...6`。
|
||||
|
||||
| `fm_mode` | 模式 | 主要参数 | 说明 |
|
||||
| ---: | --- | --- | --- |
|
||||
| `0` | None | - | 输出力为 0 |
|
||||
| `1` | FreeWeight | `fm_mset`, `fm_kin`, `fm_bfr`, `fm_rad` | 自由重量/惯性补偿 |
|
||||
| `2` | EccentricOverload | `fm_mset`, `fm_kin`, `fm_bfr`, `fm_kecc`, `fm_vth`, Spotter 参数 | 离心超负荷 |
|
||||
| `3` | Viscous | `fm_cdrv`, `fm_crec`, `fm_rad` | 粘滞/流体阻尼 |
|
||||
| `4` | Elastic | `fm_k`, `fm_x0`, `fm_rad` | 弹性/弹簧 |
|
||||
| `5` | IsokineticSpotting | `fm_mset`, `fm_vmax`, `fm_gwall`, Spotter 参数 | 等速速度墙 |
|
||||
| `6` | Spotter | `fm_mset`, `fm_kin`, `fm_bfr`, Spotter 参数 | 保护模式 |
|
||||
|
||||
## 5. 示例
|
||||
|
||||
设置右侧设备为离心超负荷模式:
|
||||
|
||||
```json
|
||||
{
|
||||
"fm_mode": 2,
|
||||
"fm_mset": 10.0,
|
||||
"fm_vth": 0.3,
|
||||
"fm_kecc": 1.5,
|
||||
"fm_kin": 1.0,
|
||||
"fm_bfr": 0.05,
|
||||
"fm_rad": 0.04
|
||||
}
|
||||
```
|
||||
|
||||
对应写入帧:
|
||||
|
||||
```text
|
||||
key = 0xAF
|
||||
command = 0x01
|
||||
json_string = '{"fm_mode":2,"fm_mset":10.0,"fm_vth":0.3,"fm_kecc":1.5,"fm_kin":1.0,"fm_bfr":0.05,"fm_rad":0.04}'
|
||||
|
||||
payload = [0x01] + UTF8(json_string)
|
||||
frame = [0xAF, len(payload) + 3, 0x00] + payload
|
||||
```
|
||||
|
||||
写入 TX characteristic UUID:
|
||||
|
||||
```text
|
||||
6e400002-b5a3-f393-e0a9-e50e24dcca9e
|
||||
```
|
||||
|
||||
## 6. 查询当前力控参数
|
||||
|
||||
查询参数同样写 TX characteristic。
|
||||
|
||||
查询当前模式参数:
|
||||
|
||||
```text
|
||||
frame = [key, 0x04, 0x00, 0x02]
|
||||
```
|
||||
|
||||
查询指定模式参数,例如查询 `fm_mode = 2`:
|
||||
|
||||
```text
|
||||
payload = [0x02] + UTF8('{"fm_mode":2}')
|
||||
frame = [key, len(payload) + 3, 0x00] + payload
|
||||
```
|
||||
|
||||
设备会通过 RX characteristic notify 返回 JSON 参数。返回可能分包,需要按包序号拼接:
|
||||
|
||||
```text
|
||||
[0] key
|
||||
[1] length
|
||||
[2] crc
|
||||
[3] type = 0x02
|
||||
[4] packet_index
|
||||
[5] packet_count
|
||||
[6...] utf8(json_fragment)
|
||||
```
|
||||
|
||||
当 `packet_index == packet_count` 时,说明最后一包已收到,可以拼接所有 `json_fragment` 后解析 JSON。
|
||||
|
||||
## 7. 心跳包
|
||||
当连接上设备之后,需要每隔2s发送心跳包给设备,维持和设备的连接
|
||||
```text
|
||||
[0] key
|
||||
[1] length
|
||||
[2] crc
|
||||
[3] command = 0x00
|
||||
```
|
||||
## 8. 注意事项
|
||||
|
||||
- 写入前必须先连接设备并开启 RX notify。
|
||||
- 写入 key 必须和设备侧别一致:左侧 `0x6F`,右侧 `0xAF`。
|
||||
- `crc` 当前固定填 `0x00`。
|
||||
- JSON 使用 UTF-8 编码。
|
||||
@@ -1,15 +0,0 @@
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
|
||||
/* sensor.c 切换通道时需要此寄存器地址 */
|
||||
#define ADS1256_REG_MUX 0x01
|
||||
|
||||
int ads1256_init(void);
|
||||
int ads1256_wait_drdy(uint16_t timeout_ms);
|
||||
void ads1256_write_reg(uint8_t reg, uint8_t val);
|
||||
uint8_t ads1256_read_reg(uint8_t reg);
|
||||
void ads1256_sync_wakeup(void);
|
||||
int ads1256_hwreset(void);
|
||||
int32_t ads1256_read_data(void);
|
||||
void ads1256_write_cmd(uint8_t cmd);
|
||||
int ads1256_recover(int max_retries);
|
||||
@@ -1,40 +0,0 @@
|
||||
#pragma once
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/** @brief BLE 原始数据接收回调类型。 */
|
||||
typedef void (*ble_rx_cb_t)(const uint8_t *data, uint16_t len);
|
||||
|
||||
/**
|
||||
* @brief 初始化 BLE 协议栈和 NUS 服务。
|
||||
*
|
||||
* @retval 0 成功。
|
||||
* @retval 负值 初始化错误码。
|
||||
*/
|
||||
int ble_transport_init(void);
|
||||
|
||||
/** @brief 启动 BLE 可连接广播。 */
|
||||
void ble_transport_adv_start(void);
|
||||
|
||||
/**
|
||||
* @brief 通过 NUS 发送二进制数据,内部自动重试最多 3 次。
|
||||
*
|
||||
* @param data 待发送字节缓冲区。
|
||||
* @param len 字节数。
|
||||
*/
|
||||
void ble_transport_send(const uint8_t *data, uint16_t len);
|
||||
|
||||
/**
|
||||
* @brief 检查 BLE 连接是否就绪(已连接且通知已使能)。
|
||||
*
|
||||
* @retval true 可发送数据。
|
||||
* @retval false 未就绪。
|
||||
*/
|
||||
bool ble_transport_is_ready(void);
|
||||
|
||||
/**
|
||||
* @brief 注册上层原始数据接收回调,NUS 收到数据时透传调用。
|
||||
*
|
||||
* @param cb 回调函数指针。
|
||||
*/
|
||||
void ble_transport_register_rx_cb(ble_rx_cb_t cb);
|
||||
@@ -1,13 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
int button_init(void);
|
||||
|
||||
/**
|
||||
* @brief EMI 自恢复事件指示:点亮 LED1 并在 3 秒后自动熄灭。
|
||||
*
|
||||
* 复用按键反馈所用的 LED1,外部子系统(如 sensor)在检测到 ADS1256
|
||||
* 寄存器被 EMI 干扰并完成恢复后调用,给出可见的故障告警。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
void button_led_indicate_emi(void);
|
||||
@@ -1,124 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/* ─── 板面几何 & 传感器物理常量 ─── */
|
||||
|
||||
#define BOARD_HALF_WIDTH_CM 42.5f /* 传感器到板中心 X 方向距离 (cm) */
|
||||
#define BOARD_HALF_LENGTH_CM 16.0f /* 传感器到板中心 Y 方向距离 (cm) */
|
||||
|
||||
/*
|
||||
* 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
|
||||
*/
|
||||
#define SENSOR_EXCITATION_V 5.0f /* 传感器激励电压 (V) */
|
||||
#define SENSOR_SENSITIVITY_MVV 1.75f /* 传感器灵敏度 (mV/V) */
|
||||
#define SENSOR_RATED_LOAD_KG 50.0f /* 传感器满量程 (kg) */
|
||||
#define ADS1256_VREF_V 2.5f /* ADS1256 参考电压 (V) */
|
||||
#define ADS1256_PGA 64 /* ADS1256 增益倍数 */
|
||||
|
||||
/* 满量程时的 ADC 计数值 */
|
||||
#define ADC_COUNTS_AT_RATED \
|
||||
((SENSOR_SENSITIVITY_MVV * SENSOR_EXCITATION_V) / (2.0f * ADS1256_VREF_V * 1000.0f / ADS1256_PGA) * 8388607.0f)
|
||||
/* 1 LSB 对应的力 (kg/count) */
|
||||
#define ADC_TO_FORCE_SCALE (SENSOR_RATED_LOAD_KG / ADC_COUNTS_AT_RATED)
|
||||
|
||||
/* CoP 有效判定:迟滞阈值,防止边界抖动 */
|
||||
#define COP_FORCE_ENTER_THRESHOLD 3.0f /* 总力超过此值才判定有人 (kg) */
|
||||
#define COP_FORCE_EXIT_THRESHOLD 1.0f /* 总力低于此值才判定离开 (kg) */
|
||||
|
||||
/*
|
||||
* 通道级二阶 Butterworth 低通:fc=20Hz, fs=100Hz
|
||||
* 10Hz 通过 98%,20Hz -3dB,25Hz -6.6dB
|
||||
* 保留 8-10Hz 振动板信号,同时为 50Hz 输出抗混叠
|
||||
*/
|
||||
#define LPF_B0 0.2065720838f
|
||||
#define LPF_B1 0.4131441677f
|
||||
#define LPF_B2 0.2065720838f
|
||||
#define LPF_A1 (-0.3695273774f)
|
||||
#define LPF_A2 0.1958157127f
|
||||
|
||||
/* ─── 标定常量 ─── */
|
||||
|
||||
#define CAL_NUM_CHANNELS 4 /* 差分通道数 */
|
||||
#define CAL_NUM_GRID_PTS 9 /* L2 网格标定点数 (3×3) */
|
||||
|
||||
/* ─── 标定运行时数据(sensor 线程只读) ─── */
|
||||
struct cal_runtime {
|
||||
int32_t zero[CAL_NUM_CHANNELS];
|
||||
float gain[CAL_NUM_CHANNELS]; /* kg/count */
|
||||
bool l1_zero_valid;
|
||||
bool l1_gain_valid;
|
||||
bool l2_valid;
|
||||
float grid_err_x[CAL_NUM_GRID_PTS]; /* CoP X 误差 (cm) */
|
||||
float grid_err_y[CAL_NUM_GRID_PTS]; /* CoP Y 误差 (cm) */
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 初始化标定模块,从 NVS 加载持久化数据或使用默认值。
|
||||
*
|
||||
* 必须在 settings_load() 之后、sensor 线程启动之前调用。
|
||||
*
|
||||
* @retval 0 成功。
|
||||
*/
|
||||
int cal_init(void);
|
||||
|
||||
/**
|
||||
* @brief 获取当前生效的标定数据指针。
|
||||
*
|
||||
* sensor 线程在每帧开始时调用 cal_check_update() 后,通过本函数
|
||||
* 拿到稳定的数据指针用于当帧计算。
|
||||
*
|
||||
* @return 指向内部 working copy 的只读指针。
|
||||
*/
|
||||
const struct cal_runtime *cal_get_working(void);
|
||||
|
||||
/**
|
||||
* @brief 检查标定数据是否有更新,若有则刷新 working copy。
|
||||
*
|
||||
* sensor 线程在每帧循环顶部调用。
|
||||
*
|
||||
* @retval true 数据已刷新。
|
||||
* @retval false 无更新。
|
||||
*/
|
||||
bool cal_check_update(void);
|
||||
|
||||
/**
|
||||
* @brief 入队一条来自 BLE 的标定命令。
|
||||
*
|
||||
* 从 BLE 回调上下文调用,不做 ADC 操作,仅存储命令参数并设置 pending 标志。
|
||||
*
|
||||
* @param subcmd 子命令码。
|
||||
* @param target 通道号 (0-3) 或网格点号 (0-8),或 0xFF 表示全部。
|
||||
* @param param 浮点参数(如已知质量 kg),无参数时为 0。
|
||||
*
|
||||
* @retval 0 命令已入队。
|
||||
* @retval -EBUSY 上一条命令尚未被 sensor 线程消费。
|
||||
* @retval -EINVAL 状态机拒绝该命令。
|
||||
*/
|
||||
int cal_enqueue_command(uint8_t subcmd, uint8_t target, float param);
|
||||
|
||||
/**
|
||||
* @brief 在 sensor 线程中执行待处理的标定命令。
|
||||
*
|
||||
* 若存在 pending 命令,执行 ADC 测量、更新状态机、通过 BLE 发送响应。
|
||||
* 调用者应在本函数返回 true 时跳过本帧的正常采集。
|
||||
*
|
||||
* @retval true 执行了标定命令(本帧不做正常采集)。
|
||||
* @retval false 无待处理命令。
|
||||
*/
|
||||
bool cal_execute_pending(void);
|
||||
|
||||
/**
|
||||
* @brief 对计算出的 CoP 坐标施加 L2 网格补偿。
|
||||
*
|
||||
* 仅在 L2 标定有效时进行修正,否则不改变输入值。
|
||||
*
|
||||
* @param[in,out] cop_x CoP X 坐标 (cm)。
|
||||
* @param[in,out] cop_y CoP Y 坐标 (cm)。
|
||||
*/
|
||||
void cal_apply_l2_correction(float *cop_x, float *cop_y);
|
||||
@@ -1,235 +0,0 @@
|
||||
#pragma once
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <zephyr/kernel.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 /* 下行: 心跳包 */
|
||||
#define PROTO_TYPE_CAL_CMD 0x30U /* 下行: 标定命令 */
|
||||
#define PROTO_TYPE_CAL_RESP 0x31U /* 上行: 标定响应 */
|
||||
|
||||
/* CoP flags 位定义 */
|
||||
#define COP_FLAG_FORCE_VALID 0x01U /* 总力 > 阈值,CoP 坐标有意义 */
|
||||
|
||||
/* ═══════════════════════════════════════════════════════════════════
|
||||
* 帧结构定义
|
||||
* ═══════════════════════════════════════════════════════════════════ */
|
||||
|
||||
/* ─── 上行: 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;
|
||||
uint8_t sync1;
|
||||
uint8_t type;
|
||||
uint8_t flags;
|
||||
int16_t cop_x;
|
||||
int16_t cop_y;
|
||||
int16_t force;
|
||||
uint8_t crc;
|
||||
} __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;
|
||||
uint8_t sync1;
|
||||
uint8_t type;
|
||||
uint8_t flags;
|
||||
float cable_length;
|
||||
float velocity;
|
||||
uint8_t crc;
|
||||
} __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;
|
||||
uint8_t sync1;
|
||||
uint8_t type;
|
||||
float K;
|
||||
float B;
|
||||
float Tau;
|
||||
uint8_t crc;
|
||||
} __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;
|
||||
uint8_t sync1;
|
||||
uint8_t type;
|
||||
float threshold;
|
||||
uint8_t enable;
|
||||
uint8_t crc;
|
||||
} __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;
|
||||
uint8_t sync1;
|
||||
uint8_t type;
|
||||
uint8_t counter;
|
||||
uint8_t crc;
|
||||
} __attribute__((packed));
|
||||
|
||||
union heartbeat_pkt_t {
|
||||
struct heartbeat_frame_t frame;
|
||||
uint8_t bytes[sizeof(struct heartbeat_frame_t)];
|
||||
};
|
||||
|
||||
/* ═══════════════════════════════════════════════════════════════════
|
||||
* 标定子命令码 & 状态码
|
||||
* ═══════════════════════════════════════════════════════════════════ */
|
||||
|
||||
#define CAL_SUBCMD_START_L1 0x01U
|
||||
#define CAL_SUBCMD_TARE_CH 0x02U
|
||||
#define CAL_SUBCMD_GAIN_CH 0x03U
|
||||
#define CAL_SUBCMD_COMMIT_L1 0x04U
|
||||
#define CAL_SUBCMD_ABORT 0x05U
|
||||
#define CAL_SUBCMD_START_L2 0x10U
|
||||
#define CAL_SUBCMD_RECORD_GRID 0x11U
|
||||
#define CAL_SUBCMD_COMMIT_L2 0x12U
|
||||
#define CAL_SUBCMD_ERASE 0x20U
|
||||
#define CAL_SUBCMD_QUERY 0x21U
|
||||
|
||||
#define CAL_STATUS_OK 0x00U
|
||||
#define CAL_STATUS_ERR_STATE 0x01U
|
||||
#define CAL_STATUS_ERR_NVS 0x02U
|
||||
#define CAL_STATUS_ERR_PARAM 0x03U
|
||||
|
||||
/* ─── 下行: 标定命令帧 (10 bytes) ───
|
||||
* [AA 55 30 subcmd target param(4) crc]
|
||||
*/
|
||||
struct cal_cmd_frame_t {
|
||||
uint8_t sync0;
|
||||
uint8_t sync1;
|
||||
uint8_t type;
|
||||
uint8_t subcmd;
|
||||
uint8_t target;
|
||||
float param;
|
||||
uint8_t crc;
|
||||
} __attribute__((packed));
|
||||
|
||||
union cal_cmd_pkt_t {
|
||||
struct cal_cmd_frame_t frame;
|
||||
uint8_t bytes[sizeof(struct cal_cmd_frame_t)];
|
||||
};
|
||||
|
||||
/* ─── 上行: 标定响应帧 (14 bytes) ───
|
||||
* [AA 55 31 status subcmd data(8) crc]
|
||||
*/
|
||||
struct cal_resp_frame_t {
|
||||
uint8_t sync0;
|
||||
uint8_t sync1;
|
||||
uint8_t type;
|
||||
uint8_t status;
|
||||
uint8_t subcmd;
|
||||
uint8_t data[8];
|
||||
uint8_t crc;
|
||||
} __attribute__((packed));
|
||||
|
||||
union cal_resp_pkt_t {
|
||||
struct cal_resp_frame_t frame;
|
||||
uint8_t bytes[sizeof(struct cal_resp_frame_t)];
|
||||
};
|
||||
|
||||
/* ═══════════════════════════════════════════════════════════════════
|
||||
* 下行消息队列 payload
|
||||
* ═══════════════════════════════════════════════════════════════════ */
|
||||
|
||||
struct comm_msg_resistance {
|
||||
float K, B, Tau;
|
||||
};
|
||||
|
||||
struct comm_msg_spotter {
|
||||
float threshold;
|
||||
bool enable;
|
||||
};
|
||||
|
||||
struct comm_msg_heartbeat {
|
||||
uint8_t counter;
|
||||
};
|
||||
|
||||
enum comm_msg_type {
|
||||
COMM_MSG_RESISTANCE,
|
||||
COMM_MSG_SPOTTER,
|
||||
COMM_MSG_HEARTBEAT,
|
||||
};
|
||||
|
||||
struct comm_msg {
|
||||
enum comm_msg_type type;
|
||||
union {
|
||||
struct comm_msg_resistance resistance;
|
||||
struct comm_msg_spotter spotter;
|
||||
struct comm_msg_heartbeat heartbeat;
|
||||
};
|
||||
};
|
||||
|
||||
/* ═══════════════════════════════════════════════════════════════════
|
||||
* 协议层 API
|
||||
* ═══════════════════════════════════════════════════════════════════ */
|
||||
|
||||
/**
|
||||
* @brief 初始化协议层,注册 BLE 传输层收数据回调。
|
||||
*
|
||||
* 必须在 ble_transport_init() 之后调用。
|
||||
*
|
||||
* @retval 0 成功。
|
||||
*/
|
||||
int comm_protocol_init(void);
|
||||
|
||||
/**
|
||||
* @brief 组包并发送 CoP 上行帧。
|
||||
*
|
||||
* @param flags COP_FLAG_* 位组合。
|
||||
* @param cop_x 压力中心 X (cm)。
|
||||
* @param cop_y 压力中心 Y (cm)。
|
||||
* @param force 总力 (kg)。
|
||||
*/
|
||||
void comm_protocol_send_cop(uint8_t flags, int16_t cop_x, int16_t cop_y, int16_t force);
|
||||
|
||||
/**
|
||||
* @brief 组包并发送标定响应上行帧。
|
||||
*
|
||||
* @param status 结果状态码 (CAL_STATUS_*)。
|
||||
* @param subcmd 对应的子命令回显。
|
||||
* @param data 8 字节响应数据。
|
||||
*/
|
||||
void comm_protocol_send_cal_resp(uint8_t status, uint8_t subcmd, const uint8_t data[8]);
|
||||
@@ -1,4 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
int sensor_init(void);
|
||||
void sensor_perform_tare(void);
|
||||
@@ -1,49 +0,0 @@
|
||||
# 基础蓝牙
|
||||
CONFIG_BT=y
|
||||
CONFIG_BT_PERIPHERAL=y
|
||||
CONFIG_BT_DEVICE_NAME="GML670_System"
|
||||
CONFIG_BT_MAX_CONN=1
|
||||
CONFIG_BT_NUS=y
|
||||
|
||||
# DLE + PHY
|
||||
CONFIG_BT_BUF_ACL_RX_SIZE=251
|
||||
CONFIG_BT_BUF_ACL_TX_SIZE=251
|
||||
CONFIG_BT_DATA_LEN_UPDATE=y
|
||||
CONFIG_BT_USER_DATA_LEN_UPDATE=y
|
||||
CONFIG_BT_AUTO_DATA_LEN_UPDATE=y
|
||||
CONFIG_BT_L2CAP_TX_MTU=247
|
||||
CONFIG_BT_PHY_UPDATE=y
|
||||
CONFIG_BT_USER_PHY_UPDATE=y
|
||||
CONFIG_BT_AUTO_PHY_UPDATE=y
|
||||
|
||||
# 缓冲区
|
||||
CONFIG_BT_BUF_ACL_TX_COUNT=3
|
||||
CONFIG_BT_L2CAP_TX_BUF_COUNT=6
|
||||
|
||||
# 存储
|
||||
CONFIG_FLASH=y
|
||||
CONFIG_FLASH_MAP=y
|
||||
CONFIG_NVS=y
|
||||
CONFIG_SETTINGS=y
|
||||
CONFIG_SETTINGS_NVS=y
|
||||
|
||||
# 硬件驱动
|
||||
CONFIG_SPI=y
|
||||
CONFIG_GPIO=y
|
||||
CONFIG_CBPRINTF_FP_SUPPORT=y
|
||||
|
||||
# 日志 (切换到 RTT,避免日志占用 UART/Console 输出链)
|
||||
CONFIG_CONSOLE=n
|
||||
CONFIG_UART_CONSOLE=n
|
||||
CONFIG_SERIAL=n
|
||||
CONFIG_LOG=y
|
||||
CONFIG_USE_SEGGER_RTT=y
|
||||
CONFIG_LOG_BACKEND_RTT=y
|
||||
CONFIG_LOG_BACKEND_RTT_MODE_DROP=y
|
||||
CONFIG_LOG_BACKEND_UART=n
|
||||
CONFIG_LOG_MODE_DEFERRED=y
|
||||
CONFIG_LOG_BUFFER_SIZE=4096
|
||||
CONFIG_SEGGER_RTT_BUFFER_SIZE_UP=4096
|
||||
CONFIG_MAIN_STACK_SIZE=4096
|
||||
|
||||
CONFIG_CRC=y
|
||||
-366
@@ -1,366 +0,0 @@
|
||||
#include "ads1256.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/drivers/spi.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
|
||||
LOG_MODULE_REGISTER(ads1256, LOG_LEVEL_INF);
|
||||
|
||||
/* --- ADS1256 命令 --- */
|
||||
#define CMD_WAKEUP 0x00
|
||||
#define CMD_RDATA 0x01
|
||||
#define CMD_RREG 0x10
|
||||
#define CMD_WREG 0x50
|
||||
#define CMD_SELFCAL 0xF0
|
||||
#define CMD_SYNC 0xFC
|
||||
#define CMD_SDATAC 0x0F
|
||||
|
||||
/* --- ADS1256 寄存器地址 --- */
|
||||
#define REG_STATUS 0x00
|
||||
#define REG_MUX 0x01
|
||||
#define REG_ADCON 0x02
|
||||
#define REG_DRATE 0x03
|
||||
#define REG_IO 0x04
|
||||
#define REG_OFC0 0x05
|
||||
#define REG_OFC1 0x06
|
||||
#define REG_OFC2 0x07
|
||||
#define REG_FSC0 0x08
|
||||
#define REG_FSC1 0x09
|
||||
#define REG_FSC2 0x0A
|
||||
|
||||
#define SPI_OP (SPI_OP_MODE_MASTER | SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_LINES_SINGLE)
|
||||
|
||||
/* --- 硬件资源 --- */
|
||||
static struct spi_config spi_cfg;
|
||||
static const struct device *spi_dev;
|
||||
static const struct gpio_dt_spec cs_spec = SPI_CS_GPIOS_DT_SPEC_GET(DT_NODELABEL(ads1256));
|
||||
static const struct gpio_dt_spec drdy_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_drdy), gpios);
|
||||
static const struct gpio_dt_spec reset_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_reset), gpios);
|
||||
static const struct gpio_dt_spec pdwn_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_pdwn), gpios);
|
||||
|
||||
struct ads1256_reg_snapshot {
|
||||
uint8_t status;
|
||||
uint8_t mux;
|
||||
uint8_t adcon;
|
||||
uint8_t drate;
|
||||
uint8_t io;
|
||||
uint8_t ofc0;
|
||||
uint8_t ofc1;
|
||||
uint8_t ofc2;
|
||||
uint8_t fsc0;
|
||||
uint8_t fsc1;
|
||||
uint8_t fsc2;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 等待 DRDY 拉低(转换结果可读),带超时保护。
|
||||
*
|
||||
* @param timeout_ms 超时时间(毫秒)。
|
||||
*
|
||||
* @retval 0 DRDY 在超时前变为可读状态。
|
||||
* @retval -ETIMEDOUT 超时后仍未等到本次转换结果。
|
||||
*/
|
||||
int ads1256_wait_drdy(uint16_t timeout_ms) {
|
||||
/* drdy_spec 配了 GPIO_ACTIVE_LOW,逻辑 1 表示 DRDY 有效(物理拉低) */
|
||||
int64_t deadline = k_uptime_get() + timeout_ms;
|
||||
while (gpio_pin_get_dt(&drdy_spec) == 0) {
|
||||
k_usleep(10);
|
||||
if (k_uptime_get() >= deadline) {
|
||||
LOG_WRN("DRDY timeout (%u ms)", timeout_ms);
|
||||
return -ETIMEDOUT;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 向 ADS1256 写单个寄存器。
|
||||
*
|
||||
* @param reg 目标寄存器地址。
|
||||
* @param val 要写入的寄存器值。
|
||||
*/
|
||||
void ads1256_write_reg(uint8_t reg, uint8_t val) {
|
||||
ads1256_wait_drdy(50);
|
||||
uint8_t tx_buf[3] = { CMD_WREG | reg, 0x00, val };
|
||||
struct spi_buf tx = { .buf = tx_buf, .len = 3 };
|
||||
struct spi_buf_set tx_set = { .buffers = &tx, .count = 1 };
|
||||
spi_write(spi_dev, &spi_cfg, &tx_set);
|
||||
/* t11: WREG 后至少 4 × tCLKIN ≈ 0.5µs */
|
||||
k_busy_wait(2);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从 ADS1256 读单个寄存器。
|
||||
*
|
||||
* ADS1256 RREG 时序要求:命令+数据在同一次 CS 拉低内完成,
|
||||
* 中间需 t6 延时。这里用单次 transceive 保证 CS 不释放。
|
||||
*
|
||||
* @param reg 目标寄存器地址。
|
||||
*
|
||||
* @return 读取到的寄存器值。
|
||||
*/
|
||||
uint8_t ads1256_read_reg(uint8_t reg) {
|
||||
ads1256_wait_drdy(50);
|
||||
/* tx: [RREG|reg, 0x00, dummy_for_t6, dummy_read]
|
||||
* rx: [x, x, x, data] — 前 3 字节是命令+延时期间的垃圾 */
|
||||
uint8_t tx_buf[4] = { CMD_RREG | reg, 0x00, 0xFF, 0xFF };
|
||||
uint8_t rx_buf[4] = { 0 };
|
||||
struct spi_buf tx = { .buf = tx_buf, .len = 4 };
|
||||
struct spi_buf_set tx_set = { .buffers = &tx, .count = 1 };
|
||||
struct spi_buf rx = { .buf = rx_buf, .len = 4 };
|
||||
struct spi_buf_set rx_set = { .buffers = &rx, .count = 1 };
|
||||
spi_transceive(spi_dev, &spi_cfg, &tx_set, &rx_set);
|
||||
return rx_buf[3];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 连续读取 ADS1256 关键寄存器,形成一次诊断快照。
|
||||
*
|
||||
* @param snapshot 输出快照对象,不能为空。
|
||||
*/
|
||||
static void ads1256_read_snapshot(struct ads1256_reg_snapshot *snapshot) {
|
||||
snapshot->status = ads1256_read_reg(REG_STATUS);
|
||||
snapshot->mux = ads1256_read_reg(REG_MUX);
|
||||
snapshot->adcon = ads1256_read_reg(REG_ADCON);
|
||||
snapshot->drate = ads1256_read_reg(REG_DRATE);
|
||||
snapshot->io = ads1256_read_reg(REG_IO);
|
||||
snapshot->ofc0 = ads1256_read_reg(REG_OFC0);
|
||||
snapshot->ofc1 = ads1256_read_reg(REG_OFC1);
|
||||
snapshot->ofc2 = ads1256_read_reg(REG_OFC2);
|
||||
snapshot->fsc0 = ads1256_read_reg(REG_FSC0);
|
||||
snapshot->fsc1 = ads1256_read_reg(REG_FSC1);
|
||||
snapshot->fsc2 = ads1256_read_reg(REG_FSC2);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 仅打印恢复前后发生变化的寄存器。
|
||||
*
|
||||
* @param tag 日志标签,用于区分恢复成功与失败后的变化。
|
||||
* @param before 恢复前寄存器快照。
|
||||
* @param after 恢复后寄存器快照。
|
||||
*/
|
||||
static void ads1256_log_snapshot_delta(
|
||||
const char *tag,
|
||||
const struct ads1256_reg_snapshot *before,
|
||||
const struct ads1256_reg_snapshot *after) {
|
||||
if (before->status != after->status) {
|
||||
LOG_WRN("%s STATUS: 0x%02X -> 0x%02X", tag, before->status, after->status);
|
||||
}
|
||||
if (before->mux != after->mux) {
|
||||
LOG_WRN("%s MUX: 0x%02X -> 0x%02X", tag, before->mux, after->mux);
|
||||
}
|
||||
if (before->adcon != after->adcon) {
|
||||
LOG_WRN("%s ADCON: 0x%02X -> 0x%02X", tag, before->adcon, after->adcon);
|
||||
}
|
||||
if (before->drate != after->drate) {
|
||||
LOG_WRN("%s DRATE: 0x%02X -> 0x%02X", tag, before->drate, after->drate);
|
||||
}
|
||||
if (before->io != after->io) {
|
||||
LOG_WRN("%s IO: 0x%02X -> 0x%02X", tag, before->io, after->io);
|
||||
}
|
||||
if (before->ofc0 != after->ofc0) {
|
||||
LOG_WRN("%s OFC0: 0x%02X -> 0x%02X", tag, before->ofc0, after->ofc0);
|
||||
}
|
||||
if (before->ofc1 != after->ofc1) {
|
||||
LOG_WRN("%s OFC1: 0x%02X -> 0x%02X", tag, before->ofc1, after->ofc1);
|
||||
}
|
||||
if (before->ofc2 != after->ofc2) {
|
||||
LOG_WRN("%s OFC2: 0x%02X -> 0x%02X", tag, before->ofc2, after->ofc2);
|
||||
}
|
||||
if (before->fsc0 != after->fsc0) {
|
||||
LOG_WRN("%s FSC0: 0x%02X -> 0x%02X", tag, before->fsc0, after->fsc0);
|
||||
}
|
||||
if (before->fsc1 != after->fsc1) {
|
||||
LOG_WRN("%s FSC1: 0x%02X -> 0x%02X", tag, before->fsc1, after->fsc1);
|
||||
}
|
||||
if (before->fsc2 != after->fsc2) {
|
||||
LOG_WRN("%s FSC2: 0x%02X -> 0x%02X", tag, before->fsc2, after->fsc2);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送单字节命令到 ADS1256。
|
||||
*
|
||||
* @param cmd 命令字。
|
||||
*/
|
||||
void ads1256_write_cmd(uint8_t cmd) {
|
||||
ads1256_wait_drdy(50);
|
||||
struct spi_buf tx = { .buf = &cmd, .len = 1 };
|
||||
struct spi_buf_set tx_set = { .buffers = &tx, .count = 1 };
|
||||
spi_write(spi_dev, &spi_cfg, &tx_set);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送 SYNC + WAKEUP 命令,触发一次同步采样。
|
||||
*
|
||||
* SYNC 和 WAKEUP 必须在同一次 CS 内完成,用单次 transceive 保持 CS。
|
||||
* 中间插入 dummy 字节满足 t11 延时 (24 × tCLKIN ≈ 3.1µs)。
|
||||
*/
|
||||
void ads1256_sync_wakeup(void) {
|
||||
/* [SYNC, dummy(延时), WAKEUP] — 500kHz 下每字节 16µs,1 字节 dummy 远超 3.1µs */
|
||||
uint8_t tx_buf[3] = { CMD_SYNC, 0xFF, CMD_WAKEUP };
|
||||
struct spi_buf tx = { .buf = tx_buf, .len = 3 };
|
||||
struct spi_buf_set tx_set = { .buffers = &tx, .count = 1 };
|
||||
spi_write(spi_dev, &spi_cfg, &tx_set);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 硬件复位 ADS1256(通过 RESET 引脚),等待复位后自校准完成。
|
||||
*
|
||||
* @retval 0 芯片完成复位并重新进入可通信状态。
|
||||
* @retval -ETIMEDOUT 复位后等待 DRDY 超时,芯片未按预期响应。
|
||||
*/
|
||||
int ads1256_hwreset(void) {
|
||||
/* reset_spec 配了 GPIO_ACTIVE_LOW:逻辑 1 = 物理 LOW = 断言复位 */
|
||||
gpio_pin_set_dt(&reset_spec, 1);
|
||||
/* t16: 最小复位脉宽 4 × tCLKIN ≈ 0.5µs,取 1ms 余量 */
|
||||
k_msleep(10);
|
||||
gpio_pin_set_dt(&reset_spec, 0);
|
||||
/* 复位释放后芯片执行自校准,等待完成 */
|
||||
k_msleep(10);
|
||||
return ads1256_wait_drdy(500);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读取当前 24 位转换结果,符号扩展为 int32_t。
|
||||
*
|
||||
* RDATA 命令后需 t6 延时再读 3 字节数据,全部在同一次 CS 内完成。
|
||||
*
|
||||
* @return 当前 ADC 转换原始值。
|
||||
*/
|
||||
int32_t ads1256_read_data(void) {
|
||||
/* tx: [RDATA, dummy(t6延时), MSB, MID, LSB]
|
||||
* rx: [x, x, MSB, MID, LSB] */
|
||||
uint8_t tx_buf[5] = { CMD_RDATA, 0xFF, 0xFF, 0xFF, 0xFF };
|
||||
uint8_t rx_buf[5] = { 0 };
|
||||
struct spi_buf tx = { .buf = tx_buf, .len = 5 };
|
||||
struct spi_buf_set tx_set = { .buffers = &tx, .count = 1 };
|
||||
struct spi_buf rx = { .buf = rx_buf, .len = 5 };
|
||||
struct spi_buf_set rx_set = { .buffers = &rx, .count = 1 };
|
||||
spi_transceive(spi_dev, &spi_cfg, &tx_set, &rx_set);
|
||||
int32_t val = ((int32_t)rx_buf[2] << 16) | ((int32_t)rx_buf[3] << 8) | rx_buf[4];
|
||||
if (val & 0x800000) val |= 0xFF000000;
|
||||
return -val;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化 ADS1256:配置 GPIO/SPI,复位芯片,写入寄存器,自校准。
|
||||
*
|
||||
* @retval 0 初始化成功,芯片参数已经写入并校验通过。
|
||||
* @retval -ENODEV SPI 控制器未就绪,无法访问 ADS1256。
|
||||
* @retval -ETIMEDOUT 复位或校准等待阶段未收到芯片响应。
|
||||
* @retval -EIO 寄存器回读校验失败,SPI 通信结果不可信。
|
||||
*/
|
||||
int ads1256_init(void) {
|
||||
/* GPIO */
|
||||
/* PDWN 低有效:配为 INACTIVE(物理高电平) 保持芯片正常运行 */
|
||||
gpio_pin_configure_dt(&pdwn_spec, GPIO_OUTPUT_INACTIVE);
|
||||
gpio_pin_configure_dt(&reset_spec, GPIO_OUTPUT_INACTIVE);
|
||||
gpio_pin_configure_dt(&drdy_spec, GPIO_INPUT);
|
||||
|
||||
/* SPI — CS 由 SPIM 驱动自动管理 */
|
||||
spi_dev = DEVICE_DT_GET(DT_NODELABEL(spi1));
|
||||
spi_cfg.operation = SPI_OP;
|
||||
spi_cfg.frequency = 500000;
|
||||
spi_cfg.slave = 0;
|
||||
spi_cfg.cs.gpio = cs_spec;
|
||||
spi_cfg.cs.delay = 0;
|
||||
|
||||
if (!device_is_ready(spi_dev)) {
|
||||
LOG_ERR("SPI device not ready");
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int ret = ads1256_hwreset();
|
||||
if (ret) {
|
||||
LOG_ERR("ADS1256 not detected (DRDY no response after reset)");
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* 关闭连续输出模式,进入命令模式 */
|
||||
ads1256_write_cmd(CMD_SDATAC);
|
||||
k_busy_wait(100);
|
||||
|
||||
/*
|
||||
* STATUS = 0x06: ORDER=MSB, ACAL=1(自动校准), BUFEN=1(开启输入缓冲)
|
||||
* GML670 50kg 传感器输出阻抗 1kΩ,Buffer OFF + PGA=64 时输入阻抗仅 ~8kΩ
|
||||
* (datasheet Table 10),导致 ~11% 负载误差。开 Buffer 后输入阻抗 >10MΩ。
|
||||
*/
|
||||
ads1256_write_reg(REG_STATUS, 0x06);
|
||||
ads1256_write_reg(REG_MUX, 0x01);
|
||||
/* ADCON: CLK_OUT=OFF, SDCS=OFF, PGA=64 */
|
||||
ads1256_write_reg(REG_ADCON, 0x07);
|
||||
/*
|
||||
* DRATE: 3750 SPS (0xC0)
|
||||
* 50Hz 循环 = 20ms 预算。4 通道各采 10 次均值:
|
||||
* 单通道 = t18(0.44ms) + 9 × 1/3750(0.27ms) = 2.87ms
|
||||
* 4 通道 = 11.5ms,余量 8.5ms
|
||||
* 相比 30kSPS: ENOB 从 16.5 → 17.4 bit (Buffer On, PGA=64)
|
||||
*/
|
||||
ads1256_write_reg(REG_DRATE, 0xC0);
|
||||
|
||||
/* 显式自校准:Buffer/PGA 变更后必须重新校准 (datasheet p25) */
|
||||
ads1256_write_cmd(CMD_SELFCAL);
|
||||
/* SELFCAL 完成后 DRDY 拉低 (datasheet: "Do not send additional commands
|
||||
after issuing this command until DRDY goes low") */
|
||||
ads1256_wait_drdy(500);
|
||||
|
||||
/* 回读关键寄存器验证 SPI 通信正确性 */
|
||||
uint8_t status = ads1256_read_reg(REG_STATUS);
|
||||
uint8_t adcon = ads1256_read_reg(REG_ADCON);
|
||||
uint8_t drate = ads1256_read_reg(REG_DRATE);
|
||||
LOG_INF("ADS1256 regs: STATUS=0x%02X ADCON=0x%02X DRATE=0x%02X", status, adcon, drate);
|
||||
|
||||
if (drate != 0xC0) {
|
||||
if (status == 0x00 && adcon == 0x00 && drate == 0x00) {
|
||||
LOG_ERR("ADS1256 SPI read failure (all regs 0x00) — check MISO wiring");
|
||||
} else if (status == 0xFF && adcon == 0xFF && drate == 0xFF) {
|
||||
LOG_ERR("ADS1256 SPI read failure (all regs 0xFF) — MISO floating or not connected");
|
||||
} else {
|
||||
LOG_ERR("ADS1256 register verify failed (DRATE=0x%02X, expected 0xC0)", drate);
|
||||
}
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
LOG_INF("ADS1256 initialized (BUFEN=1, PGA=64, 3750SPS)");
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief EMI 恢复:重写关键寄存器并验证,用于马达干扰后自恢复。
|
||||
*
|
||||
* @param max_retries 最大重试次数,每次间隔 20ms。
|
||||
*
|
||||
* @retval 0 恢复成功。
|
||||
* @retval -EIO 达到最大重试次数仍未恢复。
|
||||
*/
|
||||
int ads1256_recover(int max_retries) {
|
||||
struct ads1256_reg_snapshot before_snapshot;
|
||||
ads1256_read_snapshot(&before_snapshot);
|
||||
|
||||
for (int i = 0; i < max_retries; i++) {
|
||||
ads1256_write_reg(REG_STATUS, 0x06);
|
||||
ads1256_write_reg(REG_ADCON, 0x07);
|
||||
ads1256_write_reg(REG_DRATE, 0xC0);
|
||||
ads1256_sync_wakeup();
|
||||
k_msleep(20);
|
||||
|
||||
uint8_t adcon = ads1256_read_reg(REG_ADCON);
|
||||
uint8_t drate = ads1256_read_reg(REG_DRATE);
|
||||
if (adcon == 0x07 && drate == 0xC0) {
|
||||
struct ads1256_reg_snapshot after_snapshot;
|
||||
ads1256_read_snapshot(&after_snapshot);
|
||||
ads1256_log_snapshot_delta("ADS1256 recover reg change", &before_snapshot, &after_snapshot);
|
||||
LOG_WRN("ADS1256 recovered after %d retries", i + 1);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
struct ads1256_reg_snapshot failed_snapshot;
|
||||
ads1256_read_snapshot(&failed_snapshot);
|
||||
ads1256_log_snapshot_delta(
|
||||
"ADS1256 recover reg change after retries exhausted", &before_snapshot, &failed_snapshot);
|
||||
LOG_ERR("ADS1256 recovery failed after %d retries", max_retries);
|
||||
return -EIO;
|
||||
}
|
||||
@@ -1,144 +0,0 @@
|
||||
#include "ble_transport.h"
|
||||
|
||||
#include <bluetooth/services/nus.h>
|
||||
#include <zephyr/bluetooth/bluetooth.h>
|
||||
#include <zephyr/bluetooth/gatt.h>
|
||||
#include <zephyr/bluetooth/uuid.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/settings/settings.h>
|
||||
|
||||
LOG_MODULE_REGISTER(ble_transport, LOG_LEVEL_INF);
|
||||
|
||||
#define BT_UUID_NUS_VAL BT_UUID_128_ENCODE(0x6e400001, 0xb5a3, 0xf393, 0xe0a9, 0xe50e24dcca9e)
|
||||
|
||||
/* --- 广播数据 --- */
|
||||
static const struct bt_data ad[] = {
|
||||
BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
|
||||
BT_DATA_BYTES(BT_DATA_UUID128_ALL, BT_UUID_NUS_VAL),
|
||||
};
|
||||
static const struct bt_data sd[] = {
|
||||
BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
|
||||
};
|
||||
|
||||
/* --- 连接状态 --- */
|
||||
static struct bt_conn *current_conn;
|
||||
static volatile bool nus_notification_enabled;
|
||||
static struct k_work_delayable adv_work;
|
||||
|
||||
/* 上层收数据回调,必须在 ble_transport_adv_start() 前完成注册 */
|
||||
static ble_rx_cb_t rx_cb;
|
||||
|
||||
/** @brief 延时广播任务,断链后重新进入可连接状态。 */
|
||||
static void adv_work_handler(struct k_work *work) {
|
||||
ARG_UNUSED(work);
|
||||
ble_transport_adv_start();
|
||||
}
|
||||
|
||||
/** @brief 连接建立回调,接管连接引用并收紧连接参数。 */
|
||||
static void connected(struct bt_conn *conn, uint8_t err) {
|
||||
if (err) {
|
||||
LOG_WRN("Connection failed (err %u)", err);
|
||||
return;
|
||||
}
|
||||
|
||||
current_conn = bt_conn_ref(conn);
|
||||
k_work_cancel_delayable(&adv_work);
|
||||
nus_notification_enabled = false;
|
||||
LOG_INF("BLE connected");
|
||||
|
||||
struct bt_le_conn_param param = { .interval_min = 6, .interval_max = 12, .latency = 0, .timeout = 400 };
|
||||
bt_conn_le_param_update(conn, ¶m);
|
||||
}
|
||||
|
||||
/** @brief 断开回调,释放连接引用并安排延时重广播。 */
|
||||
static void disconnected(struct bt_conn *conn, uint8_t reason) {
|
||||
ARG_UNUSED(conn);
|
||||
LOG_WRN("BLE disconnected (reason 0x%02x)", reason);
|
||||
if (current_conn) {
|
||||
bt_conn_unref(current_conn);
|
||||
current_conn = NULL;
|
||||
}
|
||||
nus_notification_enabled = false;
|
||||
k_work_schedule(&adv_work, K_MSEC(1000));
|
||||
}
|
||||
|
||||
/** @brief NUS 通知使能状态回调。 */
|
||||
static void nus_send_enabled(enum bt_nus_send_status status) {
|
||||
nus_notification_enabled = (status == BT_NUS_SEND_STATUS_ENABLED);
|
||||
LOG_INF("NUS send_enabled: status=%d, enabled=%d", status, nus_notification_enabled);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief NUS 收数据回调,直接转发给上层注册的处理函数。
|
||||
*/
|
||||
static void nus_received_cb(struct bt_conn *conn, const uint8_t *data, uint16_t len) {
|
||||
ARG_UNUSED(conn);
|
||||
if (rx_cb) {
|
||||
rx_cb(data, len);
|
||||
}
|
||||
}
|
||||
|
||||
static struct bt_nus_cb nus_cb = {
|
||||
.send_enabled = nus_send_enabled,
|
||||
.received = nus_received_cb,
|
||||
};
|
||||
BT_CONN_CB_DEFINE(conn_callbacks) = { .connected = connected, .disconnected = disconnected };
|
||||
|
||||
int ble_transport_init(void) {
|
||||
k_work_init_delayable(&adv_work, adv_work_handler);
|
||||
|
||||
int err = bt_enable(NULL);
|
||||
if (err) {
|
||||
LOG_ERR("Bluetooth init failed (err %d)", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
settings_load();
|
||||
|
||||
err = bt_nus_init(&nus_cb);
|
||||
if (err) {
|
||||
LOG_ERR("NUS init failed (err %d)", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
LOG_INF("BLE transport initialized");
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ble_transport_adv_start(void) {
|
||||
bt_le_adv_start(BT_LE_ADV_CONN_FAST_2, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd));
|
||||
}
|
||||
|
||||
void ble_transport_send(const uint8_t *data, uint16_t len) {
|
||||
static bool first_send_logged = false;
|
||||
static uint32_t send_ok_count;
|
||||
static uint32_t send_fail_count;
|
||||
|
||||
if (!nus_notification_enabled || !current_conn) return;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
int err = bt_nus_send(current_conn, data, len);
|
||||
if (err == 0) {
|
||||
send_ok_count++;
|
||||
if (!first_send_logged) {
|
||||
LOG_INF("First NUS send OK (len=%u)", len);
|
||||
first_send_logged = true;
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (i == 0) {
|
||||
LOG_DBG("NUS send retry (err=%d)", err);
|
||||
}
|
||||
k_usleep(500);
|
||||
}
|
||||
send_fail_count++;
|
||||
LOG_WRN("NUS send failed x3 (len=%u, ok=%u, fail=%u)", len, send_ok_count, send_fail_count);
|
||||
}
|
||||
bool ble_transport_is_ready(void) {
|
||||
return nus_notification_enabled && (current_conn != NULL);
|
||||
}
|
||||
|
||||
void ble_transport_register_rx_cb(ble_rx_cb_t cb) {
|
||||
rx_cb = cb;
|
||||
}
|
||||
-132
@@ -1,132 +0,0 @@
|
||||
#include "button.h"
|
||||
#include "sensor.h"
|
||||
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
|
||||
LOG_MODULE_REGISTER(button, LOG_LEVEL_INF);
|
||||
|
||||
/* DK Button1 = sw0, LED1 = led0 */
|
||||
static const struct gpio_dt_spec btn1 = GPIO_DT_SPEC_GET(DT_ALIAS(sw0), gpios);
|
||||
static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
|
||||
|
||||
static struct gpio_callback btn_cb_data;
|
||||
static struct k_work_delayable led_off_work;
|
||||
static struct k_work_delayable emi_off_work;
|
||||
|
||||
#define LED_FLASH_MS 200
|
||||
#define LED_BLINK_COUNT 3
|
||||
#define EMI_LED_HOLD_MS 3000
|
||||
|
||||
static struct k_work tare_work;
|
||||
static volatile int blink_remaining;
|
||||
|
||||
/**
|
||||
* @brief 处理 LED 延时翻转,完成一次按键反馈闪烁序列。
|
||||
*
|
||||
* @param work Zephyr delayable work 入口参数,当前实现未直接使用。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void led_off_handler(struct k_work *work) {
|
||||
if (blink_remaining <= 0) return;
|
||||
|
||||
blink_remaining--;
|
||||
/* 奇数次:点亮;偶数次:熄灭 */
|
||||
gpio_pin_set_dt(&led1, blink_remaining & 1);
|
||||
if (blink_remaining > 0) {
|
||||
k_work_schedule(&led_off_work, K_MSEC(LED_FLASH_MS));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief EMI 指示熄灯 work:3 秒延时到期后熄灭 LED1。
|
||||
*
|
||||
* 与 led_off_work 分开是因为按键闪烁序列依赖 blink_remaining 的奇偶节拍,
|
||||
* EMI 指示是单次长亮,混用会让 blink_remaining 状态机错乱。
|
||||
*
|
||||
* @param work Zephyr delayable work 入口参数,当前实现未直接使用。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void emi_off_handler(struct k_work *work) {
|
||||
ARG_UNUSED(work);
|
||||
gpio_pin_set_dt(&led1, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在工作队列上下文执行去皮,并启动 LED 闪烁反馈。
|
||||
*
|
||||
* @param work Zephyr work 入口参数,当前实现未直接使用。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void tare_work_handler(struct k_work *work) {
|
||||
LOG_INF("Button1 pressed → tare");
|
||||
sensor_perform_tare();
|
||||
|
||||
/* LED1 闪烁 3 次:亮-灭-亮-灭-亮-灭 = 6 个状态翻转 */
|
||||
blink_remaining = LED_BLINK_COUNT * 2;
|
||||
gpio_pin_set_dt(&led1, 1);
|
||||
k_work_schedule(&led_off_work, K_MSEC(LED_FLASH_MS));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按键 GPIO 中断回调,只负责把去皮请求转交到工作队列。
|
||||
*
|
||||
* @param dev 触发中断的 GPIO 设备,当前实现未直接使用。
|
||||
* @param cb GPIO 回调对象,当前实现未直接使用。
|
||||
* @param pins 本次触发的引脚位图,当前实现未直接使用。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void btn1_isr(const struct device *dev, struct gpio_callback *cb, uint32_t pins) {
|
||||
ARG_UNUSED(dev);
|
||||
ARG_UNUSED(cb);
|
||||
ARG_UNUSED(pins);
|
||||
k_work_submit(&tare_work);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化按键模块:Button1 按下触发去皮 + LED1 闪烁反馈。
|
||||
*
|
||||
* @retval 0 按键中断、去皮 work 和 LED 反馈均初始化成功。
|
||||
* @retval -ENODEV 按键或 LED 对应的 GPIO 设备未就绪。
|
||||
* @retval 负值 GPIO 配置、中断配置或回调注册阶段返回的具体错误码。
|
||||
*/
|
||||
int button_init(void) {
|
||||
if (!gpio_is_ready_dt(&btn1) || !gpio_is_ready_dt(&led1)) {
|
||||
LOG_ERR("GPIO device not ready");
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int err;
|
||||
|
||||
err = gpio_pin_configure_dt(&btn1, GPIO_INPUT);
|
||||
if (err) return err;
|
||||
|
||||
err = gpio_pin_interrupt_configure_dt(&btn1, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
if (err) return err;
|
||||
|
||||
err = gpio_pin_configure_dt(&led1, GPIO_OUTPUT_INACTIVE);
|
||||
if (err) return err;
|
||||
|
||||
gpio_init_callback(&btn_cb_data, btn1_isr, BIT(btn1.pin));
|
||||
|
||||
err = gpio_add_callback(btn1.port, &btn_cb_data);
|
||||
if (err) return err;
|
||||
|
||||
k_work_init(&tare_work, tare_work_handler);
|
||||
k_work_init_delayable(&led_off_work, led_off_handler);
|
||||
k_work_init_delayable(&emi_off_work, emi_off_handler);
|
||||
|
||||
LOG_INF("Button1 → tare + LED1 flash initialized");
|
||||
return 0;
|
||||
}
|
||||
|
||||
void button_led_indicate_emi(void) {
|
||||
/* 直接拉高 LED 并延后 3 秒熄灭;重复触发会刷新熄灭时刻,从而支持连续 EMI 事件累计指示 */
|
||||
gpio_pin_set_dt(&led1, 1);
|
||||
k_work_reschedule(&emi_off_work, K_MSEC(EMI_LED_HOLD_MS));
|
||||
}
|
||||
@@ -1,630 +0,0 @@
|
||||
#include "calibration.h"
|
||||
#include "ads1256.h"
|
||||
#include "comm_protocol.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/settings/settings.h>
|
||||
#include <zephyr/sys/atomic.h>
|
||||
|
||||
LOG_MODULE_REGISTER(calibration, LOG_LEVEL_INF);
|
||||
|
||||
#define CAL_SETTINGS_ROOT "cal"
|
||||
#define CAL_SETTINGS_VER CAL_SETTINGS_ROOT "/ver"
|
||||
#define CAL_SETTINGS_ZERO CAL_SETTINGS_ROOT "/zero"
|
||||
#define CAL_SETTINGS_GAIN CAL_SETTINGS_ROOT "/gain"
|
||||
#define CAL_SETTINGS_GRID_X CAL_SETTINGS_ROOT "/grid_x"
|
||||
#define CAL_SETTINGS_GRID_Y CAL_SETTINGS_ROOT "/grid_y"
|
||||
#define CAL_VERSION_L1_VALID BIT(0)
|
||||
#define CAL_VERSION_L2_VALID BIT(1)
|
||||
#define CAL_FLAG_PENDING 0
|
||||
#define CAL_FLAG_DIRTY 0
|
||||
#define CAL_AVG_COUNT 17
|
||||
#define CAL_GRID_X_LEFT (-28.33f)
|
||||
#define CAL_GRID_X_MID (0.0f)
|
||||
#define CAL_GRID_X_RIGHT (28.33f)
|
||||
#define CAL_GRID_Y_BOTTOM (-10.67f)
|
||||
#define CAL_GRID_Y_MID (0.0f)
|
||||
#define CAL_GRID_Y_TOP (10.67f)
|
||||
|
||||
enum cal_state {
|
||||
CAL_STATE_IDLE = 0,
|
||||
CAL_STATE_L1_IN_PROGRESS,
|
||||
CAL_STATE_L1_READY,
|
||||
CAL_STATE_L2_IN_PROGRESS,
|
||||
CAL_STATE_L2_READY,
|
||||
};
|
||||
|
||||
struct cal_pending_cmd {
|
||||
uint8_t subcmd;
|
||||
uint8_t target;
|
||||
float param;
|
||||
};
|
||||
|
||||
static struct cal_runtime committed;
|
||||
static struct cal_runtime working;
|
||||
static struct cal_pending_cmd pending_cmd;
|
||||
static atomic_t pending_flags;
|
||||
static atomic_t cal_flags;
|
||||
static enum cal_state state = CAL_STATE_IDLE;
|
||||
static const uint8_t mux_channels[CAL_NUM_CHANNELS] = { 0x01, 0x23, 0x45, 0x67 };
|
||||
|
||||
/**
|
||||
* @brief 对采样数组做原地升序排序。
|
||||
*
|
||||
* @param arr 待排序数组。
|
||||
* @param n 元素个数。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void sort_array(int32_t *arr, int n) {
|
||||
for (int i = 0; i < n - 1; i++) {
|
||||
for (int j = 0; j < n - i - 1; j++) {
|
||||
if (arr[j] > arr[j + 1]) {
|
||||
int32_t tmp = arr[j];
|
||||
arr[j] = arr[j + 1];
|
||||
arr[j + 1] = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将一份运行时标定数据重置为默认值。
|
||||
*
|
||||
* 默认值只保留“未标定但可运行”的最小语义:零点为 0,增益回落到
|
||||
* `ADC_TO_FORCE_SCALE`。这样擦除或首次上电后,系统仍能继续输出基础 CoP。
|
||||
*
|
||||
* @param[out] runtime 待重置的标定数据。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void cal_reset_runtime(struct cal_runtime *runtime) {
|
||||
memset(runtime, 0, sizeof(*runtime));
|
||||
for (int i = 0; i < CAL_NUM_CHANNELS; i++) {
|
||||
runtime->gain[i] = ADC_TO_FORCE_SCALE;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 计算当前 committed 数据对应的版本位图。
|
||||
*
|
||||
* @return 版本位图。
|
||||
*/
|
||||
static uint32_t cal_make_version(void) {
|
||||
uint32_t version = 0U;
|
||||
|
||||
if (committed.l1_zero_valid && committed.l1_gain_valid) {
|
||||
version |= CAL_VERSION_L1_VALID;
|
||||
}
|
||||
if (committed.l2_valid) {
|
||||
version |= CAL_VERSION_L2_VALID;
|
||||
}
|
||||
|
||||
return version;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送标定响应帧。
|
||||
*
|
||||
* `data[0]` 固定回显 `target`,`data[1]` 固定回显当前状态机状态,其余 6 字节
|
||||
* 由命令处理逻辑按上下文填充。这样上位机至少总能知道“哪条命令作用在谁身上,
|
||||
* 执行后落在什么状态”,不需要依赖日志猜测固件内部阶段。
|
||||
*
|
||||
* @param status 响应状态码。
|
||||
* @param subcmd 子命令码。
|
||||
* @param target 命令目标。
|
||||
* @param value4 4 字节上下文值。
|
||||
* @param extra2 2 字节补充值。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void cal_send_resp(uint8_t status, uint8_t subcmd, uint8_t target, uint32_t value4, uint16_t extra2) {
|
||||
uint8_t data[8] = { 0 };
|
||||
|
||||
data[0] = target;
|
||||
data[1] = (uint8_t)state;
|
||||
memcpy(&data[2], &value4, sizeof(value4));
|
||||
memcpy(&data[6], &extra2, sizeof(extra2));
|
||||
comm_protocol_send_cal_resp(status, subcmd, data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读取指定通道的中值 ADC 原始计数。
|
||||
*
|
||||
* 标定读数必须和正式采样走同一条 MUX/DRDY/中值链路,否则即便数学公式正确,
|
||||
* 也会因为采样路径不一致而把系统误差带进标定结果。
|
||||
*
|
||||
* @param ch 通道号,范围 0-3。
|
||||
*
|
||||
* @return 中值 ADC 计数。
|
||||
*/
|
||||
static int32_t cal_read_channel_median(uint8_t ch) {
|
||||
int32_t samples[CAL_AVG_COUNT];
|
||||
|
||||
ads1256_write_reg(ADS1256_REG_MUX, mux_channels[ch]);
|
||||
ads1256_sync_wakeup();
|
||||
|
||||
for (int i = 0; i < CAL_AVG_COUNT; i++) {
|
||||
ads1256_wait_drdy(50);
|
||||
samples[i] = ads1256_read_data();
|
||||
}
|
||||
|
||||
sort_array(samples, CAL_AVG_COUNT);
|
||||
return samples[CAL_AVG_COUNT / 2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将网格点编号映射到 3x3 目标坐标。
|
||||
*
|
||||
* 编号采用行优先顺序:`0..2` 为上排,`3..5` 为中排,`6..8` 为下排。
|
||||
* 这样坐标定义集中在一个函数里,后续若手机侧编号不同,只需要改这一处映射。
|
||||
*
|
||||
* @param target 网格点编号。
|
||||
* @param[out] x 目标 X 坐标。
|
||||
* @param[out] y 目标 Y 坐标。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void cal_grid_target_to_xy(uint8_t target, float *x, float *y) {
|
||||
uint8_t row = target / 3U;
|
||||
uint8_t col = target % 3U;
|
||||
|
||||
*x = (col == 0U) ? CAL_GRID_X_LEFT : ((col == 1U) ? CAL_GRID_X_MID : CAL_GRID_X_RIGHT);
|
||||
*y = (row == 0U) ? CAL_GRID_Y_TOP : ((row == 1U) ? CAL_GRID_Y_MID : CAL_GRID_Y_BOTTOM);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 用当前 committed 的 L1 参数测一次 CoP。
|
||||
*
|
||||
* L2 标定记录的是“当前测得的 CoP 与已知目标点之间的误差”,所以这里必须
|
||||
* 显式使用 committed 里的零点/增益,而不是 working 里旧版本的数据。
|
||||
*
|
||||
* @param[out] cop_x 测得的 CoP X。
|
||||
* @param[out] cop_y 测得的 CoP Y。
|
||||
*
|
||||
* @retval 0 测量成功。
|
||||
* @retval -ERANGE 总力过低,当前 CoP 无意义。
|
||||
*/
|
||||
static int cal_measure_cop(float *cop_x, float *cop_y) {
|
||||
static const float sensor_x[CAL_NUM_CHANNELS] = {
|
||||
+BOARD_HALF_WIDTH_CM,
|
||||
+BOARD_HALF_WIDTH_CM,
|
||||
-BOARD_HALF_WIDTH_CM,
|
||||
-BOARD_HALF_WIDTH_CM,
|
||||
};
|
||||
static const float sensor_y[CAL_NUM_CHANNELS] = {
|
||||
+BOARD_HALF_LENGTH_CM,
|
||||
-BOARD_HALF_LENGTH_CM,
|
||||
-BOARD_HALF_LENGTH_CM,
|
||||
+BOARD_HALF_LENGTH_CM,
|
||||
};
|
||||
float total = 0.0f;
|
||||
float wx = 0.0f;
|
||||
float wy = 0.0f;
|
||||
|
||||
for (int i = 0; i < CAL_NUM_CHANNELS; i++) {
|
||||
int32_t raw = cal_read_channel_median((uint8_t)i);
|
||||
float force = (float)(raw - committed.zero[i]) * committed.gain[i];
|
||||
if (force < 0.0f) force = 0.0f;
|
||||
|
||||
total += force;
|
||||
wx += force * sensor_x[i];
|
||||
wy += force * sensor_y[i];
|
||||
}
|
||||
|
||||
if (total < COP_FORCE_ENTER_THRESHOLD) {
|
||||
*cop_x = 0.0f;
|
||||
*cop_y = 0.0f;
|
||||
return -ERANGE;
|
||||
}
|
||||
|
||||
*cop_x = wx / total;
|
||||
*cop_y = wy / total;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将当前 committed 数据持久化到 NVS。
|
||||
*
|
||||
* @retval 0 保存成功。
|
||||
* @retval 负值 settings 子系统返回的具体错误码。
|
||||
*/
|
||||
static int cal_save_all(void) {
|
||||
uint32_t version = cal_make_version();
|
||||
int err = settings_save_one(CAL_SETTINGS_VER, &version, sizeof(version));
|
||||
|
||||
if (err) return err;
|
||||
err = settings_save_one(CAL_SETTINGS_ZERO, committed.zero, sizeof(committed.zero));
|
||||
if (err) return err;
|
||||
err = settings_save_one(CAL_SETTINGS_GAIN, committed.gain, sizeof(committed.gain));
|
||||
if (err) return err;
|
||||
err = settings_save_one(CAL_SETTINGS_GRID_X, committed.grid_err_x, sizeof(committed.grid_err_x));
|
||||
if (err) return err;
|
||||
return settings_save_one(CAL_SETTINGS_GRID_Y, committed.grid_err_y, sizeof(committed.grid_err_y));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 擦除所有标定键。
|
||||
*
|
||||
* @retval 0 擦除成功。
|
||||
* @retval 负值 settings 子系统返回的具体错误码。
|
||||
*/
|
||||
static int cal_delete_all(void) {
|
||||
int err = settings_delete(CAL_SETTINGS_VER);
|
||||
|
||||
if (err) return err;
|
||||
err = settings_delete(CAL_SETTINGS_ZERO);
|
||||
if (err) return err;
|
||||
err = settings_delete(CAL_SETTINGS_GAIN);
|
||||
if (err) return err;
|
||||
err = settings_delete(CAL_SETTINGS_GRID_X);
|
||||
if (err) return err;
|
||||
return settings_delete(CAL_SETTINGS_GRID_Y);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief settings 子树加载回调。
|
||||
*
|
||||
* @param key `cal/` 后的子键名。
|
||||
* @param len 数据长度。
|
||||
* @param read_cb backend 读回调。
|
||||
* @param cb_arg backend 私有参数。
|
||||
*
|
||||
* @retval 0 处理成功。
|
||||
* @retval 负值 读失败。
|
||||
*/
|
||||
static int cal_settings_set(const char *key, size_t len, settings_read_cb read_cb, void *cb_arg) {
|
||||
const char *next;
|
||||
int rc;
|
||||
|
||||
if (settings_name_steq(key, "ver", &next) && !next && len == sizeof(uint32_t)) {
|
||||
uint32_t version = 0U;
|
||||
rc = read_cb(cb_arg, &version, sizeof(version));
|
||||
if (rc >= 0) {
|
||||
committed.l1_zero_valid = (version & CAL_VERSION_L1_VALID) != 0U;
|
||||
committed.l1_gain_valid = (version & CAL_VERSION_L1_VALID) != 0U;
|
||||
committed.l2_valid = (version & CAL_VERSION_L2_VALID) != 0U;
|
||||
return 0;
|
||||
}
|
||||
return rc;
|
||||
}
|
||||
|
||||
if (settings_name_steq(key, "zero", &next) && !next && len == sizeof(committed.zero)) {
|
||||
rc = read_cb(cb_arg, committed.zero, sizeof(committed.zero));
|
||||
return (rc < 0) ? rc : 0;
|
||||
}
|
||||
|
||||
if (settings_name_steq(key, "gain", &next) && !next && len == sizeof(committed.gain)) {
|
||||
rc = read_cb(cb_arg, committed.gain, sizeof(committed.gain));
|
||||
return (rc < 0) ? rc : 0;
|
||||
}
|
||||
|
||||
if (settings_name_steq(key, "grid_x", &next) && !next && len == sizeof(committed.grid_err_x)) {
|
||||
rc = read_cb(cb_arg, committed.grid_err_x, sizeof(committed.grid_err_x));
|
||||
return (rc < 0) ? rc : 0;
|
||||
}
|
||||
|
||||
if (settings_name_steq(key, "grid_y", &next) && !next && len == sizeof(committed.grid_err_y)) {
|
||||
rc = read_cb(cb_arg, committed.grid_err_y, sizeof(committed.grid_err_y));
|
||||
return (rc < 0) ? rc : 0;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
SETTINGS_STATIC_HANDLER_DEFINE(calibration, CAL_SETTINGS_ROOT, NULL, cal_settings_set, NULL, NULL);
|
||||
|
||||
/**
|
||||
* @brief 初始化标定模块,从 NVS 加载持久化数据或使用默认值。
|
||||
*
|
||||
* @retval 0 成功。
|
||||
*/
|
||||
int cal_init(void) {
|
||||
cal_reset_runtime(&committed);
|
||||
cal_reset_runtime(&working);
|
||||
atomic_clear(&pending_flags);
|
||||
atomic_clear(&cal_flags);
|
||||
state = CAL_STATE_IDLE;
|
||||
|
||||
(void)settings_load_subtree(CAL_SETTINGS_ROOT);
|
||||
|
||||
working = committed;
|
||||
if (working.l2_valid) {
|
||||
state = CAL_STATE_L2_READY;
|
||||
} else if (working.l1_zero_valid && working.l1_gain_valid) {
|
||||
state = CAL_STATE_L1_READY;
|
||||
}
|
||||
|
||||
LOG_INF("Calibration init: l1=%d l2=%d", working.l1_gain_valid, working.l2_valid);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取当前生效的标定数据指针。
|
||||
*
|
||||
* @return 指向内部 working copy 的只读指针。
|
||||
*/
|
||||
const struct cal_runtime *cal_get_working(void) {
|
||||
return &working;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 检查标定数据是否有更新,若有则刷新 working copy。
|
||||
*
|
||||
* @retval true 数据已刷新。
|
||||
* @retval false 无更新。
|
||||
*/
|
||||
bool cal_check_update(void) {
|
||||
if (!atomic_test_and_clear_bit(&cal_flags, CAL_FLAG_DIRTY)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
working = committed;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 入队一条来自 BLE 的标定命令。
|
||||
*
|
||||
* @param subcmd 子命令码。
|
||||
* @param target 通道号或网格点号。
|
||||
* @param param 浮点参数。
|
||||
*
|
||||
* @retval 0 命令已入队。
|
||||
* @retval -EBUSY 上一条命令尚未被消费。
|
||||
* @retval -EINVAL 参数非法。
|
||||
*/
|
||||
int cal_enqueue_command(uint8_t subcmd, uint8_t target, float param) {
|
||||
switch (subcmd) {
|
||||
case CAL_SUBCMD_TARE_CH:
|
||||
case CAL_SUBCMD_GAIN_CH:
|
||||
if (target >= CAL_NUM_CHANNELS) return -EINVAL;
|
||||
break;
|
||||
case CAL_SUBCMD_RECORD_GRID:
|
||||
if (target >= CAL_NUM_GRID_PTS) return -EINVAL;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
pending_cmd.subcmd = subcmd;
|
||||
pending_cmd.target = target;
|
||||
pending_cmd.param = param;
|
||||
|
||||
if (atomic_test_and_set_bit(&pending_flags, CAL_FLAG_PENDING)) {
|
||||
return -EBUSY;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在 sensor 线程中执行待处理的标定命令。
|
||||
*
|
||||
* @retval true 执行了标定命令。
|
||||
* @retval false 当前没有待处理命令。
|
||||
*/
|
||||
bool cal_execute_pending(void) {
|
||||
struct cal_pending_cmd cmd;
|
||||
uint8_t status = CAL_STATUS_OK;
|
||||
uint32_t value4 = 0U;
|
||||
uint16_t extra2 = 0U;
|
||||
|
||||
if (!atomic_test_and_clear_bit(&pending_flags, CAL_FLAG_PENDING)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
cmd = pending_cmd;
|
||||
|
||||
switch (cmd.subcmd) {
|
||||
case CAL_SUBCMD_START_L1:
|
||||
committed = working;
|
||||
committed.l1_zero_valid = false;
|
||||
committed.l1_gain_valid = false;
|
||||
committed.l2_valid = false;
|
||||
memset(committed.grid_err_x, 0, sizeof(committed.grid_err_x));
|
||||
memset(committed.grid_err_y, 0, sizeof(committed.grid_err_y));
|
||||
state = CAL_STATE_L1_IN_PROGRESS;
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_TARE_CH:
|
||||
if (state != CAL_STATE_L1_IN_PROGRESS) {
|
||||
status = CAL_STATUS_ERR_STATE;
|
||||
break;
|
||||
}
|
||||
committed.zero[cmd.target] = cal_read_channel_median(cmd.target);
|
||||
memcpy(&value4, &committed.zero[cmd.target], sizeof(committed.zero[cmd.target]));
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_GAIN_CH:
|
||||
if (state != CAL_STATE_L1_IN_PROGRESS) {
|
||||
status = CAL_STATUS_ERR_STATE;
|
||||
break;
|
||||
}
|
||||
if (cmd.param <= 0.0f) {
|
||||
status = CAL_STATUS_ERR_PARAM;
|
||||
break;
|
||||
}
|
||||
{
|
||||
int32_t median = cal_read_channel_median(cmd.target);
|
||||
int32_t delta = median - committed.zero[cmd.target];
|
||||
if (delta == 0) {
|
||||
status = CAL_STATUS_ERR_PARAM;
|
||||
break;
|
||||
}
|
||||
committed.gain[cmd.target] = cmd.param / (float)delta;
|
||||
memcpy(&value4, &committed.gain[cmd.target], sizeof(committed.gain[cmd.target]));
|
||||
}
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_COMMIT_L1:
|
||||
if (state != CAL_STATE_L1_IN_PROGRESS) {
|
||||
status = CAL_STATUS_ERR_STATE;
|
||||
break;
|
||||
}
|
||||
committed.l1_zero_valid = true;
|
||||
committed.l1_gain_valid = true;
|
||||
committed.l2_valid = false;
|
||||
if (cal_save_all() != 0) {
|
||||
status = CAL_STATUS_ERR_NVS;
|
||||
break;
|
||||
}
|
||||
state = CAL_STATE_L1_READY;
|
||||
value4 = cal_make_version();
|
||||
atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY);
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_ABORT:
|
||||
committed = working;
|
||||
state = working.l2_valid
|
||||
? CAL_STATE_L2_READY
|
||||
: ((working.l1_zero_valid && working.l1_gain_valid) ? CAL_STATE_L1_READY : CAL_STATE_IDLE);
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_START_L2:
|
||||
if (!(working.l1_zero_valid && working.l1_gain_valid)) {
|
||||
status = CAL_STATUS_ERR_STATE;
|
||||
break;
|
||||
}
|
||||
committed = working;
|
||||
memset(committed.grid_err_x, 0, sizeof(committed.grid_err_x));
|
||||
memset(committed.grid_err_y, 0, sizeof(committed.grid_err_y));
|
||||
committed.l2_valid = false;
|
||||
state = CAL_STATE_L2_IN_PROGRESS;
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_RECORD_GRID:
|
||||
if (state != CAL_STATE_L2_IN_PROGRESS) {
|
||||
status = CAL_STATUS_ERR_STATE;
|
||||
break;
|
||||
}
|
||||
{
|
||||
float measured_x;
|
||||
float measured_y;
|
||||
float target_x;
|
||||
float target_y;
|
||||
|
||||
if (cal_measure_cop(&measured_x, &measured_y) != 0) {
|
||||
status = CAL_STATUS_ERR_PARAM;
|
||||
break;
|
||||
}
|
||||
|
||||
cal_grid_target_to_xy(cmd.target, &target_x, &target_y);
|
||||
committed.grid_err_x[cmd.target] = measured_x - target_x;
|
||||
committed.grid_err_y[cmd.target] = measured_y - target_y;
|
||||
memcpy(&value4, &committed.grid_err_x[cmd.target], sizeof(committed.grid_err_x[cmd.target]));
|
||||
memcpy(&extra2, &committed.grid_err_y[cmd.target], sizeof(extra2));
|
||||
}
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_COMMIT_L2:
|
||||
if (state != CAL_STATE_L2_IN_PROGRESS) {
|
||||
status = CAL_STATUS_ERR_STATE;
|
||||
break;
|
||||
}
|
||||
committed.l2_valid = true;
|
||||
if (cal_save_all() != 0) {
|
||||
committed.l2_valid = false;
|
||||
status = CAL_STATUS_ERR_NVS;
|
||||
break;
|
||||
}
|
||||
state = CAL_STATE_L2_READY;
|
||||
value4 = cal_make_version();
|
||||
atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY);
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_ERASE:
|
||||
if (cal_delete_all() != 0) {
|
||||
status = CAL_STATUS_ERR_NVS;
|
||||
break;
|
||||
}
|
||||
cal_reset_runtime(&committed);
|
||||
state = CAL_STATE_IDLE;
|
||||
atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY);
|
||||
break;
|
||||
|
||||
case CAL_SUBCMD_QUERY:
|
||||
value4 = cal_make_version();
|
||||
extra2 = (uint16_t)((working.l1_zero_valid ? BIT(0) : 0U) | (working.l1_gain_valid ? BIT(1) : 0U) |
|
||||
(working.l2_valid ? BIT(2) : 0U));
|
||||
break;
|
||||
|
||||
default:
|
||||
status = CAL_STATUS_ERR_PARAM;
|
||||
break;
|
||||
}
|
||||
|
||||
cal_send_resp(status, cmd.subcmd, cmd.target, value4, extra2);
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 对计算出的 CoP 坐标施加 L2 网格补偿。
|
||||
*
|
||||
* @param[in,out] cop_x CoP X 坐标 (cm)。
|
||||
* @param[in,out] cop_y CoP Y 坐标 (cm)。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
void cal_apply_l2_correction(float *cop_x, float *cop_y) {
|
||||
float x;
|
||||
float y;
|
||||
int ix;
|
||||
int iy;
|
||||
float x0;
|
||||
float x1;
|
||||
float y0;
|
||||
float y1;
|
||||
float tx;
|
||||
float ty;
|
||||
int idx00;
|
||||
int idx10;
|
||||
int idx01;
|
||||
int idx11;
|
||||
float ex0;
|
||||
float ex1;
|
||||
float ey0;
|
||||
float ey1;
|
||||
float err_x;
|
||||
float err_y;
|
||||
|
||||
if (!working.l2_valid || !cop_x || !cop_y) {
|
||||
return;
|
||||
}
|
||||
|
||||
x = *cop_x;
|
||||
y = *cop_y;
|
||||
|
||||
if (x < CAL_GRID_X_LEFT) x = CAL_GRID_X_LEFT;
|
||||
if (x > CAL_GRID_X_RIGHT) x = CAL_GRID_X_RIGHT;
|
||||
if (y < CAL_GRID_Y_BOTTOM) y = CAL_GRID_Y_BOTTOM;
|
||||
if (y > CAL_GRID_Y_TOP) y = CAL_GRID_Y_TOP;
|
||||
|
||||
ix = (x <= CAL_GRID_X_MID) ? 0 : 1;
|
||||
iy = (y <= CAL_GRID_Y_MID) ? 0 : 1;
|
||||
|
||||
x0 = (ix == 0) ? CAL_GRID_X_LEFT : CAL_GRID_X_MID;
|
||||
x1 = (ix == 0) ? CAL_GRID_X_MID : CAL_GRID_X_RIGHT;
|
||||
y0 = (iy == 0) ? CAL_GRID_Y_BOTTOM : CAL_GRID_Y_MID;
|
||||
y1 = (iy == 0) ? CAL_GRID_Y_MID : CAL_GRID_Y_TOP;
|
||||
|
||||
tx = (x1 - x0) == 0.0f ? 0.0f : (x - x0) / (x1 - x0);
|
||||
ty = (y1 - y0) == 0.0f ? 0.0f : (y - y0) / (y1 - y0);
|
||||
|
||||
idx00 = (2 - iy) * 3 + ix;
|
||||
idx10 = idx00 + 1;
|
||||
idx01 = (1 - iy) * 3 + ix;
|
||||
idx11 = idx01 + 1;
|
||||
|
||||
ex0 = working.grid_err_x[idx00] + tx * (working.grid_err_x[idx10] - working.grid_err_x[idx00]);
|
||||
ex1 = working.grid_err_x[idx01] + tx * (working.grid_err_x[idx11] - working.grid_err_x[idx01]);
|
||||
ey0 = working.grid_err_y[idx00] + tx * (working.grid_err_y[idx10] - working.grid_err_y[idx00]);
|
||||
ey1 = working.grid_err_y[idx01] + tx * (working.grid_err_y[idx11] - working.grid_err_y[idx01]);
|
||||
err_x = ex0 + ty * (ex1 - ex0);
|
||||
err_y = ey0 + ty * (ey1 - ey0);
|
||||
|
||||
*cop_x = x - err_x;
|
||||
*cop_y = y - err_y;
|
||||
}
|
||||
@@ -1,154 +0,0 @@
|
||||
#include "comm_protocol.h"
|
||||
#include "ble_transport.h"
|
||||
#include "calibration.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
|
||||
LOG_MODULE_REGISTER(comm_protocol, LOG_LEVEL_INF);
|
||||
|
||||
#define MSGQ_DEPTH 8
|
||||
|
||||
K_MSGQ_DEFINE(rx_msgq, sizeof(struct comm_msg), MSGQ_DEPTH, 4);
|
||||
|
||||
/**
|
||||
* @brief 计算帧 CRC-8/MAXIM,校验范围为 type 字段到 payload 末尾。
|
||||
*
|
||||
* @param frame_bytes 完整帧的字节起始地址。
|
||||
* @param frame_size 帧总字节数(含 sync + crc)。
|
||||
*
|
||||
* @return CRC-8 值。
|
||||
*/
|
||||
static inline uint8_t frame_crc(const uint8_t *frame_bytes, size_t frame_size) {
|
||||
/* 跳过 sync0+sync1,校验到倒数第二字节(crc 字段之前) */
|
||||
return crc8(frame_bytes + 2, frame_size - 3, 0x31, 0x00, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief BLE 收数据回调,解析下行帧并推入消息队列。
|
||||
*
|
||||
* @param data 原始字节流。
|
||||
* @param len 字节数。
|
||||
*/
|
||||
static void protocol_rx_handler(const uint8_t *data, uint16_t len) {
|
||||
if (len < 3 || data[0] != PROTO_SYNC0 || data[1] != PROTO_SYNC1) return;
|
||||
|
||||
struct comm_msg msg;
|
||||
|
||||
switch (data[2]) {
|
||||
case PROTO_TYPE_RESISTANCE:
|
||||
if (len < sizeof(struct resistance_frame_t)) return;
|
||||
if (data[sizeof(struct resistance_frame_t) - 1] != frame_crc(data, sizeof(struct resistance_frame_t))) {
|
||||
LOG_WRN("Resistance frame CRC mismatch");
|
||||
return;
|
||||
}
|
||||
union resistance_pkt_t rpkt;
|
||||
memcpy(rpkt.bytes, data, sizeof(struct resistance_frame_t));
|
||||
msg.type = COMM_MSG_RESISTANCE;
|
||||
msg.resistance.K = rpkt.frame.K;
|
||||
msg.resistance.B = rpkt.frame.B;
|
||||
msg.resistance.Tau = rpkt.frame.Tau;
|
||||
LOG_INF(
|
||||
"RX Resistance: K=%.1f B=%.1f Tau=%.2f", (double)msg.resistance.K, (double)msg.resistance.B,
|
||||
(double)msg.resistance.Tau);
|
||||
break;
|
||||
|
||||
case PROTO_TYPE_SPOTTER:
|
||||
if (len < sizeof(struct spotter_frame_t)) return;
|
||||
if (data[sizeof(struct spotter_frame_t) - 1] != frame_crc(data, sizeof(struct spotter_frame_t))) {
|
||||
LOG_WRN("Spotter frame CRC mismatch");
|
||||
return;
|
||||
}
|
||||
union spotter_pkt_t spkt;
|
||||
memcpy(spkt.bytes, data, sizeof(struct spotter_frame_t));
|
||||
msg.type = COMM_MSG_SPOTTER;
|
||||
msg.spotter.threshold = spkt.frame.threshold;
|
||||
msg.spotter.enable = spkt.frame.enable != 0;
|
||||
LOG_INF("RX Spotter: threshold=%.1f enable=%d", (double)msg.spotter.threshold, msg.spotter.enable);
|
||||
break;
|
||||
|
||||
case PROTO_TYPE_HEARTBEAT:
|
||||
if (len < sizeof(struct heartbeat_frame_t)) return;
|
||||
if (data[sizeof(struct heartbeat_frame_t) - 1] != frame_crc(data, sizeof(struct heartbeat_frame_t))) {
|
||||
LOG_WRN("Heartbeat frame CRC mismatch");
|
||||
return;
|
||||
}
|
||||
msg.type = COMM_MSG_HEARTBEAT;
|
||||
msg.heartbeat.counter = data[3];
|
||||
LOG_DBG("RX Heartbeat");
|
||||
break;
|
||||
|
||||
/* 标定命令不走 msgq:cal 模块自带入队/pending 机制,
|
||||
* sensor 线程通过 cal_execute_pending() 消费,无需额外中转 */
|
||||
case PROTO_TYPE_CAL_CMD:
|
||||
if (len < sizeof(struct cal_cmd_frame_t)) return;
|
||||
if (data[sizeof(struct cal_cmd_frame_t) - 1] != frame_crc(data, sizeof(struct cal_cmd_frame_t))) {
|
||||
LOG_WRN("Cal cmd frame CRC mismatch");
|
||||
return;
|
||||
}
|
||||
{
|
||||
union cal_cmd_pkt_t cpkt;
|
||||
memcpy(cpkt.bytes, data, sizeof(struct cal_cmd_frame_t));
|
||||
|
||||
int err = cal_enqueue_command(cpkt.frame.subcmd, cpkt.frame.target, cpkt.frame.param);
|
||||
if (err == 0) {
|
||||
LOG_INF(
|
||||
"RX CalCmd: sub=0x%02x target=%u param=%.2f", cpkt.frame.subcmd, cpkt.frame.target,
|
||||
(double)cpkt.frame.param);
|
||||
} else if (err == -EBUSY) {
|
||||
uint8_t resp[8] = { cpkt.frame.target, 0 };
|
||||
comm_protocol_send_cal_resp(CAL_STATUS_ERR_STATE, cpkt.frame.subcmd, resp);
|
||||
LOG_WRN("Cal cmd busy (sub=0x%02x)", cpkt.frame.subcmd);
|
||||
} else if (err == -EINVAL) {
|
||||
uint8_t resp[8] = { cpkt.frame.target, 0 };
|
||||
comm_protocol_send_cal_resp(CAL_STATUS_ERR_PARAM, cpkt.frame.subcmd, resp);
|
||||
LOG_WRN("Cal cmd invalid (sub=0x%02x)", cpkt.frame.subcmd);
|
||||
} else {
|
||||
uint8_t resp[8] = { cpkt.frame.target, 0 };
|
||||
comm_protocol_send_cal_resp(CAL_STATUS_ERR_STATE, cpkt.frame.subcmd, resp);
|
||||
LOG_ERR("Cal cmd unexpected err=%d (sub=0x%02x)", err, cpkt.frame.subcmd);
|
||||
}
|
||||
}
|
||||
return;
|
||||
|
||||
default:
|
||||
LOG_DBG("Unknown frame type: 0x%02x", data[2]);
|
||||
return;
|
||||
}
|
||||
|
||||
if (k_msgq_put(&rx_msgq, &msg, K_NO_WAIT)) {
|
||||
LOG_WRN("rx_msgq full, dropped type=%d", msg.type);
|
||||
}
|
||||
}
|
||||
|
||||
int comm_protocol_init(void) {
|
||||
ble_transport_register_rx_cb(protocol_rx_handler);
|
||||
LOG_INF("comm_protocol initialized");
|
||||
return 0;
|
||||
}
|
||||
|
||||
void comm_protocol_send_cop(uint8_t flags, int16_t cop_x, int16_t cop_y, int16_t force) {
|
||||
union cop_pkt_t pkt;
|
||||
pkt.frame.sync0 = PROTO_SYNC0;
|
||||
pkt.frame.sync1 = PROTO_SYNC1;
|
||||
pkt.frame.type = PROTO_TYPE_COP;
|
||||
pkt.frame.flags = flags;
|
||||
pkt.frame.cop_x = cop_x;
|
||||
pkt.frame.cop_y = cop_y;
|
||||
pkt.frame.force = force;
|
||||
pkt.frame.crc = frame_crc(pkt.bytes, sizeof(pkt.bytes));
|
||||
ble_transport_send(pkt.bytes, sizeof(pkt.bytes));
|
||||
}
|
||||
|
||||
void comm_protocol_send_cal_resp(uint8_t status, uint8_t subcmd, const uint8_t data[8]) {
|
||||
union cal_resp_pkt_t pkt;
|
||||
pkt.frame.sync0 = PROTO_SYNC0;
|
||||
pkt.frame.sync1 = PROTO_SYNC1;
|
||||
pkt.frame.type = PROTO_TYPE_CAL_RESP;
|
||||
pkt.frame.status = status;
|
||||
pkt.frame.subcmd = subcmd;
|
||||
memcpy(pkt.frame.data, data, 8);
|
||||
pkt.frame.crc = frame_crc(pkt.bytes, sizeof(pkt.bytes));
|
||||
ble_transport_send(pkt.bytes, sizeof(pkt.bytes));
|
||||
}
|
||||
-53
@@ -1,53 +0,0 @@
|
||||
/*
|
||||
* GML670 Balance Board — CoP Binary Protocol
|
||||
* Features:
|
||||
* - 4-channel ADS1256 with averaging filter.
|
||||
* - Board-side CoP (Center of Pressure) computation.
|
||||
* - Binary protocol over BLE NUS (17-byte CoP frame, 50 Hz).
|
||||
* - Downlink: resistance params, spotter mode, heartbeat.
|
||||
*/
|
||||
|
||||
#include "ble_transport.h"
|
||||
#include "button.h"
|
||||
#include "calibration.h"
|
||||
#include "comm_protocol.h"
|
||||
#include "sensor.h"
|
||||
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
|
||||
LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
|
||||
|
||||
/**
|
||||
* @brief 初始化系统各模块并进入主线程驻留状态。
|
||||
*
|
||||
* @retval 0 理论上的正常返回值;当前实现进入永久休眠后不会主动返回。
|
||||
* @retval -1 按键模块或传感器模块初始化失败。
|
||||
*/
|
||||
int main(void) {
|
||||
k_msleep(2000);
|
||||
LOG_INF("--- GML670 System (CoP Binary Protocol) ---");
|
||||
|
||||
ble_transport_init();
|
||||
comm_protocol_init();
|
||||
ble_transport_adv_start();
|
||||
|
||||
if (cal_init()) {
|
||||
LOG_ERR("Failed to initialize calibration");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (button_init()) {
|
||||
LOG_ERR("Failed to initialize buttons");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (sensor_init()) {
|
||||
LOG_ERR("Failed to initialize sensor");
|
||||
return -1;
|
||||
}
|
||||
|
||||
k_sleep(K_FOREVER);
|
||||
|
||||
return 0;
|
||||
}
|
||||
-313
@@ -1,313 +0,0 @@
|
||||
#include "sensor.h"
|
||||
#include "ads1256.h"
|
||||
#include "ble_transport.h"
|
||||
#include "button.h"
|
||||
#include "calibration.h"
|
||||
#include "comm_protocol.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/sys/atomic.h>
|
||||
|
||||
LOG_MODULE_REGISTER(sensor, LOG_LEVEL_INF);
|
||||
|
||||
/*
|
||||
* 每通道每帧采 9 次取中值。3750 SPS 下:
|
||||
* 9 次各 0.27ms = 2.4ms/通道
|
||||
* 4 通道 = 9.7ms,帧率 ~100Hz
|
||||
* 提高帧率将奈奎斯特频率抬至 ~50Hz,避免低速马达振动混叠
|
||||
*/
|
||||
#define AVG_COUNT 9
|
||||
#define DEADZONE_THRESHOLD 250
|
||||
|
||||
/* MUX 通道配置:4 路差分 */
|
||||
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_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_CM, /* S0: FR */
|
||||
-BOARD_HALF_LENGTH_CM, /* S1: BR */
|
||||
-BOARD_HALF_LENGTH_CM, /* S2: BL */
|
||||
+BOARD_HALF_LENGTH_CM, /* S3: FL */
|
||||
};
|
||||
|
||||
/* --- 内部状态 --- */
|
||||
static int32_t sensor_offsets[4];
|
||||
static int32_t filtered[4];
|
||||
/* 二阶 Butterworth 滤波器状态 (Direct Form II Transposed) */
|
||||
static float lp_z1[4]; /* 延迟节点 1 */
|
||||
static float lp_z2[4]; /* 延迟节点 2 */
|
||||
static atomic_t tare_requested;
|
||||
|
||||
/* --- 线程 --- */
|
||||
#define SENSOR_STACK_SIZE 2048
|
||||
#define SENSOR_PRIORITY 5
|
||||
static K_THREAD_STACK_DEFINE(sensor_stack, SENSOR_STACK_SIZE);
|
||||
static struct k_thread sensor_thread;
|
||||
|
||||
/**
|
||||
* @brief 对整型数组做原地升序排序。
|
||||
*
|
||||
* @param arr 待排序的数据缓冲区;这里要求调用方传入可写数组,
|
||||
* 因为去皮流程需要直接在采样缓冲区上重排以减少额外拷贝。
|
||||
* @param n 数组元素个数;显式传入长度是为了让该内部工具函数只依赖调用现场,
|
||||
* 避免隐式假设固定采样数后影响后续维护。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void sort_array(int32_t *arr, int n) {
|
||||
for (int i = 0; i < n - 1; i++) {
|
||||
for (int j = 0; j < n - i - 1; j++) {
|
||||
if (arr[j] > arr[j + 1]) {
|
||||
int32_t temp = arr[j];
|
||||
arr[j] = arr[j + 1];
|
||||
arr[j + 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 执行一次四路传感器去皮,更新每路零点偏移。
|
||||
*
|
||||
* @param 无。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void do_tare(void) {
|
||||
LOG_INF("Taring...");
|
||||
int32_t sorted_buf[AVG_COUNT];
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
ads1256_write_reg(ADS1256_REG_MUX, mux_channels[i]);
|
||||
ads1256_sync_wakeup();
|
||||
for (int k = 0; k < AVG_COUNT; k++) {
|
||||
ads1256_wait_drdy(50);
|
||||
sorted_buf[k] = ads1256_read_data();
|
||||
}
|
||||
sort_array(sorted_buf, AVG_COUNT);
|
||||
sensor_offsets[i] = sorted_buf[AVG_COUNT / 2];
|
||||
}
|
||||
|
||||
LOG_INF(
|
||||
"Tare offsets=[%ld,%ld,%ld,%ld]", (long)sensor_offsets[0], (long)sensor_offsets[1], (long)sensor_offsets[2],
|
||||
(long)sensor_offsets[3]);
|
||||
memset(filtered, 0, sizeof(filtered));
|
||||
memset(lp_z1, 0, sizeof(lp_z1));
|
||||
memset(lp_z2, 0, sizeof(lp_z2));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 完成一帧四通道采集,并写入去皮后的滤波结果。
|
||||
*
|
||||
* @param 无。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void acquire_cycle(void) {
|
||||
int32_t samples[AVG_COUNT];
|
||||
|
||||
for (int ch = 0; ch < 4; ch++) {
|
||||
ads1256_write_reg(ADS1256_REG_MUX, mux_channels[ch]);
|
||||
ads1256_sync_wakeup();
|
||||
|
||||
for (int k = 0; k < AVG_COUNT; k++) {
|
||||
int drdy_err = ads1256_wait_drdy(50);
|
||||
if (drdy_err) {
|
||||
LOG_WRN("ch%d sample%d DRDY timeout", ch, k);
|
||||
}
|
||||
samples[k] = ads1256_read_data();
|
||||
}
|
||||
|
||||
sort_array(samples, AVG_COUNT);
|
||||
int32_t median = samples[AVG_COUNT / 2] - sensor_offsets[ch];
|
||||
if (median > -DEADZONE_THRESHOLD && median < DEADZONE_THRESHOLD) median = 0;
|
||||
|
||||
/* 二阶 Butterworth 低通 (Direct Form II Transposed) */
|
||||
float x = (float)median;
|
||||
float y = LPF_B0 * x + lp_z1[ch];
|
||||
lp_z1[ch] = LPF_B1 * x - LPF_A1 * y + lp_z2[ch];
|
||||
lp_z2[ch] = LPF_B2 * x - LPF_A2 * y;
|
||||
filtered[ch] = (int32_t)y;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 根据四路受力结果计算压力中心(CoP)和总力。
|
||||
*
|
||||
* @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(int16_t *out_x, int16_t *out_y, int16_t *out_force) {
|
||||
static bool force_valid_state = false;
|
||||
|
||||
const struct cal_runtime *cal = cal_get_working();
|
||||
float forces[4];
|
||||
float total = 0.0f;
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
float gain = cal->l1_gain_valid ? cal->gain[i] : ADC_TO_FORCE_SCALE;
|
||||
forces[i] = (float)filtered[i] * gain;
|
||||
/* 压力传感器不可能产生负力,负值是零漂 */
|
||||
if (forces[i] < 0.0f) forces[i] = 0.0f;
|
||||
total += forces[i];
|
||||
}
|
||||
|
||||
/* 迟滞判定:避免阈值附近反复切换 */
|
||||
if (!force_valid_state) {
|
||||
if (total < COP_FORCE_ENTER_THRESHOLD) {
|
||||
*out_force = 0;
|
||||
*out_x = 0;
|
||||
*out_y = 0;
|
||||
return false;
|
||||
}
|
||||
force_valid_state = true;
|
||||
} else {
|
||||
if (total < COP_FORCE_EXIT_THRESHOLD) {
|
||||
force_valid_state = false;
|
||||
*out_force = 0;
|
||||
*out_x = 0;
|
||||
*out_y = 0;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
*out_force = (int16_t)total;
|
||||
|
||||
float wx = 0.0f, wy = 0.0f;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
wx += forces[i] * sensor_x[i];
|
||||
wy += forces[i] * sensor_y[i];
|
||||
}
|
||||
*out_x = (int16_t)(wx / total);
|
||||
*out_y = (int16_t)(wy / total);
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 传感器后台线程主循环,负责采集、去皮处理、CoP 计算和蓝牙发送。
|
||||
*
|
||||
* @param p1 Zephyr 线程入口预留参数,当前未使用;
|
||||
* @param p2 Zephyr 线程入口预留参数,当前未使用;
|
||||
* @param p3 Zephyr 线程入口预留参数,当前未使用;
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
static void sensor_thread_fn(void *p1, void *p2, void *p3) {
|
||||
(void)p1;
|
||||
(void)p2;
|
||||
(void)p3;
|
||||
|
||||
uint8_t debug_log_divider = 0;
|
||||
uint8_t send_divider = 0;
|
||||
int64_t frame_ts = k_uptime_get();
|
||||
|
||||
while (1) {
|
||||
int64_t now = k_uptime_get();
|
||||
int64_t frame_ms = now - frame_ts;
|
||||
frame_ts = now;
|
||||
|
||||
cal_check_update();
|
||||
if (cal_execute_pending()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* 去皮请求 */
|
||||
if (atomic_cas(&tare_requested, 1, 0)) {
|
||||
do_tare();
|
||||
}
|
||||
|
||||
/* 采集 */
|
||||
int64_t t0 = k_uptime_get();
|
||||
acquire_cycle();
|
||||
int64_t t1 = k_uptime_get();
|
||||
int64_t acq = t1 - t0;
|
||||
|
||||
/* EMI 自恢复:acq 异常短说明寄存器被干扰改写 */
|
||||
if (acq < 10) {
|
||||
LOG_ERR("EMI detected (acq=%lld ms)", (long long)acq);
|
||||
ads1256_recover(10);
|
||||
/* 点亮 LED1 作为 EMI 恢复指示,3 秒后由 button 模块自动熄灭 */
|
||||
button_led_indicate_emi();
|
||||
continue;
|
||||
}
|
||||
|
||||
/* CoP 计算 + 发送 */
|
||||
int16_t cop_x, cop_y, force;
|
||||
bool valid = compute_cop(&cop_x, &cop_y, &force);
|
||||
uint8_t flags = valid ? COP_FLAG_FORCE_VALID : 0;
|
||||
if (valid) {
|
||||
float fx = (float)cop_x;
|
||||
float fy = (float)cop_y;
|
||||
cal_apply_l2_correction(&fx, &fy);
|
||||
cop_x = (int16_t)fx;
|
||||
cop_y = (int16_t)fy;
|
||||
}
|
||||
/* 2:1 降采样:内部 ~100Hz 计算,50Hz 输出 */
|
||||
if (++send_divider >= 2) {
|
||||
send_divider = 0;
|
||||
comm_protocol_send_cop(flags, cop_x, cop_y, force);
|
||||
}
|
||||
|
||||
/* 调试日志(5 Hz) */
|
||||
if (++debug_log_divider >= 10) {
|
||||
debug_log_divider = 0;
|
||||
LOG_INF(
|
||||
"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 frame=%lld ms", (long long)(t1 - t0), (long long)frame_ms);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化压力传感器模块:ADS1256 硬件 + 初始去皮 + 创建采集线程。
|
||||
*
|
||||
* 调用后采集线程自动启动,以 50 Hz 循环采集、计算 CoP 并发送 BLE 数据。
|
||||
*
|
||||
* @retval 0 初始化成功,传感器线程已经启动并完成一次初始去皮。
|
||||
* @retval 负值 ADS1256 初始化阶段返回的具体错误码。
|
||||
*/
|
||||
int sensor_init(void) {
|
||||
int err = ads1256_init();
|
||||
if (err) return err;
|
||||
|
||||
/* SELFCAL 后 ADC 模拟链路需几个转换周期才能完全建立,空读排空 pipeline */
|
||||
for (int i = 0; i < 4; i++) {
|
||||
ads1256_wait_drdy(50);
|
||||
(void)ads1256_read_data();
|
||||
}
|
||||
|
||||
memset(sensor_offsets, 0, sizeof(sensor_offsets));
|
||||
memset(filtered, 0, sizeof(filtered));
|
||||
|
||||
do_tare();
|
||||
|
||||
k_thread_create(
|
||||
&sensor_thread, sensor_stack, SENSOR_STACK_SIZE, sensor_thread_fn, NULL, NULL, NULL, SENSOR_PRIORITY, 0,
|
||||
K_NO_WAIT);
|
||||
k_thread_name_set(&sensor_thread, "sensor");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 执行四路去皮(零点校准),可从任意线程调用。
|
||||
*
|
||||
* @return 无返回值。
|
||||
*/
|
||||
void sensor_perform_tare(void) {
|
||||
atomic_set(&tare_requested, 1);
|
||||
}
|
||||
@@ -1,6 +0,0 @@
|
||||
# 用绝对路径将 hci_ipc overlay 传给网络核,
|
||||
# 避免 sysbuild 在 SDK 源码目录下解析相对路径而找不到文件。
|
||||
set(hci_ipc_EXTRA_CONF_FILE
|
||||
${CMAKE_CURRENT_LIST_DIR}/sysbuild/hci_ipc.conf
|
||||
CACHE INTERNAL "" FORCE
|
||||
)
|
||||
@@ -1,2 +0,0 @@
|
||||
# 让 sysbuild 自动构建 hci_ipc 网络核固件,west flash 一次刷两个核
|
||||
SB_CONFIG_NETCORE_HCI_IPC=y
|
||||
@@ -1,9 +0,0 @@
|
||||
# 网络核 hci_ipc overlay: 开启 DLE 251B,解除链路层分片瓶颈
|
||||
|
||||
# DLE 依赖链: DATA_LEN_UPDATE -> BT_CTLR_DATA_LENGTH -> DATA_LENGTH_MAX
|
||||
CONFIG_BT_DATA_LEN_UPDATE=y
|
||||
CONFIG_BT_CTLR_DATA_LENGTH_MAX=251
|
||||
|
||||
# 控制器 ACL 缓冲区必须能容纳 251B 的 LL PDU
|
||||
CONFIG_BT_BUF_ACL_RX_SIZE=251
|
||||
CONFIG_BT_BUF_ACL_TX_SIZE=251
|
||||
Reference in New Issue
Block a user