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

[feat] UART command interface, per-motor control, filtering modules

Browse Source 
- Add callback_task with ISR-based UART command parser (@command\n protocol)
- Add log_printf with mutex protection to prevent printf interleaving
- Add per-motor enable/disable (motor enable yaw|pitch)
- Add PID tuning via UART (pid roll kp 15)
- Add cmd_parser module (registration + tokenize + dispatch)
- Add UART layer architecture (interface → HAL → BSP)
- Add filter modules (lowpass, moving_average, notch, rate_limiter)
- Rewrite I2C bus and UART bus modules
- Rewrite PID controller and MF4010V2 motor driver
- Fix soft I2C driver

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
robinson
2026-05-21 01:38:11 +08:00
parent 75966b174d
commit cf934cf6b7
46 changed files with 2372 additions and 2131 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
modules/bus/i2c/i2c_bus.c Normal file
View File

@@ -0,0 +1,34 @@
#include "i2c_bus.h"
#include "i2c_if.h"
#include <stdbool.h>
int i2c_init(i2c_t* obj, int ch) {
if (!obj || obj->initialized) {
return -1;
}
obj->ch = ch;
obj->initialized = true;
i2c_if_init(ch);
return 0;
}
int i2c_mem_write(i2c_t* obj, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, const uint8_t *data, uint16_t size, uint32_t timeout) {
if (!obj || obj->initialized == false)
return -1;
return i2c_if_mem_write(obj->ch, dev_addr, mem_addr, mem_add_size, data, size, timeout);
}
int i2c_mem_read(i2c_t* obj, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, uint8_t *data, uint16_t size, uint32_t timeout) {
if (!obj || obj->initialized == false)
return -1;
return i2c_if_mem_read(obj->ch, dev_addr, mem_addr, mem_add_size, data, size, timeout);
}
int i2c_write(i2c_t* obj, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout) {
if (!obj || obj->initialized == false)
return -1;
return i2c_if_write(obj->ch, dev_addr, data, size, timeout);
}
int i2c_read(i2c_t* obj, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout) {
if (!obj || obj->initialized == false)
return -1;
return i2c_if_read(obj->ch, dev_addr, data, size, timeout);
}

17
modules/bus/i2c/i2c_bus.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef I2C_BUS_H
#define I2C_BUS_H
#include <stdint.h>
#include <stdbool.h>
typedef struct {
int ch;
bool initialized;
} i2c_t;
int i2c_init(i2c_t* obj, int ch);
int i2c_mem_write(i2c_t* obj, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, const uint8_t *data, uint16_t size, uint32_t timeout);
int i2c_mem_read(i2c_t* obj, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, uint8_t *data, uint16_t size, uint32_t timeout);
int i2c_write(i2c_t* obj, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout);
int i2c_read(i2c_t* obj, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout);
#endif

76
modules/bus/uart/uart.c Normal file
View File

@@ -0,0 +1,76 @@
#include "uart.h"
int uart_init(uart_t *uart, int ch, const uart_config_t *cfg)
{
if (!uart || !cfg)
return -1;
uart->ch = ch;
uart->config = *cfg;
int ret = uart_if_init(ch, cfg);
if (ret == 0)
uart->initialized = 1;
return ret;
}
int uart_deinit(uart_t *uart)
{
if (!uart || !uart->initialized)
return -1;
int ret = uart_if_deinit(uart->ch);
if (ret == 0)
uart->initialized = 0;
return ret;
}
int uart_send(uart_t *uart, const uint8_t *data, size_t size, uint32_t timeout)
{
if (!uart || !uart->initialized || !data)
return -1;
return uart_if_send(uart->ch, data, size, timeout);
}
int uart_receive(uart_t *uart, uint8_t *data, size_t size, uint32_t timeout)
{
if (!uart || !uart->initialized || !data)
return -1;
return uart_if_receive(uart->ch, data, size, timeout);
}
int uart_send_it(uart_t *uart, const uint8_t *data, size_t size)
{
if (!uart || !uart->initialized || !data)
return -1;
return uart_if_send_it(uart->ch, data, size);
}
int uart_receive_it(uart_t *uart, uint8_t *data, size_t size)
{
if (!uart || !uart->initialized || !data)
return -1;
return uart_if_receive_it(uart->ch, data, size);
}
int uart_putc(uart_t *uart, uint8_t c)
{
if (!uart || !uart->initialized)
return -1;
return uart_if_putc(uart->ch, c);
}
int uart_getc(uart_t *uart, uint8_t *c)
{
if (!uart || !uart->initialized || !c)
return -1;
return uart_if_getc(uart->ch, c);
}

