cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(angle): 引入角度卡尔曼滤波器 EncoderAngleKalman

Browse Source 
新增 EncoderAngleKalman 类以处理编码器角度噪声，改善角度反馈质量，并配合速度滤波器提供更准确的速度估算。

perf(CAN): 优化CAN数据打包与状态标识字段
改进 pack_u16_to_bytes 等函数实现方式，使用 reinterpret_cast 提高效率；
扩展 Status_code 为多个独立状态位，增强诊断能力。

feat(FOC): 添加电机锁止和停止功能
新增 drv_stop 和 drv_hold 方法用于控制电机占空比，支持紧急制动与角度保持。优化了FOC控制器的接口设计，提升安全性和可控性。

fix(filter): 修正中值滤波器数组大小动态分配问题
将 medium_filter 函数中的排序数组改为使用传入参数 size 动态定义，避免固定大小带来的资源浪费或越界风险，提高滤波精度和稳定性。

chore(project): 移除冗余文件及代码清理
移除了 thermal.cpp/h 文件及相关引用，调整项目结构，删除未使用的全局变量定义、注释和头文件包含，精简系统初始化流程。
This commit is contained in:
cbb
2025-11-22 23:59:43 +08:00
parent a7a3805839
commit 59f161bd73
19 changed files with 310 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/ADC.cpp
+1 -1
View File
@@ -10,7 +10,6 @@ constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float ADC_SCALE = 3.3f / 4096.0f;
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
constexpr float VCC_FACTOR = 11.0f;
// constexpr float CURRENT_OFFSET = 0 - 4096.0f / 2.0f;
constexpr float CURRENT_OFFSET = -1.60f;
constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
@@ -21,6 +20,7 @@ constexpr float MIN_CURRENT = -40.0f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
extern CAN_transmiter CAN0_transmiter;
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
Robot_V2/device/Can.cpp
+9 -6
View File
@@ -20,7 +20,7 @@ constexpr u16 DEV_CAN0_MASK = 0x0000;
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
extern CAN_transmiter CAN0_transmiter;
CAN_transmiter CAN0_transmiter; // CAN初始化
extern FOC_Controlroller FOC;
extern sampler sampler;
extern angle_sensor KTH7823;
@@ -432,10 +432,10 @@ void CAN_transmiter::update(void) {
mcu_temp = sampler.get_mcu_temp() * 100;
motor_temp = sampler.get_drv_temp() * 100;
Power_Voltage = sampler.get_vbus_voltage() * 1000;
rt_angle_union.f = KTH7823.getAngle();
rt_angle_union.f = FOC.rt_angle;
// rt_velocity_union.f = KTH7823.getVelocity();
rt_velocity_union.f = FOC.rt_velocity;
Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
MotorStatus = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
Target_Control_Mode = FOC.control_mode;
controller_Mode = FOC.control_mode;
@@ -591,7 +591,7 @@ u8 CAN_transmiter::CAN_all_Send() {
success_flag += CAN_Send(Current_FEED, 6, transmit_data);
// 打包Status_FEED
transmit_data[0] = Status_code;
transmit_data[0] = MotorStatus;
transmit_data[1] = controller_Mode;
pack_u16_to_bytes(transmit_data, 2, motor_temp);
pack_u16_to_bytes(transmit_data, 4, mcu_temp);
@@ -621,7 +621,11 @@ u8 CAN_transmiter::CANFD_all_Send() {
pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 6, u16(w_current_union.f));
pack_u16_to_bytes(transmit_data, 8, u16(v_current_union.f));
transmit_data[10] = Status_code;
transmit_data[10] = (transmit_data[10] & ~0x07) |
((MotorStatus & 0x01) |
((MagStatus & 0x01) << 1) |
((ThermalStatus & 0x01) << 2));
pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
pack_float_to_bytes(transmit_data, 13, rt_angle_union);
pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
@@ -645,7 +649,6 @@ u8 CAN_transmiter::CANFD_all_Send() {
* 1: 有新消息
*/
void CAN_transmiter::communication_check() {
uint8_t i = 0;
// CAN0 发送数据标志
if (SET == can0_send_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
Robot_V2/device/Can.h
+19 -26
View File
@@ -17,39 +17,30 @@ typedef union {
} float_union_t;
// 添加辅助函数用于将u16类型数据打包到u8数组中
static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
buffer[offset] = (u8)(value & 0xFF);
buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
static inline void pack_u16_to_bytes(u8 * const buffer, const u16 offset, const u16 value) {
*(reinterpret_cast<u16*>(buffer + offset)) = value;
}
// 添加辅助函数用于将float类型数据打包到u8数组中
static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
float_union_t &float_union) {
buffer[offset] = float_union.b[0];
buffer[offset + 1] = float_union.b[1];
buffer[offset + 2] = float_union.b[2];
buffer[offset + 3] = float_union.b[3];
static inline void pack_float_to_bytes(u8 * const buffer, const u16 offset,
const float_union_t &float_union) {
*(reinterpret_cast<u32*>(buffer + offset)) = *(reinterpret_cast<const u32*>(float_union.b));
}
// 添加辅助函数用于从u8数组中解包short类型数据
static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) {
short value = (short)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
static inline short unpack_bytes_to_short(const u8 * const buffer, const u16 offset) {
return *(reinterpret_cast<const short*>(buffer + offset));
}
// 添加辅助函数用于从u8数组中解包u16类型数据
static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) {
u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
static inline u16 unpack_bytes_to_u16(const u8 * const buffer, const u16 offset) {
return *(reinterpret_cast<const u16*>(buffer + offset));
}
// 添加辅助函数用于从u8数组中解包float类型数据
static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
float_union_t float_union;
float_union.b[0] = buffer[offset];
float_union.b[1] = buffer[offset + 1];
float_union.b[2] = buffer[offset + 2];
float_union.b[3] = buffer[offset + 3];
static inline float_union_t unpack_bytes_to_float(const u8 * const buffer, const u16 offset) {
static float_union_t float_union;
*(reinterpret_cast<u32*>(float_union.b)) = *(reinterpret_cast<const u32*>(buffer + offset));
return float_union;
}
@@ -79,7 +70,7 @@ class CAN_transmiter {
u16 Power_Voltage = 0; // 电源电压
float_union_t rt_angle_union = {0}; // 实时角度
float_union_t rt_velocity_union = {0}; // 实时速度
u8 Status_code = 0; // 状态码
bool MotorStatus = 0; // 电机状态码
float_union_t target_angle_union = {0}; // 目标角度
u8 Target_Control_Mode = 0; // 目标控制模式
float_union_t target_velocity_union = {0}; // 目标速度
@@ -103,12 +94,14 @@ class CAN_transmiter {
// 添加构造函数声明
CAN_transmiter();
can_receive_message_struct receive_message; // 接收消息结构体
FlagStatus can0_receive_flag; // CAN0接收标志
FlagStatus can0_send_flag; // CAN0发送标志
can_receive_message_struct receive_message; // 接收消息结构体
FlagStatus can0_receive_flag; // CAN0接收标志
FlagStatus can0_send_flag; // CAN0发送标志
float_union_t testfloat_union = {0}; // 测试用
void communication_check(); // 通信检查
bool MagStatus = 0; // 磁状态码
bool ThermalStatus = 0; // 热能状态码
void communication_check(); // 通信检查
};
#ifdef __cplusplus
Robot_V2/device/Timer.cpp
+9 -8
View File
@@ -1,4 +1,5 @@
#include "Timer.h"
#include "Can.h"
#include "DengFOC.h"
#include "Timer.h"
#include "gd32c10x.h"
@@ -28,6 +29,7 @@ static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
extern u16 adc_value[7];
extern FOC_Controlroller FOC;
extern TaskScheduler scheduler;
extern CAN_transmiter CAN0_transmiter;
// 添加系统运行时间计数器
static volatile std::u64 system_runtime_us = 0;
@@ -179,6 +181,7 @@ void TIMER2_IRQHandler(void) {
break;
}
case 4: {
CAN0_transmiter.can0_send_flag = SET;
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
break;
}
@@ -198,6 +201,7 @@ void TIMER2_IRQHandler(void) {
break;
}
case 2: {
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
break;
}
}
@@ -275,7 +279,7 @@ void timer1_gpio_config(void) {
gpio_bit_set(GPIOB, GPIO_PIN_0);
gpio_bit_set(GPIOB, GPIO_PIN_12);
/* 配置TIMER1完全重映射 - 应该使用DISABLE，因为我们使用的是默认引脚 */
/* 配置TIMER1完全重映射*/
gpio_pin_remap_config(GPIO_TIMER1_FULL_REMAP, DISABLE);
}
@@ -358,7 +362,6 @@ extern "C" void TIMER1_IRQHandler(void) {
timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3);
scheduler.taskFlags |= (1 << TASK_CURRENT_UPDATE);
// 在这里添加需要在PWM周期90%位置执行的代码
// 例如触发ADC采样或其他操作
}
}
@@ -391,10 +394,8 @@ void Set_Pwm(u16 channel, float duty_cycle) {
\note The channel parameter must be between 0 and 3 inclusive.
*/
void Set_3Pwm(float duty_cycle0, float duty_cycle1, float duty_cycle2) {
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
duty_cycle0 * TIMER1_PERIOD);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1,
duty_cycle1 * TIMER1_PERIOD);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
duty_cycle2 * TIMER1_PERIOD);
// 直接写寄存器
TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD);
TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD);
TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD);
}
Robot_V2/device/ang_sensor.cpp
+9 -4
View File
@@ -1,11 +1,13 @@
#include "ang_sensor.h"
#include "Filter.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include "initial.h"
#include <cstdio>
#include <cstring>
#include <stdint.h>
#include "initial.h"
#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
#define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
@@ -17,12 +19,16 @@ constexpr u16 FAULT_RECEIVE = 0xFFFF;
constexpr float VELOCITY_SCALE = 1e4f;
constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
constexpr float ANGLE_WRAP_VALUE = 360.0f;
constexpr int SIZE_FLOAT = sizeof(float);
constexpr u8 SIZE_FLOAT = sizeof(float);
// DMA传输完成标志
volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
angle_sensor KTH7823; // 角度传感器初始化
EncoderAngleKalman Angle_kalman(KTH7823.getAngle(), 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(Angle_kalman.getAngle(), 1e-5f, 1e-6f, 0.02f);
/**
* @brief SPI DMA initialization
*/
@@ -293,11 +299,10 @@ float angle_sensor::getRawVelocity(void) const {
* @brief 获取角度值(原始级)
* @return 角度值(原始级)
*/
float angle_sensor::getcurrent_sensorangle(void) const {
float angle_sensor::getcurrent_sensorangle(void) const {
return current_angle_deg;
}
/**
* @brief 重置累积角度值
*/