ArmDev/motor-controller
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[feat] 添加二轴云台增稳控制和 Soft I2C 模块

Browse Source 
主要变更:
- 二轴云台增稳控制 (control_task.c): 保持 Roll=0, Pitch=0
  - 双 PID 控制器补偿基座倾斜
  - Yaw 电机补偿 Roll 轴，Pitch 电机补偿 Pitch 轴
  - 100Hz 控制循环

- Soft I2C 模块 (modules/bus/soft_i2c/):
  - 基于 GPIO 位时序的 I2C 实现
  - 支持多实例，可适配到 i2c_if 接口
  - 适配层用于 WIT 传感器 (sensor_i2c_port.c)

- MF4010V2 驱动增强:
  - 添加 mf4010v2_set_angle() 位置控制函数
  - 完善命令生成函数文档

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
CloudPlant Team
2026-04-18 21:56:30 +08:00
parent dcb33f8a99
commit 1710e1294e
11 changed files with 1504 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

454
modules/bus/soft_i2c/soft_i2c.c Normal file
View File

@@ -0,0 +1,454 @@
/**
* @file soft_i2c.c
* @brief 软件 I2C（位模拟）驱动程序实现
* @note 基于 GPIO 位时序实现
*/
#include "soft_i2c.h"
#include "cmsis_os2.h"
#include "gpio_if.h"
#include "cmsis_compiler.h"
#include "projdefs.h"
/* ============================================================================
* 内部辅助函数
* ============================================================================ */
/**
* @brief 微秒级延时
* @note 简单软件延时，实际时间取决于编译器优化和主频
* 对于精确时序，建议替换为定时器延时
*/
void soft_i2c_delay_us(uint32_t us)
{
#ifndef
volatile uint32_t count;
/* 假设 168MHz 主频，每个循环约 1us */
/* 根据实际主频调整系数 */
while (us--) {
for (count = 0; count < 21; count++) {
__NOP();
}
}
#else
osDelay(pdMS_TO_TICKS(us/1000u));
#endif
}
/**
* @brief 设置 SDA 输出
*/
static void soft_i2c_sda_out(soft_i2c_t *i2c, uint8_t val)
{
gpio_if_write(i2c->config.sda_gpio_ch, val);
}
/**
* @brief 读取 SDA 输入（释放为输入模式）
* @note 软件 I2C 需要在读取前释放 SDA 线
*/
static uint8_t soft_i2c_sda_in(soft_i2c_t *i2c)
{
/* 读取时 SDA 应为输入模式 */
/* 注意：当前 gpio_if 没有输入/输出模式切换 */
/* 假设 GPIO 已配置为合适的模式 */
return (uint8_t)gpio_if_read(i2c->config.sda_gpio_ch);
}
/**
* @brief 设置 SCL 输出
*/
static void soft_i2c_scl_out(soft_i2c_t *i2c, uint8_t val)
{
gpio_if_write(i2c->config.scl_gpio_ch, val);
}
/**
* @brief 读取 SCL 输入（未使用，保留用于时钟拉伸支持）
*/
#if 0
static uint8_t soft_i2c_scl_in(soft_i2c_t *i2c)
{
return (uint8_t)gpio_if_read(i2c->config.scl_gpio_ch);
}
#endif
/**
* @brief 产生半个 I2C 周期延时
*/
static inline void soft_i2c_half_cycle(soft_i2c_t *i2c)
{
soft_i2c_delay_us(i2c->config.delay_us);
}
/* ============================================================================
* 核心 API 实现
* ============================================================================ */
int soft_i2c_init(soft_i2c_t *i2c, const soft_i2c_config_t *config)
{
if (!i2c || !config) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 复制配置 */
i2c->config = *config;
i2c->bus_busy = false;
i2c->last_error = 0;
/* 初始化 GPIO */
/* 注意：GPIO 模式应在外部配置好（开漏输出 + 上拉） */
if (gpio_if_init(config->sda_gpio_ch) != 0) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
if (gpio_if_init(config->scl_gpio_ch) != 0) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 初始状态：SDA 和 SCL 为高（空闲状态）*/
soft_i2c_sda_out(i2c, 1);
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
return SOFT_I2C_OK;
}
int soft_i2c_start(soft_i2c_t *i2c)
{
if (!i2c) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 检查总线是否忙 */
if (i2c->bus_busy) {
/* 可选：总线忙处理 */
}
/* SCL 高期间，SDA 由高变低 -> 起始条件 */
soft_i2c_sda_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_sda_out(i2c, 0);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 0);
soft_i2c_half_cycle(i2c);
i2c->bus_busy = true;
return SOFT_I2C_OK;
}
int soft_i2c_stop(soft_i2c_t *i2c)
{
if (!i2c) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* SCL 高期间，SDA 由低变高 -> 停止条件 */
soft_i2c_sda_out(i2c, 0);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_sda_out(i2c, 1);
soft_i2c_half_cycle(i2c);
i2c->bus_busy = false;
return SOFT_I2C_OK;
}
int soft_i2c_write_byte(soft_i2c_t *i2c, uint8_t data)
{
uint8_t i;
uint8_t ack;
if (!i2c) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 发送 8 位数据，MSB 在前 */
for (i = 0; i < 8; i++) {
/* SCL 低期间改变 SDA */
soft_i2c_scl_out(i2c, 0);
/* 输出数据位 */
if (data & 0x80) {
soft_i2c_sda_out(i2c, 1);
} else {
soft_i2c_sda_out(i2c, 0);
}
data <<= 1;
soft_i2c_half_cycle(i2c);
/* SCL 高期间数据有效 */
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 0);
}
/* 释放 SDA，准备读取 ACK */
soft_i2c_sda_out(i2c, 1);
soft_i2c_half_cycle(i2c);
/* 读取 ACK/NACK */
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
ack = soft_i2c_sda_in(i2c);
soft_i2c_scl_out(i2c, 0);
soft_i2c_half_cycle(i2c);
if (ack) {
return SOFT_I2C_ERR_NACK;
}
return SOFT_I2C_OK;
}
int soft_i2c_read_byte(soft_i2c_t *i2c, bool ack, uint8_t *data)
{
uint8_t i;
uint8_t result = 0;
if (!i2c || !data) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 释放 SDA（作为输入）*/
soft_i2c_sda_out(i2c, 1);
soft_i2c_half_cycle(i2c);
/* 读取 8 位数据，MSB 在前 */
for (i = 0; i < 8; i++) {
/* SCL 高期间读取数据 */
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
result <<= 1;
if (soft_i2c_sda_in(i2c)) {
result |= 0x01;
}
soft_i2c_scl_out(i2c, 0);
soft_i2c_half_cycle(i2c);
}
*data = result;
/* 发送 ACK/NACK */
if (ack) {
soft_i2c_sda_out(i2c, 0); /* ACK */
} else {
soft_i2c_sda_out(i2c, 1); /* NACK */
}
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 1);
soft_i2c_half_cycle(i2c);
soft_i2c_scl_out(i2c, 0);
soft_i2c_half_cycle(i2c);
return SOFT_I2C_OK;
}
int soft_i2c_send_addr(soft_i2c_t *i2c, uint8_t addr, bool write)
{
uint8_t addr_byte;
if (!i2c) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 7 位地址 + 读写位 */
addr_byte = (addr << 1) | (write ? 0x00 : 0x01);
return soft_i2c_write_byte(i2c, addr_byte);
}
/* ============================================================================
* 高级 API 实现
* ============================================================================ */
int soft_i2c_mem_write(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, const uint8_t *data, uint16_t size,
uint32_t timeout)
{
uint16_t i;
int ret;
(void)timeout; /* 未使用 */
if (!i2c || !data || (mem_addr_size != 1 && mem_addr_size != 2)) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 发送设备地址（写）*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 发送寄存器/内存地址 */
if (mem_addr_size == 2) {
ret = soft_i2c_write_byte(i2c, (uint8_t)(mem_addr >> 8));
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_write_byte(i2c, (uint8_t)mem_addr);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 发送数据 */
for (i = 0; i < size; i++) {
ret = soft_i2c_write_byte(i2c, data[i]);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_stop(i2c);
return ret;
}
int soft_i2c_mem_read(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, uint8_t *data, uint16_t size,
uint32_t timeout)
{
uint16_t i;
int ret;
(void)timeout; /* 未使用 */
if (!i2c || !data || (mem_addr_size != 1 && mem_addr_size != 2)) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
/* 第一阶段：写入寄存器地址 */
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 发送设备地址（写）*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 发送寄存器/内存地址 */
if (mem_addr_size == 2) {
ret = soft_i2c_write_byte(i2c, (uint8_t)(mem_addr >> 8));
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_write_byte(i2c, (uint8_t)mem_addr);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 重复起始条件（Repeated Start）*/
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 第二阶段：读取数据 */
/* 发送设备地址（读）*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, false);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 读取数据 */
for (i = 0; i < size; i++) {
/* 最后一个字节发送 NACK，否则发送 ACK */
ret = soft_i2c_read_byte(i2c, (i < (size - 1)), &data[i]);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_stop(i2c);
return ret;
}
int soft_i2c_write(soft_i2c_t *i2c, uint16_t dev_addr, const uint8_t *data,
uint16_t size, uint32_t timeout)
{
uint16_t i;
int ret;
(void)timeout; /* 未使用 */
if (!i2c || !data) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 发送设备地址（写）*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, true);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 发送数据 */
for (i = 0; i < size; i++) {
ret = soft_i2c_write_byte(i2c, data[i]);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_stop(i2c);
return ret;
}
int soft_i2c_read(soft_i2c_t *i2c, uint16_t dev_addr, uint8_t *data,
uint16_t size, uint32_t timeout)
{
uint16_t i;
int ret;
(void)timeout; /* 未使用 */
if (!i2c || !data) {
return SOFT_I2C_ERR_INVALID_PARAM;
}
ret = soft_i2c_start(i2c);
if (ret != SOFT_I2C_OK) return ret;
/* 发送设备地址（读）*/
ret = soft_i2c_send_addr(i2c, (uint8_t)dev_addr, false);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
/* 读取数据 */
for (i = 0; i < size; i++) {
ret = soft_i2c_read_byte(i2c, (i < (size - 1)), &data[i]);
if (ret != SOFT_I2C_OK) {
soft_i2c_stop(i2c);
return ret;
}
}
ret = soft_i2c_stop(i2c);
return ret;
}

192
modules/bus/soft_i2c/soft_i2c.h Normal file
View File

@@ -0,0 +1,192 @@
#ifndef __SOFT_I2C_H__
#define __SOFT_I2C_H__
/**
* @file soft_i2c.h
* @brief 软件 I2C（位模拟）驱动程序
* @note 基于 GPIO 位时序实现的 I2C 总线驱动
* - 支持多实例（多个独立的 soft I2C 总线）
* - 标准模式 (100kHz) / 快速模式 (400kHz)
* - 7 位设备地址
*/
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#ifdef __cplusplus
extern "C" {
#endif
/* ============================================================================
* 状态码和错误定义
* ============================================================================ */
/**
* @brief Soft I2C 状态码
*/
typedef enum {
SOFT_I2C_OK = 0, /**< 成功 */
SOFT_I2C_ERR_TIMEOUT = -1, /**< 超时错误 */
SOFT_I2C_ERR_NACK = -2, /**< NACK 错误（设备无应答） */
SOFT_I2C_ERR_INVALID_PARAM = -3, /**< 无效参数 */
SOFT_I2C_ERR_BUS_ERROR = -4, /**< 总线错误 */
SOFT_I2C_ERR_ARBITRATION = -5 /**< 仲裁丢失 */
} soft_i2c_status_t;
/* ============================================================================
* 配置结构体
* ============================================================================ */
/**
* @brief Soft I2C 配置结构体
*/
typedef struct {
int sda_gpio_ch; /**< SDA GPIO 通道号 */
int scl_gpio_ch; /**< SCL GPIO 通道号 */
uint32_t delay_us; /**< 半周期延时（微秒），决定总线速率 */
/**< 例：5us = 100kHz, 2.5us = 200kHz, 1.25us = 400kHz */
bool open_drain; /**< 是否开漏模式（默认 true）*/
} soft_i2c_config_t;
/**
* @brief Soft I2C 主机结构体
*/
typedef struct {
soft_i2c_config_t config; /**< 配置参数 */
bool bus_busy; /**< 总线忙标志 */
uint8_t last_error; /**< 最后错误码 */
} soft_i2c_t;
/* ============================================================================
* 核心 API 函数
* ============================================================================ */
/**
* @brief 初始化 Soft I2C 主机
* @param i2c: Soft I2C 结构体指针
* @param config: 配置参数
* @retval 成功返回 SOFT_I2C_OK，失败返回错误码
*/
int soft_i2c_init(soft_i2c_t *i2c, const soft_i2c_config_t *config);
/**
* @brief 产生 I2C 起始条件
* @param i2c: Soft I2C 结构体指针
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_start(soft_i2c_t *i2c);
/**
* @brief 产生 I2C 停止条件
* @param i2c: Soft I2C 结构体指针
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_stop(soft_i2c_t *i2c);
/**
* @brief 发送一个字节
* @param i2c: Soft I2C 结构体指针
* @param data: 要发送的数据
* @retval 成功返回 SOFT_I2C_OK，NACK 返回 SOFT_I2C_ERR_NACK
*/
int soft_i2c_write_byte(soft_i2c_t *i2c, uint8_t data);
/**
* @brief 读取一个字节
* @param i2c: Soft I2C 结构体指针
* @param ack: 是否发送 ACK (true=发送 ACK, false=发送 NACK)
* @param data: 读取到的数据（输出）
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_read_byte(soft_i2c_t *i2c, bool ack, uint8_t *data);
/**
* @brief 发送设备地址（含读写位）
* @param i2c: Soft I2C 结构体指针
* @param addr: 7 位设备地址
* @param write: 读写方向（true=写，false=读）
* @retval 成功返回 SOFT_I2C_OK，NACK 返回 SOFT_I2C_ERR_NACK
*/
int soft_i2c_send_addr(soft_i2c_t *i2c, uint8_t addr, bool write);
/* ============================================================================
* 高级 API 函数（类似硬件 I2C 的操作）
* ============================================================================ */
/**
* @brief I2C 内存写入（带寄存器地址）
* @param i2c: Soft I2C 结构体指针
* @param dev_addr: 7 位设备地址
* @param mem_addr: 寄存器/内存地址
* @param mem_addr_size: 寄存器地址字节数（1 或 2）
* @param data: 要写入的数据
* @param size: 数据长度
* @param timeout: 超时时间（未使用，预留）
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_mem_write(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, const uint8_t *data, uint16_t size,
uint32_t timeout);
/**
* @brief I2C 内存读取（带寄存器地址）
* @param i2c: Soft I2C 结构体指针
* @param dev_addr: 7 位设备地址
* @param mem_addr: 寄存器/内存地址
* @param mem_addr_size: 寄存器地址字节数（1 或 2）
* @param data: 读取的数据缓冲区
* @param size: 数据长度
* @param timeout: 超时时间（未使用，预留）
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_mem_read(soft_i2c_t *i2c, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, uint8_t *data, uint16_t size,
uint32_t timeout);
/**
* @brief I2C 写入（不带寄存器地址）
* @param i2c: Soft I2C 结构体指针
* @param dev_addr: 7 位设备地址
* @param data: 要写入的数据
* @param size: 数据长度
* @param timeout: 超时时间（未使用，预留）
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_write(soft_i2c_t *i2c, uint16_t dev_addr, const uint8_t *data,
uint16_t size, uint32_t timeout);
/**
* @brief I2C 读取（不带寄存器地址）
* @param i2c: Soft I2C 结构体指针
* @param dev_addr: 7 位设备地址
* @param data: 读取的数据缓冲区
* @param size: 数据长度
* @param timeout: 超时时间（未使用，预留）
* @retval 成功返回 SOFT_I2C_OK
*/
int soft_i2c_read(soft_i2c_t *i2c, uint16_t dev_addr, uint8_t *data,
uint16_t size, uint32_t timeout);
/* ============================================================================
* 辅助函数
* ============================================================================ */
/**
* @brief 产生 I2C 时钟脉冲
* @param i2c: Soft I2C 结构体指针
*/
void soft_i2c_generate_clock(soft_i2c_t *i2c);
/**
* @brief 延时函数（微秒级）
* @param us: 延时微秒数
* @note 可替换为实际的延时实现
*/
void soft_i2c_delay_us(uint32_t us);
#ifdef __cplusplus
}
#endif
#endif /* __SOFT_I2C_H__ */

106
modules/bus/soft_i2c/soft_i2c_example.c Normal file
View File

@@ -0,0 +1,106 @@
/**
* @file soft_i2c_example.c
* @brief Soft I2C 使用示例
* @note 演示如何使用 soft_i2c 模块
*/
#include "soft_i2c.h"
#include "soft_i2c_hal.h"
#include "i2c_if.h"
#include <stdint.h>
/* ============================================================================
* 示例 1: 直接使用 soft_i2c API
* ============================================================================ */
void soft_i2c_example_direct(void)
{
soft_i2c_t i2c;
soft_i2c_config_t config = {
.sda_gpio_ch = 0, /* 根据实际硬件修改 */
.scl_gpio_ch = 1,
.delay_us = 5, /* 100kHz */
.open_drain = true
};
uint8_t data[2];
/* 初始化 */
soft_i2c_init(&i2c, &config);
/* 示例：读取 AT24C02 EEPROM (地址 0x50) 寄存器 0x00 的 2 个字节 */
soft_i2c_mem_read(&i2c, 0x50, 0x00, 1, data, 2, 0);
/* 示例：写入 2 个字节 */
soft_i2c_mem_write(&i2c, 0x50, 0x00, 1, data, 2, 0);
}
/* ============================================================================
* 示例 2: 使用 HAL 适配器（通过 i2c_if 接口）
* ============================================================================ */
void soft_i2c_example_hal(void)
{
uint8_t data[16];
/* 初始化 soft_i2c HAL，从通道 2 开始 */
/* 调用后，通道 2 和 3 对应 soft_i2c 总线 */
soft_i2c_hal_init(2);
/* 现在可以使用标准 i2c_if 接口 */
/* 读取第一个 soft_i2c 总线上的设备 */
i2c_if_mem_read(2, 0x50, 0x00, 1, data, 16, 100);
/* 读取第二个 soft_i2c 总线上的设备 */
i2c_if_mem_read(3, 0x68, 0x00, 1, data, 16, 100);
}
/* ============================================================================
* 示例 3: MPU6050 读取示例
* ============================================================================ */
void soft_i2c_example_mpu6050(soft_i2c_t *i2c)
{
uint8_t who_am_i;
uint8_t data[14];
/* 读取 WHO_AM_I 寄存器 (0x75) */
soft_i2c_mem_read(i2c, 0x68, 0x75, 1, &who_am_i, 1, 0);
/* 读取加速度计和陀螺仪数据 (0x3B-0x48) */
soft_i2c_mem_read(i2c, 0x68, 0x3B, 1, data, 14, 0);
/* ACCEL_XOUT_H */
int16_t accel_x = (int16_t)((data[0] << 8) | data[1]);
(void)accel_x;
}
/* ============================================================================
* 示例 4: OLED 显示写入示例 (SSD1306)
* ============================================================================ */
void soft_i2c_example_oled_command(soft_i2c_t *i2c, uint8_t cmd)
{
uint8_t buf[2];
/* SSD1306 命令模式：第一字节 0x00 = 命令 */
buf[0] = 0x00;
buf[1] = cmd;
soft_i2c_write(i2c, 0x3C, buf, 2, 0);
}
void soft_i2c_example_oled_data(soft_i2c_t *i2c, const uint8_t *data, uint16_t size)
{
uint8_t static_buf[129]; /* 静态缓冲区，最大 128 字节数据 + 1 字节命令 */
uint16_t i;
if (size > 128) return; /* 限制最大长度 */
/* SSD1306 数据模式：第一字节 0x40 = 数据 */
static_buf[0] = 0x40;
for (i = 0; i < size; i++) {
static_buf[i + 1] = data[i];
}
soft_i2c_write(i2c, 0x3C, static_buf, size + 1, 0);
}

172
modules/bus/soft_i2c/soft_i2c_hal.c Normal file
View File

@@ -0,0 +1,172 @@
/**
* @file soft_i2c_hal.c
* @brief Soft I2C HAL 适配器实现
* @note 将 soft_i2c 模块适配到现有的 i2c_ops_t 接口
*
* 使用示例：
* @code
* // 初始化 2 个 soft_i2c 总线（通道 2 和 3）
* soft_i2c_hal_init(2);
*
* // 通过标准接口访问
* i2c_if_read(2, 0x50, buffer, 16, 100); // 读取第一个 soft_i2c 总线
* @endcode
*/
#include "soft_i2c_hal.h"
#include "soft_i2c.h"
#include "i2c_if.h"
#include "gpio_if.h"
#include <stddef.h>
/* ============================================================================
* 配置区域
* ============================================================================ */
/**
* @brief 最大支持的 soft_i2c 总线数量
*/
#ifndef SOFT_I2C_MAX_CHANNELS
#define SOFT_I2C_MAX_CHANNELS 2
#endif
/**
* @brief Soft I2C 总线配置表
* @note 根据实际硬件连接修改引脚配置
*/
static const soft_i2c_config_t soft_i2c_configs[SOFT_I2C_MAX_CHANNELS] = {
{
.sda_gpio_ch = 0, /* SDA1 - 修改为实际 GPIO 通道 */
.scl_gpio_ch = 1, /* SCL1 */
.delay_us = 5, /* 5us = 约 100kHz */
.open_drain = true
},
{
.sda_gpio_ch = 2, /* SDA2 - 修改为实际 GPIO 通道 */
.scl_gpio_ch = 3, /* SCL2 */
.delay_us = 2, /* 2us = 约 250kHz */
.open_drain = true
}
};
/**
* @brief Soft I2C 主机实例
*/
static soft_i2c_t soft_i2c_hosts[SOFT_I2C_MAX_CHANNELS];
/**
* @brief 通道偏移量（soft_i2c 在 i2c_if 中的起始通道号）
*/
static int g_channel_offset = -1;
/* ============================================================================
* i2c_ops_t 接口实现
* ============================================================================ */
static int soft_i2c_hal_init_impl(int ch)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_init(&soft_i2c_hosts[local_ch], &soft_i2c_configs[local_ch]);
}
static int soft_i2c_hal_mem_write(int ch, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, const uint8_t *data,
uint16_t size, uint32_t timeout)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_mem_write(&soft_i2c_hosts[local_ch], dev_addr, mem_addr,
mem_addr_size, data, size, timeout);
}
static int soft_i2c_hal_mem_read(int ch, uint16_t dev_addr, uint16_t mem_addr,
uint16_t mem_addr_size, uint8_t *data,
uint16_t size, uint32_t timeout)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_mem_read(&soft_i2c_hosts[local_ch], dev_addr, mem_addr,
mem_addr_size, data, size, timeout);
}
static int soft_i2c_hal_write(int ch, uint16_t dev_addr, const uint8_t *data,
uint16_t size, uint32_t timeout)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_write(&soft_i2c_hosts[local_ch], dev_addr, data, size, timeout);
}
static int soft_i2c_hal_read(int ch, uint16_t dev_addr, uint8_t *data,
uint16_t size, uint32_t timeout)
{
int local_ch = ch - g_channel_offset;
if (local_ch < 0 || local_ch >= SOFT_I2C_MAX_CHANNELS) {
return -1;
}
return soft_i2c_read(&soft_i2c_hosts[local_ch], dev_addr, data, size, timeout);
}
/**
* @brief soft_i2c 的 i2c_ops_t 实现
*/
static const i2c_ops_t soft_i2c_ops = {
.init = soft_i2c_hal_init_impl,
.mem_write = soft_i2c_hal_mem_write,
.mem_read = soft_i2c_hal_mem_read,
.write = soft_i2c_hal_write,
.read = soft_i2c_hal_read
};
/* ============================================================================
* 公共 API 实现
* ============================================================================ */
int soft_i2c_hal_init(int channel_offset)
{
int i;
if (g_channel_offset >= 0) {
/* 已初始化 */
return -1;
}
g_channel_offset = channel_offset;
/* 初始化所有 soft_i2c 通道 */
for (i = 0; i < SOFT_I2C_MAX_CHANNELS; i++) {
int ret = soft_i2c_init(&soft_i2c_hosts[i], &soft_i2c_configs[i]);
if (ret != SOFT_I2C_OK) {
/* 初始化失败，但仍继续 */
}
}
/* 注册到 i2c_if */
i2c_register_ops(&soft_i2c_ops);
return 0;
}
int soft_i2c_hal_get_channel_count(void)
{
return SOFT_I2C_MAX_CHANNELS;
}