23
modules/bus/uart/uart.h Normal file
View File

@@ -0,0 +1,23 @@
#ifndef UART_H
#define UART_H
#include <stdint.h>
#include <stddef.h>
#include "uart_if.h"
typedef struct {
int ch;
int initialized;
uart_config_t config;
} uart_t;
int uart_init(uart_t *uart, int ch, const uart_config_t *cfg);
int uart_deinit(uart_t *uart);
int uart_send(uart_t *uart, const uint8_t *data, size_t size, uint32_t timeout);
int uart_receive(uart_t *uart, uint8_t *data, size_t size, uint32_t timeout);
int uart_send_it(uart_t *uart, const uint8_t *data, size_t size);
int uart_receive_it(uart_t *uart, uint8_t *data, size_t size);
int uart_putc(uart_t *uart, uint8_t c);
int uart_getc(uart_t *uart, uint8_t *c);
#endif

51
modules/cmd_parser/cmd_parser.c Normal file
View File

@@ -0,0 +1,51 @@
#include "cmd_parser.h"
#include <string.h>
void cmd_parser_init(cmd_parser_t *parser)
{
parser->count = 0;
}
int cmd_parser_register(cmd_parser_t *parser, const char *name, cmd_handler_t handler)
{
size_t i;
if (parser->count >= CMD_PARSER_MAX_CMDS) return -1;
i = strlen(name);
if (i >= CMD_PARSER_NAME_LEN) i = CMD_PARSER_NAME_LEN - 1;
memcpy(parser->entries[parser->count].name, name, i);
parser->entries[parser->count].name[i] = '\0';
parser->entries[parser->count].handler = handler;
parser->count++;
return 0;
}
int cmd_parser_dispatch(cmd_parser_t *parser, char *buf, void *ctx)
{
char *argv[CMD_PARSER_MAX_ARGS];
int argc = 0;
int i;
char *p = buf;
while (*p) {
while (*p == ' ') p++;
if (*p == '\0') break;
argv[argc++] = p;
if (argc >= CMD_PARSER_MAX_ARGS) break;
while (*p && *p != ' ') p++;
if (*p) *p++ = '\0';
}
if (argc == 0) return -1;
for (i = 0; i < parser->count; i++) {
if (strcmp(argv[0], parser->entries[i].name) == 0) {
return parser->entries[i].handler(ctx, argc, argv);
}
}
return -1;
}

32
modules/cmd_parser/cmd_parser.h Normal file
View File

@@ -0,0 +1,32 @@
#ifndef CMD_PARSER_H
#define CMD_PARSER_H
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
#define CMD_PARSER_MAX_CMDS 32
#define CMD_PARSER_NAME_LEN 20
#define CMD_PARSER_MAX_ARGS 16
typedef int (*cmd_handler_t)(void *ctx, int argc, char **argv);
typedef struct {
struct {
char name[CMD_PARSER_NAME_LEN];
cmd_handler_t handler;
} entries[CMD_PARSER_MAX_CMDS];
int count;
} cmd_parser_t;
void cmd_parser_init(cmd_parser_t *parser);
int cmd_parser_register(cmd_parser_t *parser, const char *name, cmd_handler_t handler);
int cmd_parser_dispatch(cmd_parser_t *parser, char *buf, void *ctx);
#ifdef __cplusplus
}
#endif
#endif

47
modules/control/pid/pid.c
View File

