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

fix:修复任务调度不准确的问题，

Browse Source 
feat:优化电流采样速率
feat:优化PIDramp效率
feat:优化角度采样效率
fix:回滚CAN打包逻辑
feat:去除timer3
feat(FOC): 优化帕克逆变换与克拉克逆变换计算方式
将原有的三角函数计算替换为查表法，提高运算效率。同时添加了 sqrt(3)/2 的预计算常量，并优化了电压限制部分的计算逻辑，减少重复运算。
This commit is contained in:
cbb
2025-11-23 18:31:47 +08:00
parent 59f161bd73
commit 871e33e10f
14 changed files with 503 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/ADC.cpp
+18 -14
View File
@@ -1,6 +1,6 @@
#include "adc.h"
#include "Filter.h"
#include "Can.h"
#include "Filter.h"
#include "math.h"
#include <cstring>
#include <gd32c10x_dma.h>
@@ -20,7 +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; // 采样器初始化
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
@@ -139,7 +139,7 @@ sampler::sampler() {
float sampler::get_mcu_temp() {
tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
if (tempora_sample < MIN_TEMP or tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
}
return tempora_sample;
@@ -186,7 +186,7 @@ float sampler::get_current(char type) {
return ERROR_ADC_VAL;
}
}
if (tempora_sample < MIN_CURRENT || tempora_sample > MAX_CURRENT) {
if (tempora_sample < MIN_CURRENT or tempora_sample > MAX_CURRENT) {
return ERROR_ADC_VAL;
}
return tempora_sample;
@@ -202,7 +202,7 @@ float sampler::get_current(char type) {
float sampler::get_vbus_voltage() {
tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1);
// 检查电压读数是否在有效范围内
if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC ||
if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC or
vbus_samples[FILTER_SIZE / 2] < MIN_VCC) {
return ERROR_ADC_VAL;
}
@@ -237,16 +237,20 @@ void sampler::update_drv_temp() {
* @brief 更新相电流
*/
void sampler::update_current() {
memmove(currU_samples, currU_samples + 1, FILTER_SIZE * sizeof(float));
memmove(currV_samples, currV_samples + 1, FILTER_SIZE * sizeof(float));
memmove(currW_samples, currW_samples + 1, FILTER_SIZE * sizeof(float));
u8 idx = current_sample_index;
// 优化计算
const float scale = ADC_SCALE * CURRENT_FACTOR;
const float offset = CURRENT_OFFSET * CURRENT_FACTOR;
currU_samples[FILTER_SIZE] =
(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currV_samples[FILTER_SIZE] =
(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currW_samples[FILTER_SIZE] =
(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
currU_samples[idx] = adc_value[2] * scale + offset;
currV_samples[idx] = adc_value[3] * scale + offset;
currW_samples[idx] = adc_value[4] * scale + offset;
// 更新索引
current_sample_index++;
if (current_sample_index >= FILTER_SIZE - 1) {
current_sample_index = 0;
}
}
/**
Robot_V2/device/Can.cpp
+6 -1
View File
@@ -6,6 +6,7 @@
#include "gd32c10x.h"
#include "gd32c10x_can.h"
#include "gd32c10x_dma.h"
#include "systick.h"
#include <cstdio>
#include <cstdlib>
#include <cstring>
@@ -258,8 +259,12 @@ void can_loopback_init(void) {
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
can_parameter.prescaler = 6;
if (can_init(CANX, &can_parameter) != SUCCESS) {
while (1)
u16 i = 200;
while (--i>0){
delay_1ms(10);
printf("can init error \r\n");
}
system_soft_reset();
}
/* initialize filter */
Robot_V2/device/Can.h
+21 -12
View File
@@ -17,30 +17,39 @@ typedef union {
} float_union_t;
// 添加辅助函数用于将u16类型数据打包到u8数组中
static inline void pack_u16_to_bytes(u8 * const buffer, const u16 offset, const u16 value) {
*(reinterpret_cast<u16*>(buffer + offset)) = value;
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);
}
// 添加辅助函数用于将float类型数据打包到u8数组中
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));
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];
}
// 添加辅助函数用于从u8数组中解包short类型数据
static inline short unpack_bytes_to_short(const u8 * const buffer, const u16 offset) {
return *(reinterpret_cast<const short*>(buffer + offset));
static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) {
short value = (short)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
}
// 添加辅助函数用于从u8数组中解包u16类型数据
static inline u16 unpack_bytes_to_u16(const u8 * const buffer, const u16 offset) {
return *(reinterpret_cast<const u16*>(buffer + offset));
static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) {
u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
}
// 添加辅助函数用于从u8数组中解包float类型数据
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));
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];
return float_union;
}
Robot_V2/device/Timer.cpp
+34 -110
View File
@@ -8,6 +8,7 @@
#include "gd32c10x_rcu.h"
#include "gd32c10x_timer.h"
#include "initial.h"
#include <cstdio>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
@@ -43,10 +44,8 @@ void nvic_config(void) {
/* 设置NVIC优先级分组，1位抢占优先级，3位响应优先级 */
nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
nvic_irq_enable(TIMER2_IRQn, 1, 1);
nvic_irq_enable(TIMER3_IRQn, 1, 2);
nvic_irq_enable(TIMER1_IRQn, 2,
1); // 使能TIMER1中断，抢占优先级2，子优先级1
nvic_irq_enable(TIMER2_IRQn, 0, 1);
nvic_irq_enable(TIMER1_IRQn, 0, 1);
}
/**
@@ -90,47 +89,6 @@ void TIMER2_config(void) {
timer_enable(TIMER2);
}
/**
* @brief 配置定时器3
* @return void
*
*配置定时器3，用于诊断恢复
*/
void TIMER3_config(void) {
/* ----------------------------------------------------------------------------
TIMER3 Configuration:
TIMER3CLK = SystemCoreClock/120 = 1MHz, the period is 50us(50/1000000 =
50us).
----------------------------------------------------------------------------
*/
timer_parameter_struct timer_struct;
/* 使能TIMER3时钟 */
rcu_periph_clock_enable(RCU_TIMER3);
/* 复位TIMER3外设到默认状态 */
timer_deinit(TIMER3);
/* 初始化TIMER初始化参数结构体 */
timer_struct_para_init(&timer_struct);
/* TIMER3配置参数设置 */
timer_struct.prescaler =
TIMER3_PRESCALER; /* 预分频值：120分频(从0开始计数) */
timer_struct.alignedmode = TIMER_COUNTER_EDGE; /* 边沿对齐模式 */
timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
timer_struct.period =
TIMER3_PERIOD; /* 自动重装载值：50次计数(从0开始计数) */
timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
timer_init(TIMER3, &timer_struct);
/* 清除更新中断标志位 */
timer_interrupt_flag_clear(TIMER3, TIMER_INT_FLAG_UP);
/* 使能更新中断 */
timer_interrupt_enable(TIMER3, TIMER_INT_UP);
/* 使能TIMER3 */
timer_enable(TIMER3);
}
/**
* @brief 配置TIMER2中断
* @return void
@@ -155,76 +113,71 @@ uint64_t get_system_runtime_us(void) { return system_runtime_us; }
* @return void
*/
void TIMER2_IRQHandler(void) {
static u8 mode = 0;
// 检查更新中断标志并处理
if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 每100us执行一次的操作
total_us100_counter++;
scheduler.taskFlags |= (1 << TASK_ANGLE_UPDATE) |
(1 << TASK_VELOCITY_UPDATE) |
(1 << TASK_MOTOR_CONTROL);
switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
case 1: {
case 0: {
scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
break;
}
case 2: {
case 1: {
scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
break;
}
case 3: {
case 2: {
scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
break;
}
case 4: {
case 3: {
break;
}
case 9: {
CAN0_transmiter.can0_send_flag = SET;
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
break;
}
case 9: {
mode = FOC.control_mode;
break;
}
}
if (total_us100_counter >= 10) {
total_us100_counter++;
if (total_us100_counter > 9) {
total_us100_counter = 0;
// 可以添加每1ms执行的代码，无负载均衡
static u8 ms10_counter = 0;
ms10_counter++;
switch (ms10_counter) { // 每10ms执行的代码，负载均衡
case 1: {
static u8 ms1_counter = 0;
switch (ms1_counter) { // 每10ms执行的代码，负载均衡
case 0: {
scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
break;
}
case 2: {
case 1: {
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
break;
}
case 2: {
break;
}
if (ms10_counter >= 10) {
ms10_counter = 0;
}
ms1_counter++;
if (ms1_counter > 9) {
ms1_counter = 0;
// 可以添加每10ms执行的代码，无负载均衡
static u8 ms100_counter = 0;
ms100_counter++;
switch (ms100_counter) { // 每100ms执行的代码，负载均衡
case 1: {
break;
}
if (ms100_counter >= 10) {
ms100_counter = 0;
// 可以添加每100ms执行的代码
static u8 sec_counter = 0;
sec_counter++;
if (sec_counter >= 10) {
sec_counter = 0;
// 可以添加每1s执行的代码
}
static u8 ms10_counter = 0;
ms10_counter++;
if (ms10_counter > 9) {
ms10_counter = 0;
// 可以添加每100ms执行的代码
static u8 sec_counter = 0;
sec_counter++;
if (sec_counter > 9) {
sec_counter = 0;
// 可以添加每1s执行的代码
}
}
}
@@ -232,33 +185,6 @@ void TIMER2_IRQHandler(void) {
}
}
/**
* @brief TIMER3中断处理函数
* @return void
*/
void TIMER3_IRQHandler(void) {
// 检查更新中断标志并处理
if (timer_interrupt_flag_get(TIMER3, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER3, TIMER_INT_UP);
static u8 count = 0;
count++;
// 10ms测试代码
if (count >= 10) {
count = 0;
// 100ms测试代码
static u8 count_10 = 0;
count_10++;
if (count_10 >= 10) {
count_10 = 0;
// 1s测试代码
}
}
}
}
/**
* @brief TIMER1 GPIO配置函数
*/
@@ -343,8 +269,6 @@ void timer1_pwm_config(void) {
// 使能CH3比较中断
timer_interrupt_enable(TIMER1, TIMER_INT_CH3);
nvic_irq_enable(TIMER1_IRQn, 0,
2); // 使能TIMER1中断，抢占优先级2，子优先级1
// 启动TIMER1
timer_enable(TIMER1);
Robot_V2/device/Timer.h
-2
View File
@@ -11,8 +11,6 @@ void User_timer_init(void);
void Set_Pwm(uint16_t channel, float duty_cycle);
void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2);
void TIMER2_IRQHandler(void);
void TIMER3_config(void);
void TIMER3_IRQHandler(void);
void timer1_pwm_config(void);
void timer1_gpio_config(void);
Robot_V2/device/adc.h
+2
View File
@@ -7,6 +7,7 @@ extern "C" {
#include "stdint.h"
constexpr u8 FILTER_SIZE = 6;
constexpr u8 BLOCK_SIZE = FILTER_SIZE * sizeof(float);
class sampler {
private:
float vbus_samples[FILTER_SIZE + 1];
@@ -15,6 +16,7 @@ class sampler {
float currW_samples[FILTER_SIZE + 1];
float mcu_temp_samples[FILTER_SIZE + 1];
float drv_temp_samples[FILTER_SIZE + 1];
u8 current_sample_index;
float tempora_sample;
Robot_V2/device/ang_sensor.cpp
+26 -16
View File
@@ -1,5 +1,6 @@
#include "ang_sensor.h"
#include "Filter.h"
#include "core_cmFunc.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
@@ -105,8 +106,8 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
dma_transfer_number_config(DMA0, DMA_CH1, 1);
dma_channel_enable(DMA0, DMA_CH1);
// 等待传输完成，超时时间为1000个循环
uint32_t timeout = 1000;
// 等待传输完成，超时时间为100个循环
uint32_t timeout = 100;
while ((!spi_dma_tx_complete || !spi_dma_rx_complete) && --timeout) {
// 空循环等待DMA传输完成
}
@@ -209,22 +210,31 @@ angle_sensor::angle_sensor(void) { KTH7823_init(); }
* @brief 读取传感器数据
* @return 传感器数据
*/
float angle_sensor::read_sensorAngle(void) {
u16 u16data = FAULT_RECEIVE;
inline float angle_sensor::read_sensorAngle(void) {
uint16_t raw_data;
SPI_CS_ENABLE();
// 使用DMA发送命令并接收数据
if (!spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data)) {
SPI_CS_DISABLE();
current_angle_deg = u16data * ANGLE_FACTOR;
current_angle_rad = u16data * RAD_FACTOR;
return current_angle_deg;
} else {
SPI_CS_DISABLE();
// 处理读取错误
SPI_CS_ENABLE();
u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
SPI_CS_DISABLE();
if (result == 0) {
// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
float angle_deg = (float)raw_data * ANGLE_FACTOR;
current_angle_deg = angle_deg;
current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f
return angle_deg;
}
#ifdef DEBUG
// 仅在调试模式下输出错误，避免发布版本中的I/O开销
if (result != 0) {
// 使用更高效的串口输出替代printf（如果可用）
printf("Error reading angle from sensor.\n");
return FAULT_RECEIVE; // 错误返回
}
}
#endif
return (float)FAULT_RECEIVE;
}
/**
* @brief 更新传感器角度（输出级）