34
modules/bus/soft_i2c/soft_i2c_hal.h Normal file
View File

@@ -0,0 +1,34 @@
/**
* @file soft_i2c_hal.h
* @brief Soft I2C HAL 适配器头文件
* @note 将 soft_i2c 适配到现有的 i2c_ops_t 接口
*/
#ifndef __SOFT_I2C_HAL_H__
#define __SOFT_I2C_HAL_H__
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化 soft_i2c HAL 层并注册到 i2c_if
* @param channel_offset: 通道偏移量（soft_i2c 从该通道号开始）
* @retval 成功返回 0，失败返回 -1
* @note 调用后，soft_i2c 可通过 i2c_if_* 接口访问
* 例如：i2c_if_read(channel_offset, dev_addr, data, size, timeout)
*/
int soft_i2c_hal_init(int channel_offset);
/**
* @brief 获取 soft_i2c 通道数量
*/
int soft_i2c_hal_get_channel_count(void);
#ifdef __cplusplus
}
#endif
#endif /* __SOFT_I2C_HAL_H__ */

193
modules/device/motor/mf4010v2.c
View File

@@ -33,6 +33,7 @@ int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch)
motor->target_angle = 0;
motor->current_angle = 0;
memset(motor->last_response, 0, sizeof(motor->last_response));
can_if_init(motor->can_ch);
motor->last_comm_time = 0;
motor->error_count = 0;
motor->success_count = 0;
@@ -55,6 +56,12 @@ int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8])
printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id);
return MF4010_ERR_CAN_SEND;
}
can_if_recv(motor->can_ch, &msg);
if (msg.id != motor->id) {
motor->error_count++;
return MF4010_ERR_CAN_RECV;
}
memcpy(motor->last_response, msg.data, 8);
motor->success_count++;
return MF4010_OK;
}
@@ -80,7 +87,7 @@ int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg)
cmd_position_add(cmd, delta);
motor->target_angle = angle_0_01deg;
return mf4010v2_send_command(motor, motor->can_ch, cmd);
return mf4010v2_send_command(motor, cmd);
}
/**
@@ -94,7 +101,7 @@ int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps)
uint8_t cmd[8];
cmd_speed_control(cmd, speed_0_01dps);
return mf4010v2_send_command(motor, motor->can_ch, cmd);
return mf4010v2_send_command(motor, cmd);
}
/**
@@ -104,7 +111,7 @@ void mf4010v2_run(mf4010v2_t* motor)
{
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_RUNNING;
mf4010v2_send_command(motor, motor->can_ch, cmd);
mf4010v2_send_command(motor, cmd);
motor->flags |= MF4010_FLAG_RUNNING;
}
@@ -115,17 +122,177 @@ void mf4010v2_stop(mf4010v2_t* motor)
{
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_STOP;
mf4010v2_send_command(motor, motor->can_ch, cmd);
mf4010v2_send_command(motor, cmd);
motor->flags &= ~MF4010_FLAG_RUNNING;
}
/**
* @brief 获取当前速度 (返回目标速度，因为实际速度需要从编码器反馈)
*/
int32_t mf4010v2_get_speed(mf4010v2_t* motor)
{
if (!motor) return 0;
uint8_t cmd[8] =
mf4010v2_send_command(motor, motor->can_ch, cmd);
return motor->target_angle; // 占位返回
void mf4010v2_close(mf4010v2_t* motor) {
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_CLOSE;
mf4010v2_send_command(motor, cmd);
motor->flags &= ~MF4010_FLAG_ENABLED;
}
int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) {
if (!motor) return;
uint8_t cmd[8] = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
mf4010v2_send_command(motor, cmd);
pid->angle_kp = motor->last_response[2];
pid->angle_ki = motor->last_response[3];
pid->speed_kp = motor->last_response[4];
pid->speed_ki = motor->last_response[5];
pid->iq_kp = motor->last_response[6];
pid->iq_ki = motor->last_response[7];
return 0;
}
/* ============================================================================
* 命令生成函数实现
* ============================================================================ */
void cmd_torque_control(uint8_t* buf, int16_t iq)
{
buf[0] = 0xA1;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(iq & 0xFF);
buf[5] = (uint8_t)((iq >> 8) & 0xFF);
buf[6] = 0x00;
buf[7] = 0x00;
}
void cmd_speed_control(uint8_t* buf, int32_t speed)
{
buf[0] = 0xA2;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(speed & 0xFF);
buf[5] = (uint8_t)((speed >> 8) & 0xFF);
buf[6] = (uint8_t)((speed >> 16) & 0xFF);
buf[7] = (uint8_t)((speed >> 24) & 0xFF);
}
void cmd_mult_ring_pos(uint8_t* buf, int32_t angleControl) {
buf[0] = 0xA3;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = *(uint8_t *)(&angleControl);
buf[5] = *((uint8_t *)(&angleControl)+1);
buf[6] = *((uint8_t *)(&angleControl)+2);
buf[7] = *((uint8_t *)(&angleControl)+3);
}
void cmd_mult_ring_pos_with_speed(uint8_t* buf, uint16_t maxSpeed, int32_t angleControl) {
buf[0] = 0xA4;
buf[1] = 0x00;
buf[2] = *(uint8_t *)(&maxSpeed);
buf[3] = *((uint8_t *)(&maxSpeed)+1);
buf[4] = *(uint8_t *)(&angleControl);
buf[5] = *((uint8_t *)(&angleControl)+1);
buf[6] = *((uint8_t *)(&angleControl)+2);
buf[7] = *((uint8_t *)(&angleControl)+3);
}
void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle)
{
buf[0] = 0xA5;
buf[1] = direction & 0x01;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(angle & 0xFF);
buf[5] = (uint8_t)((angle >> 8) & 0xFF);
buf[6] = (uint8_t)((angle >> 16) & 0xFF);
buf[7] = (uint8_t)((angle >> 24) & 0xFF);
}
void cmd_single_ring_pos_with_speed(uint8_t* buf, uint8_t spinDirection, uint16_t maxSpeed, uint32_t angleControl)
{
buf[0] = 0xA6;
buf[1] = spinDirection & 0x01;
buf[2] = (uint8_t)(maxSpeed & 0xFF);
buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF);
buf[4] = (uint8_t)(angleControl & 0xFF);
buf[5] = (uint8_t)((angleControl >> 8) & 0xFF);
buf[6] = (uint8_t)((angleControl >> 16) & 0xFF);
buf[7] = (uint8_t)((angleControl >> 24) & 0xFF);
}
void cmd_incr_pos(uint8_t* buf, int32_t angle_increment)
{
buf[0] = 0xA7;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(angle_increment & 0xFF);
buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF);
buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF);
buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF);
}
void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment) {
buf[0] = 0xA8;
buf[1] = 0x00;
buf[2] = (uint8_t)(maxSpeed & 0xFF);
buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF);
buf[4] = (uint8_t)(angle_increment & 0xFF);
buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF);
buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF);
buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF);
}
void cmd_write_pid_params(uint8_t* buf, uint8_t location,
uint8_t angle_kp, uint8_t angle_ki,
uint8_t speed_kp, uint8_t speed_ki,
uint8_t iq_kp, uint8_t iq_ki)
{
buf[0] = location;
buf[1] = 0x00;
buf[2] = angle_kp;
buf[3] = angle_ki;
buf[4] = speed_kp;
buf[5] = speed_ki;
buf[6] = iq_kp;
buf[7] = iq_ki;
}
/* ============================================================================
* 辅助函数实现
* ============================================================================ */
bool mf4010v2_is_connected(mf4010v2_t* motor)
{
return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED);
}
bool mf4010v2_is_running(mf4010v2_t* motor)
{
return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING);
}
uint16_t mf4010v2_get_error_count(mf4010v2_t* motor)
{
return (motor != NULL) ? motor->error_count : 0;
}
void mf4010v2_reset_errors(mf4010v2_t* motor)
{
if(motor != NULL) {
motor->error_count = 0;
}
}
bool mf4010v2_response_has_error(mf4010v2_t* motor)
{
return (motor != NULL) && ((motor->last_response[0] & 0x80) != 0);
}
uint8_t mf4010v2_get_error_code(mf4010v2_t* motor)
{
return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0;
}