@@ -1,55 +1,56 @@
#include "pid.h"
#include <string.h>
void PIDController_Init(PIDController_t *pid)
void pid_init(pid_t *pid)
{
if (!pid) return;
pid->Kp = 0.0f;
pid->Ki = 0.0f;
pid->Kd = 0.0f;
pid->integral = 0.0f;
pid->prev_error = 0.0f;
pid->output_max = 204800.0f; // 电机最大速度 (0.01 DPS)
pid->prev_measurement = 0.0f;
pid->output_max = 0.0f;
pid->integral_max = 0.0f;
}
float PIDController_Compute(PIDController_t *pid, float target, float actual)
float pid_compute(pid_t *pid, float target, float actual)
{
if (!pid) return 0.0f;
float error = target - actual;
// 比例项
/* 比例项 */
float p_term = pid->Kp * error;
// 积分项 (带抗饱和)
/* 微分项（测量值微分 — 避免目标突变微分冲击） */
float d_term = pid->Kd * (-(actual - pid->prev_measurement));
pid->prev_measurement = actual;
/* 积分项（带抗饱和限幅） */
pid->integral += error;
if (pid->integral > pid->output_max) {
pid->integral = pid->output_max;
} else if (pid->integral < -pid->output_max) {
pid->integral = -pid->output_max;
if (pid->integral_max > 0.0f) {
if (pid->integral > pid->integral_max)
pid->integral = pid->integral_max;
else if (pid->integral < -pid->integral_max)
pid->integral = -pid->integral_max;
}
float i_term = pid->Ki * pid->integral;
// 微分项
float derivative = error - pid->prev_error;
float d_term = pid->Kd * derivative;
pid->prev_error = error;
float output = p_term + i_term + d_term;
// 输出限幅
if (output > pid->output_max) {
output = pid->output_max;
} else if (output < -pid->output_max) {
output = -pid->output_max;
/* 输出限幅（0=不限幅） */
if (pid->output_max > 0.0f) {
if (output > pid->output_max)
output = pid->output_max;
else if (output < -pid->output_max)
output = -pid->output_max;
}
return output;
}
void PIDController_Reset(PIDController_t *pid)
void pid_reset(pid_t *pid)
{
if (!pid) return;
pid->integral = 0.0f;
pid->prev_error = 0.0f;
pid->prev_measurement = 0.0f;
}

19
modules/control/pid/pid.h
View File

@@ -1,22 +1,21 @@
#ifndef __PID_H__
#define __PID_H__
#ifndef PID_H
#define PID_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
typedef struct {
float Kp, Ki, Kd;
float integral;
float prev_error;
float output_max;
} PIDController_t;
float prev_measurement; /* 上一拍测量值（微分用） */
float output_max; /* 输出限幅（0=不限幅） */
float integral_max; /* 积分限幅，抗饱和（0=不限幅） */
} pid_t;
void PIDController_Init(PIDController_t *pid);
float PIDController_Compute(PIDController_t *pid, float target, float actual);
void PIDController_Reset(PIDController_t *pid);
void pid_init(pid_t *pid);
float pid_compute(pid_t *pid, float target, float actual);
void pid_reset(pid_t *pid);
#ifdef __cplusplus
}

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

@@ -1,320 +1,339 @@
/**
* @file mf4010v2.c
* @brief MF4010V2 无刷电机驱动程序
* @note 改进版本：
* - 添加错误处理和返回值
* - 添加 CAN 超时机制
* - 使用安全的内联函数替代危险宏
* - 使用 HAL 层解耦硬件依赖
*/
#include "mf4010v2.h"
#include "can_if.h"
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#define WRITE_FLAG(data, flag) do {(data) |= flag; } while(0)
#define ERASE_FLAG(data, flag) do {(data) &= ~flag; } while(0)
/**
* @brief 初始化电机结构体
*/
int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch)
{
if(motor == NULL) {
return MF4010_ERR_INVALID_PARAM;
}
if(id > 0x7FF) {
printf("Error: Invalid CAN ID 0x%03X (must be 0x000-0x7FF)\r\n", id);
return MF4010_ERR_INVALID_ID;
}
motor->can_ch = can_ch;
motor->id = id;
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;
motor->flags = MF4010_FLAG_ENABLED;
return MF4010_OK;
}
/**
* @brief 发送命令并接收响应（带超时）
*/
int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8])
{
can_message_t msg;
msg.id = motor->id;
memcpy(msg.data, command, 8);
msg.length = 8;
int ret = can_if_send(motor->can_ch, &msg);
if (ret != 0) {
motor->error_count++;
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;
}
/**
* @brief 设置电机目标角度 (位置闭环)
* @param motor 电机结构体
* @param angle_0_01deg 目标角度，单位 0.01 度
*/
int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg)
{
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8];
// 使用增量位置控制 (A7 命令)
// 计算角度增量
int32_t delta = angle_0_01deg - motor->current_angle;
// 限制单次增量在合理范围内
if (delta > 18000) delta = 18000; // 最多 +180 度
if (delta < -18000) delta = -18000; // 最多 -180 度
cmd_incr_pos(cmd, delta);
motor->target_angle = angle_0_01deg;
return mf4010v2_send_command(motor, cmd);
}
/**
* @brief 设置电机速度 (速度闭环)
* @param motor 电机结构体
* @param speed_0_01dps 目标速度，单位 0.01 DPS
*/
int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps)
{
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8];
cmd_speed_control(cmd, speed_0_01dps);
return mf4010v2_send_command(motor, cmd);
}
/**
* @brief 启动电机
*/
void mf4010v2_run(mf4010v2_t* motor)
{
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_RUNNING;
mf4010v2_send_command(motor, cmd);
motor->flags |= MF4010_FLAG_RUNNING;
}
/**
* @brief 停止电机
*/
void mf4010v2_stop(mf4010v2_t* motor)
{
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_STOP;
mf4010v2_send_command(motor, cmd);
motor->flags &= ~MF4010_FLAG_RUNNING;
}
void mf4010v2_close(mf4010v2_t* motor) {
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_CLOSE;
int ret = mf4010v2_send_command(motor, cmd);
if (ret == MF4010_ERR_CAN_SEND) {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return;
} else if (ret == MF4010_ERR_CAN_RECV) {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return;
} else {
if (0 == memcmp(cmd, motor->last_response, sizeof(cmd)/sizeof(cmd[0]))) {
ERASE_FLAG(motor->flags, MF4010_FLAG_ENABLED);
return ;
} else {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return ;
}
}
}
int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) {
if (!motor) return MF4010_ERR_INVALID_PARAM;
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 MF4010_OK;
}
/* ============================================================================
* 命令生成函数实现
* ============================================================================ */
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)(max_speed & 0xFF);
buf[3] = (uint8_t)((max_speed >> 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);
}
bool mf4010v2_is_fault(mf4010v2_t* motor)
{
return (motor != NULL) && (motor->flags & MF4010_FLAG_FAULT);
}
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;
}
/**
* @file mf4010v2.c
* @brief MF4010V2 无刷电机驱动程序
* @note 改进版本：
* - 添加错误处理和返回值
* - 添加 CAN 超时机制
* - 使用安全的内联函数替代危险宏
* - 使用 HAL 层解耦硬件依赖
*/
#include "mf4010v2.h"
#include "can_if.h"
#include <stdint.h>
#include <stdio.h>
#include <string.h>
/* CAN 接收轮询超时（重试次数） */
#define MF4010_RECV_RETRIES 2000
#define WRITE_FLAG(data, flag) do {(data) |= flag; } while(0)
#define ERASE_FLAG(data, flag) do {(data) &= ~flag; } while(0)
/**
* @brief 初始化电机结构体
*/
int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch)
{
if(motor == NULL) {
return MF4010_ERR_INVALID_PARAM;
}
if(id > 0x7FF) {
printf("Error: Invalid CAN ID 0x%03X (must be 0x000-0x7FF)\r\n", id);
return MF4010_ERR_INVALID_ID;
}
motor->can_ch = can_ch;
motor->id = id;
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;
motor->flags = MF4010_FLAG_ENABLED;
return MF4010_OK;
}
/**
* @brief 发送命令并接收响应（带超时轮询）
*/
int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8])
{
can_message_t msg;
int recv_ok = 0;
if (!motor) return MF4010_ERR_INVALID_PARAM;
msg.id = motor->id;
memcpy(msg.data, command, 8);
msg.length = 8;
int ret = can_if_send(motor->can_ch, &msg);
if (ret != 0) {
motor->error_count++;
// printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id);
return MF4010_ERR_CAN_SEND;
}
/* 轮询等待电机响应（带超时） */
for (int retry = 0; retry < MF4010_RECV_RETRIES; retry++) {
ret = can_if_recv(motor->can_ch, &msg);
if (ret == 0 && msg.id == motor->id) {
recv_ok = 1;
break;
}
}
if (!recv_ok) {
motor->error_count++;
printf("Error: Timeout waiting for motor response (ID 0x%03X)\r\n", motor->id);
return MF4010_ERR_NO_RESPONSE;
}
memcpy(motor->last_response, msg.data, 8);
motor->success_count++;
return MF4010_OK;
}
/**
* @brief 设置电机目标角度 (位置闭环)
* @param motor 电机结构体
* @param angle_0_01deg 目标角度，单位 0.01 度
*/
int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg)
{
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8];
// 使用增量位置控制 (A7 命令)
// 计算角度增量
int32_t delta = angle_0_01deg - motor->current_angle;
// 限制单次增量在合理范围内
if (delta > 18000) delta = 18000; // 最多 +180 度
if (delta < -18000) delta = -18000; // 最多 -180 度
cmd_incr_pos(cmd, delta);
motor->target_angle = angle_0_01deg;
return mf4010v2_send_command(motor, cmd);
}
/**
* @brief 设置电机速度 (速度闭环)
* @param motor 电机结构体
* @param speed_0_01dps 目标速度，单位 0.01 DPS
*/
int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps)
{
if (!motor) return MF4010_ERR_INVALID_PARAM;
uint8_t cmd[8];
cmd_speed_control(cmd, speed_0_01dps);
return mf4010v2_send_command(motor, cmd);
}
/**
* @brief 启动电机
*/
void mf4010v2_run(mf4010v2_t* motor)
{
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_RUNNING;
mf4010v2_send_command(motor, cmd);
motor->flags |= MF4010_FLAG_RUNNING;
}
/**
* @brief 停止电机
*/
void mf4010v2_stop(mf4010v2_t* motor)
{
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_STOP;
mf4010v2_send_command(motor, cmd);
motor->flags &= ~MF4010_FLAG_RUNNING;
}
void mf4010v2_close(mf4010v2_t* motor) {
if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_CLOSE;
int ret = mf4010v2_send_command(motor, cmd);
if (ret == MF4010_ERR_CAN_SEND) {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return;
} else if (ret == MF4010_ERR_CAN_RECV) {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return;
} else {
if (0 == memcmp(cmd, motor->last_response, sizeof(cmd)/sizeof(cmd[0]))) {
ERASE_FLAG(motor->flags, MF4010_FLAG_ENABLED);
return ;
} else {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
return ;
}
}
}
int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) {
if (!motor) return MF4010_ERR_INVALID_PARAM;
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 MF4010_OK;
}
/* ============================================================================
* 命令生成函数实现
* ============================================================================ */
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)(max_speed & 0xFF);
buf[3] = (uint8_t)((max_speed >> 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);
}
bool mf4010v2_is_fault(mf4010v2_t* motor)
{
return (motor != NULL) && (motor->flags & MF4010_FLAG_FAULT);
}
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;
}