173
modules/device/motor/mf4010v2.h
View File

@@ -40,7 +40,8 @@ typedef enum {
MF4010_ERR_BUS_OFF = -5, /**< CAN 总线关闭 */
MF4010_ERR_CRC = -6, /**< CRC 校验错误 */
MF4010_ERR_HARDWARE = -7, /**< 硬件错误 */
MF4010_ERR_CAN_SEND = -8 /**< CAN 发送失败 */
MF4010_ERR_CAN_SEND = -8, /**< CAN 发送失败 */
MF4010_ERR_CAN_RECV = -9 /**< CAN 接收失败 */
} mf4010_status_t;
/**
@@ -63,7 +64,7 @@ typedef enum {
*/
struct __mf4010v2_t {
typedef struct __mf4010v2_t {
int can_ch; /**< CAN 通道 (0 或 1) */
uint16_t id; /**< CAN ID (0x000-0x7FF) */
uint8_t last_response[8]; /**< 最后一次响应数据 */
@@ -73,10 +74,17 @@ struct __mf4010v2_t {
uint8_t flags; /**< 状态标志 (@ref mf4010_flag_t) */
int32_t target_angle; /**< 目标角度 (0.01 度) */
int32_t current_angle; /**< 当前角度 (0.01 度) - 从编码器读取 */
};
} mf4010v2_t;
typedef struct {
uint8_t angle_kp;
uint8_t angle_ki;
uint8_t speed_kp;
uint8_t speed_ki;
uint8_t iq_kp;
uint8_t iq_ki;
} pid_param;
/* ============================================================================
@@ -95,16 +103,18 @@ int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch);
/**
* @brief 发送命令并接收响应（带超时）
* @param motor: 电机结构体指针
* @param can_ch: CAN 通道
* @param command: 8 字节命令数据
* @retval 成功返回 MF4010_OK，失败返回错误码
*/
int mf4010v2_send_command(mf4010v2_t* motor, int can_ch, const uint8_t command[8]);
int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8]);
void mf4010v2_run(mf4010v2_t* motor);
void mf4010v2_stop(mf4010v2_t* motor);
void mf4010v2_close(mf4010v2_t* motor);
int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps);
int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg);
int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid);
int32_t mf4010v2_get_speed(mf4010v2_t* motor);
/* ============================================================================
@@ -145,17 +155,7 @@ int32_t mf4010v2_get_speed(mf4010v2_t* motor);
* @param iq: 转矩控制量 (-2048 ~ 2048)
* @note 分辨率：1 LSB = 1 单位
*/
static inline void cmd_torque_control(uint8_t* buf, int16_t iq)
{
buf[0] = 0xA1;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(iq & 0xFF);
buf[5] = (uint8_t)((iq >> 8) & 0xFF);
buf[6] = 0x00;
buf[7] = 0x00;
}
void cmd_torque_control(uint8_t* buf, int16_t iq);
/**
* @brief 速度闭环控制命令
@@ -167,17 +167,26 @@ static inline void cmd_torque_control(uint8_t* buf, int16_t iq)
* uint8_t cmd[8];
* cmd_speed_control(cmd, 10000); // 100 DPS
*/
static inline void cmd_speed_control(uint8_t* buf, int32_t speed)
{
buf[0] = 0xA2;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(speed & 0xFF);
buf[5] = (uint8_t)((speed >> 8) & 0xFF);
buf[6] = (uint8_t)((speed >> 16) & 0xFF);
buf[7] = (uint8_t)((speed >> 24) & 0xFF);
}
void cmd_speed_control(uint8_t* buf, int32_t speed);
/**
* @brief 多圈位置闭环控制命令
* @param buf
* @param angleControl 0.01degree/LSB 36000表示360°
*/
void cmd_mult_ring_pos(uint8_t* buf, int32_t angleControl);
/**
* @brief 多圈位置闭环控制命令带速度限制
* @param buf
* @param maxSpeed 表示实际转速1dp/LSB 360表示360 dps
* @param angleControl 0.01degree/LSB 36000表示360°
*/
void cmd_mult_ring_pos_with_speed(uint8_t* buf, uint16_t maxSpeed, int32_t angleControl);
/**
* @brief 单圈位置闭环控制命令
@@ -185,37 +194,16 @@ static inline void cmd_speed_control(uint8_t* buf, int32_t speed)
* @param direction: 旋转方向 (0x00=顺时针，0x01=逆时针)
* @param angle: 目标角度 (单位：0.01°, 如 36000=360°)
*/
static inline void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle)
{
buf[0] = 0xA5;
buf[1] = direction & 0x01;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(angle & 0xFF);
buf[5] = (uint8_t)((angle >> 8) & 0xFF);
buf[6] = (uint8_t)((angle >> 16) & 0xFF);
buf[7] = (uint8_t)((angle >> 24) & 0xFF);
}
void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle);
/**
* @brief 增量位置闭环控制命令（带最大速度）
* @brief 单圈位置闭环控制命令（带最大速度）
* @param buf: 8 字节缓冲区（输出）
* @param direction: 旋转方向 (0x00=顺时针，0x01=逆时针)
* @param max_speed: 最大速度 (单位：DPS, 如 360=360 DPS)
* @param angle: 目标角度 (单位：0.01°, 如 36000=360°)
* @param spinDirection: 旋转方向 (0x00=顺时针，0x01=逆时针)
* @param maxSpeed: 最大速度 (单位：DPS, 如 360=360 DPS)
* @param angleControl: 目标角度 (单位：0.01°, 如 36000=360°)
*/
static inline void cmd_incr_pos_with_speed(uint8_t* buf, uint8_t direction,
uint16_t max_speed, uint32_t angle)
{
buf[0] = 0xA6;
buf[1] = direction & 0x01;
buf[2] = (uint8_t)(max_speed & 0xFF);
buf[3] = (uint8_t)((max_speed >> 8) & 0xFF);
buf[4] = (uint8_t)(angle & 0xFF);
buf[5] = (uint8_t)((angle >> 8) & 0xFF);
buf[6] = (uint8_t)((angle >> 16) & 0xFF);
buf[7] = (uint8_t)((angle >> 24) & 0xFF);
}
void cmd_single_ring_pos_with_speed(uint8_t* buf, uint8_t spinDirection,
uint16_t maxSpeed, uint32_t angleControl);
/**
* @brief 增量位置闭环控制命令
@@ -223,17 +211,18 @@ static inline void cmd_incr_pos_with_speed(uint8_t* buf, uint8_t direction,
* @param angle_increment: 角度增量 (单位：0.01°, 如 36000=360°)
* @note 正值 = 逆时针，负值 = 顺时针
*/
static inline void cmd_position_add(uint8_t* buf, int32_t angle_increment)
{
buf[0] = 0xA7;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = (uint8_t)(angle_increment & 0xFF);
buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF);
buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF);
buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF);
}
void cmd_incr_pos(uint8_t* buf, int32_t angle_increment);
/**
* @brief 增量位置闭环控制命令（带最大速度）
* @param buf: 8 字节缓冲区（输出）
* @param max_speed 1dps/LSB
* @param angle_increment: 角度增量 (单位：0.01°, 如 36000=360°)
* @note 正值 = 逆时针，负值 = 顺时针
*/
void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment);
/**
* @brief PID 参数写入命令
@@ -246,20 +235,10 @@ static inline void cmd_position_add(uint8_t* buf, int32_t angle_increment)
* @param iq_kp: 电流环 Kp (0-255)
* @param iq_ki: 电流环 Ki (0-255)
*/
static inline void cmd_write_pid_params(uint8_t* buf, uint8_t location,
uint8_t angle_kp, uint8_t angle_ki,
uint8_t speed_kp, uint8_t speed_ki,
uint8_t iq_kp, uint8_t iq_ki)
{
buf[0] = location;
buf[1] = 0x00;
buf[2] = angle_kp;
buf[3] = angle_ki;
buf[4] = speed_kp;
buf[5] = speed_ki;
buf[6] = iq_kp;
buf[7] = iq_ki;
}
void cmd_write_pid_params(uint8_t* buf, uint8_t location,
uint8_t angle_kp, uint8_t angle_ki,
uint8_t speed_kp, uint8_t speed_ki,
uint8_t iq_kp, uint8_t iq_ki);
/* ============================================================================
* 辅助函数和宏
@@ -268,36 +247,22 @@ static inline void cmd_write_pid_params(uint8_t* buf, uint8_t location,
/**
* @brief 检查电机是否在线
*/
static inline bool mf4010v2_is_connected(mf4010v2_t* motor)
{
return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED);
}
bool mf4010v2_is_connected(mf4010v2_t* motor);
/**
* @brief 检查电机是否运行
*/
static inline bool mf4010v2_is_running(mf4010v2_t* motor)
{
return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING);
}
bool mf4010v2_is_running(mf4010v2_t* motor);
/**
* @brief 获取电机错误计数
*/
static inline uint16_t mf4010v2_get_error_count(mf4010v2_t* motor)
{
return (motor != NULL) ? motor->error_count : 0;
}
uint16_t mf4010v2_get_error_count(mf4010v2_t* motor);
/**
* @brief 重置错误计数
*/
static inline void mf4010v2_reset_errors(mf4010v2_t* motor)
{
if(motor != NULL) {
motor->error_count = 0;
}
}
void mf4010v2_reset_errors(mf4010v2_t* motor);
/**
* @brief 获取响应中的命令字节
@@ -313,18 +278,12 @@ static inline void mf4010v2_reset_errors(mf4010v2_t* motor)
* @brief 检查响应是否包含错误
* @note 如果响应字节 0 的最高位为 1，表示有错误
*/
static inline bool mf4010v2_response_has_error(mf4010v2_t* motor)
{
return (motor != NULL) && ((motor->last_response[0] & 0x80) != 0);
}
bool mf4010v2_response_has_error(mf4010v2_t* motor);
/**
* @brief 获取错误码（从响应中）
*/
static inline uint8_t mf4010v2_get_error_code(mf4010v2_t* motor)
{
return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0;
}
uint8_t mf4010v2_get_error_code(mf4010v2_t* motor);
#ifdef __cplusplus
}