34
modules/filter/lowpass/lowpass.c Normal file
View File

@@ -0,0 +1,34 @@
#include "lowpass.h"
#ifndef M_PI
#define M_PI (3.14159265358979323846f)
#endif
void lowpass_init(lowpass_t *f, float alpha)
{
if (!f) return;
f->y = 0.0f;
f->alpha = (alpha > 1.0f) ? 1.0f : (alpha < 0.0f) ? 0.0f : alpha;
}
void lowpass_init_fc(lowpass_t *f, float fc, float dt)
{
if (!f || dt <= 0.0f) return;
float rc = 1.0f / (2.0f * M_PI * fc);
float alpha = dt / (rc + dt);
lowpass_init(f, alpha);
}
float lowpass_update(lowpass_t *f, float x)
{
if (!f) return 0.0f;
f->y = f->y + f->alpha * (x - f->y);
return f->y;
}
float lowpass_reset(lowpass_t *f, float x)
{
if (!f) return 0.0f;
f->y = x;
return f->y;
}

22
modules/filter/lowpass/lowpass.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef LOWPASS_H
#define LOWPASS_H
#ifdef __cplusplus
extern "C" {
#endif
typedef struct {
float y; /* 当前滤波输出 */
float alpha; /* 平滑系数 (0..1): 越大截止频率越高 */
} lowpass_t;
void lowpass_init(lowpass_t *f, float alpha);
void lowpass_init_fc(lowpass_t *f, float fc, float dt);
float lowpass_update(lowpass_t *f, float x);
float lowpass_reset(lowpass_t *f, float x);
#ifdef __cplusplus
}
#endif
#endif

46
modules/filter/moving_average/moving_average.c Normal file
View File

@@ -0,0 +1,46 @@
#include "moving_average.h"
#include <string.h>
int movavg_init(movavg_t *f, float *buffer, int size)
{
if (!f || !buffer || size < 1) return -1;
f->buffer = buffer;
f->size = size;
f->index = 0;
f->count = 0;
f->sum = 0.0f;
memset(f->buffer, 0, size * sizeof(float));
return 0;
}
float movavg_update(movavg_t *f, float x)
{
if (!f || !f->buffer || f->size < 1) return 0.0f;
f->sum -= f->buffer[f->index];
f->buffer[f->index] = x;
f->sum += x;
f->index = (f->index + 1) % f->size;
if (f->count < f->size)
f->count++;
return f->sum / (float)f->count;
}
float movavg_get(const movavg_t *f)
{
if (!f || !f->buffer || f->count == 0) return 0.0f;
return f->sum / (float)f->count;
}
void movavg_reset(movavg_t *f)
{
if (!f || !f->buffer) return;
f->index = 0;
f->count = 0;
f->sum = 0.0f;
memset(f->buffer, 0, f->size * sizeof(float));
}

25
modules/filter/moving_average/moving_average.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef MOVING_AVERAGE_H
#define MOVING_AVERAGE_H
#ifdef __cplusplus
extern "C" {
#endif
typedef struct {
float *buffer;
float sum;
int size;
int index;
int count;
} movavg_t;
int movavg_init(movavg_t *f, float *buffer, int size);
float movavg_update(movavg_t *f, float x);
float movavg_get(const movavg_t *f);
void movavg_reset(movavg_t *f);
#ifdef __cplusplus
}
#endif
#endif

46
modules/filter/notch/notch.c Normal file
View File

@@ -0,0 +1,46 @@
#include "notch.h"
#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
void notch_init(notch_t *f, float freq, float Q, float dt)
{
if (!f || dt <= 0.0f || Q <= 0.0f) return;
float w0 = 2.0f * M_PI * freq * dt;
float alpha = sinf(w0) / (2.0f * Q);
float cos_w0 = cosf(w0);
float norm = 1.0f / (1.0f + alpha);
f->b0 = norm;
f->b1 = -2.0f * cos_w0 * norm;
f->b2 = norm;
f->a1 = f->b1;
f->a2 = (1.0f - alpha) * norm;
notch_reset(f);
}
float notch_update(notch_t *f, float x)
{
if (!f) return 0.0f;
float y = f->b0 * x + f->b1 * f->x1 + f->b2 * f->x2
- f->a1 * f->y1 - f->a2 * f->y2;
f->x2 = f->x1;
f->x1 = x;
f->y2 = f->y1;
f->y1 = y;
return y;
}
void notch_reset(notch_t *f)
{
if (!f) return;
f->x1 = f->x2 = 0.0f;
f->y1 = f->y2 = 0.0f;
}

24
modules/filter/notch/notch.h Normal file
View File

@@ -0,0 +1,24 @@
#ifndef NOTCH_H
#define NOTCH_H
#ifdef __cplusplus
extern "C" {
#endif
/* 二阶 IIR 陷波滤波器，用于抑制特定频率的机械共振 */
typedef struct {
float b0, b1, b2;
float a1, a2;
float x1, x2; /* 输入历史 */
float y1, y2; /* 输出历史 */
} notch_t;
void notch_init(notch_t *f, float freq, float Q, float dt);
float notch_update(notch_t *f, float x);
void notch_reset(notch_t *f);
#ifdef __cplusplus
}
#endif
#endif

32
modules/filter/rate_limiter/rate_limiter.c Normal file
View File

@@ -0,0 +1,32 @@
#include "rate_limiter.h"
void rate_limiter_init(rate_limiter_t *rl, float max_rate)
{
if (!rl) return;
rl->y = 0.0f;
rl->max_rate = (max_rate < 0.0f) ? 0.0f : max_rate;
}
float rate_limiter_update(rate_limiter_t *rl, float x, float dt)
{
if (!rl) return 0.0f;
if (dt <= 0.0f) return rl->y;
float max_step = rl->max_rate * dt;
float diff = x - rl->y;
if (diff > max_step)
rl->y += max_step;
else if (diff < -max_step)
rl->y -= max_step;
else
rl->y = x;
return rl->y;
}
void rate_limiter_reset(rate_limiter_t *rl, float x)
{
if (!rl) return;
rl->y = x;
}

22
modules/filter/rate_limiter/rate_limiter.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef RATE_LIMITER_H
#define RATE_LIMITER_H
#ifdef __cplusplus
extern "C" {
#endif
/* 速率限制器 / 斜坡发生器：约束每秒钟的最大变化量 */
typedef struct {
float y; /* 当前输出 */
float max_rate; /* 最大变化率（单位/秒） */
} rate_limiter_t;
void rate_limiter_init(rate_limiter_t *rl, float max_rate);
float rate_limiter_update(rate_limiter_t *rl, float x, float dt);
void rate_limiter_reset(rate_limiter_t *rl, float x);
#ifdef __cplusplus
}
#endif
#endif