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

更新OTA功能，config独立成文件统一定义

Browse Source 
refactor(build): 更新项目配置和链接器设置
- 修改GD32C103CB的ROM基地址和大小配置
- 更新系统时钟向量表偏移量设置
- 在cproject.yml中添加BIN输出格式支持
- 增加-O3优化级别和代码移除选项
- 更新csolution版本依赖

feat(device): 优化ADC采样和配置管理
- 将DengFOC依赖替换为FOC模块
- 从config.h引入配置常量
- 重命名FILTER_SIZE为ADC_FILTER_SIZE以提高清晰度
- 优化温度和电流采样滤波实现

refactor(algorithm): 简化FOC控制函数接口
- 为angle_control、velocity_control等函数移除不必要的参数
- 统一函数指针类型定义
- 优化电流计算函数的代码格式
- 调整PID控制器初始化参数
This commit is contained in:
cbb
2026-01-04 22:26:22 +08:00
parent abd64da360
commit d0198fdff7
26 changed files with 716 additions and 439 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp → Robot_V2/Algorithm/FOC.cpp
+30 -54
View File
@@ -1,26 +1,15 @@
#include "DengFOC.h"
#include "FOC.h"
#include "Can.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "initial.h"
#include "config.h"
#include "motor.h"
#include "pid.h"
#include <cmath>
#include <gd32c10x.h>
#include <stdint.h>
#include "motor.h"
constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
constexpr float _1_SQRT3 = 0.57735026919f;
constexpr float _2_SQRT3 = 1.15470053838f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11;
constexpr float DIR = 1.0;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
constexpr float zero_electric_angle = 0.0f;
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
@@ -62,7 +51,7 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
float I_q = I_beta * cos_el - I_alpha * sin_el;
return I_q*1000.0f; // 返回Iq值，乘以1000以转换为mA
return I_q * 1000.0f; // 返回Iq值，乘以1000以转换为mA
}
/**
@@ -123,13 +112,15 @@ void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
/**
* @brief 电机锁止
*/
void FOC_Controlroller::drv_hold(float duty) {
if (duty > 0.95f) {
duty = 0.95f;
} else if (duty < 0.0f) {
duty = 0.0f;
float FOC_Controlroller::drv_hold() {
float hold_pwm_duty = HOLD_PWM_DUTY;
if (HOLD_PWM_DUTY > 0.95f) {
hold_pwm_duty = 0.95f;
} else if (HOLD_PWM_DUTY < 0.0f) {
hold_pwm_duty = 0.0f;
}
Set_3Pwm(duty, duty, duty);
Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
return hold_pwm_duty;
}
/**
* @brief 电机恢复
@@ -173,7 +164,8 @@ float FOC_Controlroller::velocity_open_loop() {
setPhaseVoltage(Uq, 0.0f);
// CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
return limit_voltage;
}
@@ -198,7 +190,7 @@ float FOC_Controlroller::velocity_open_loop() {
* @param target_angle: 目标角度
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control(float target_angle) {
float FOC_Controlroller::angle_control() {
// PID 控制计算
float Uq = angle_loop(target_angle - rt_angle);
@@ -216,45 +208,27 @@ float FOC_Controlroller::angle_control(float target_angle) {
* @param max_velocity: 最大速度限制 (单位: rad/s)
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control_with_velocity_limit(float target_angle) {
float FOC_Controlroller::angle_control_with_velocity_limit() {
// 计算角度误差
float angle_error = target_angle - rt_angle;
// 使用角度PID控制器计算目标速度
float target_velocity = ang_inVelAngloop(angle_error);
// 对目标速度进行限幅
target_velocity = _constrain(target_velocity, -limit_velocity, limit_velocity);
target_velocity =
_constrain(target_velocity, -limit_velocity, limit_velocity);
// 使用速度PID控制器计算最终输出电压
float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
/**
* @brief 角度闭环控制
*
* @param target_angle: 目标角度
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control_NPID(float target_angle) {
static float Kp = 20.0f;
// 控制力矩计算
float Uq = _constrain(Kp * (target_angle - rt_angle), -voltage_power_supply,
voltage_power_supply);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
@@ -264,22 +238,24 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) {
* @param target_velocity: 目标速度
* @return float: 当前速度
*/
float FOC_Controlroller::velocity_control(float target_velocity) {
float FOC_Controlroller::velocity_control() {
// PID 控制计算
float Uq = vel_loop((target_velocity - rt_velocity)*25);
float Uq = vel_loop((target_velocity - rt_velocity) * 25);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
// CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
return rt_velocity;
}
float FOC_Controlroller::current_control(float target_current) {
float FOC_Controlroller::current_control() {
// PID 控制计算
float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
float Uq = current_loop(target_current - Iq);
CAN0_transmiter.testfloat_union.f = Iq;
// 获取电机轴角度
Robot_V2/Algorithm/DengFOC.h → Robot_V2/Algorithm/FOC.h
+14 -13
View File
@@ -1,6 +1,7 @@
// 函数声明
#ifndef _DENGFOC_H
#define _DENGFOC_H
#ifndef _FOC_H
#define _FOC_H
#include "config.h"
#include "pid.h"
#include <stdint.h>
@@ -14,6 +15,7 @@ class FOC_Controlroller {
float limit_velocity = 0.0f;
float target_angle = 0.0f;
float target_velocity = 0.0f;
float target_current = 0.0f;
float rt_angle = 0.0f;
float rt_velocity = 0.0f;
bool over_current = false;
@@ -23,19 +25,18 @@ class FOC_Controlroller {
float v_safe = voltage_power_supply * 1.05f;
float duty_a = 0, duty_b = 0, duty_c = 0;
float Ua = 0, Ub = 0, Uc = 0;
float velocity_open_loop();
float angle_control(float target_angle);
float angle_control_with_velocity_limit(float target_angle);
float velocity_control(float target_velocity);
float current_control(float target_current);
float angle_control_NPID(float target_angle);
float velocity_open_loop(void);
float angle_control(void);
float angle_control_with_velocity_limit(void);
float velocity_control(void);
float current_control(void);
void update_power(void);
void DFOC_alignSensor(int _PP);
void shut_down(void);
void resume(void);
void drv_stop(void);
void drv_hold(float duty = 0.5f);
float drv_hold(void);
protected:
float shaft_angle = 0;
@@ -50,7 +51,7 @@ class FOC_Controlroller {
PIDController current_loop =
PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
PIDController vel_inVelAngloop =
PIDController{0.35f, 5.0f,1e-3, 1000000, voltage_power_supply};
PIDController{0.35f, 5.0f, 1e-3, 1000000, voltage_power_supply};
PIDController ang_inVelAngloop =
PIDController{30.0f, 0.0f, 1e-3, 1000000, 999};
@@ -58,12 +59,12 @@ class FOC_Controlroller {
float cal_Iq_Id(float current_a, float current_b, float current_c);
};
// 定义函数指针类型
typedef float (FOC_Controlroller::*FOCFunctionPtr)(void);
float _normalizeAngle(float angle);
inline float _electricalAngle();
constexpr u16 LUT_SIZE = 1024;
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
__attribute__((aligned(4))) const float sin_lut[LUT_SIZE] = {
0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f,
0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f,
Robot_V2/Algorithm/Filter.cpp
+1 -2
View File
@@ -4,14 +4,13 @@
#include <algorithm>
#include <cstring>
#include <stdint.h>
#include "config.h"
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern u32 system_runtime_us;
EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
constexpr u8 SIZE_FLOAT = sizeof(float);
/*!
\brief 中值滤波器函数
Robot_V2/RTE/Device/GD32C103CB/ac6_linker_script.sct.src
+2 -1
View File
@@ -32,7 +32,8 @@
LR_ROM0 __ROM0_BASE __ROM0_SIZE {
ER_ROM0 __ROM0_BASE __ROM0_SIZE {
*.o (RESET, +First)
*.o (RESET, +First) /* 向量表和复位处理程序 */
*(.text.init) /* 确保系统初始化代码在最前面 */
*(InRoot$$Sections)
*(+RO +XO)
}
Robot_V2/RTE/Device/GD32C103CB/regions_GD32C103CB.h
+2 -2
View File
@@ -14,10 +14,10 @@
// <o> Base address <0x0-0xFFFFFFFF:8>
// <i> Defines base address of memory region. Default: 0x08000000
// <i> Contains Startup and Vector Table
#define __ROM0_BASE 0x08000000
#define __ROM0_BASE 0x08011000
// <o> Region size [bytes] <0x0-0xFFFFFFFF:8>
// <i> Defines size of memory region. Default: 0x00020000
#define __ROM0_SIZE 0x00020000
#define __ROM0_SIZE 0x0000C000
// </h>
// <h> __ROM1 (unused)
Robot_V2/RTE/Device/GD32C103CB/system_gd32c10x.c
+2 -1
View File
@@ -42,7 +42,8 @@
#define __SYS_OSC_CLK (__IRC8M) /* main oscillator frequency */
/* Vector Table base offset */
#define VECT_TAB_OFFSET 0x00 /* This value must be a multiple of 0x200. */
#define APP_BASE ((uint32_t)0x08011000U) /*!< main FLASH base address */
#define VECT_TAB_OFFSET (uint32_t)APP_BASE-FLASH_BASE /* This value must be a multiple of 0x200. */
/* select a system clock by uncommenting the following line */
/* use IRC8M */
Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h
+7 -1
View File
@@ -1,6 +1,6 @@
/*
* CSOLUTION generated file: DO NOT EDIT!
* Generated by: csolution version 2.11.0
* Generated by: csolution version 2.12.0+p41-gfeadfc5e
*
* Project: 'Robot_V2.Debug+GD32C103CB'
* Target: 'Debug+GD32C103CB'
@@ -17,8 +17,12 @@
/* GigaDevice::Device:GD32C10x_StdPeripherals:ADC@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_ADC
/* GigaDevice::Device:GD32C10x_StdPeripherals:BKP@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_BKP
/* GigaDevice::Device:GD32C10x_StdPeripherals:CAN@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_CAN
/* GigaDevice::Device:GD32C10x_StdPeripherals:CRC@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_CRC
/* GigaDevice::Device:GD32C10x_StdPeripherals:DBG@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_DBG
/* GigaDevice::Device:GD32C10x_StdPeripherals:DMA@1.0.0 */
@@ -37,6 +41,8 @@
#define RTE_DEVICE_STDPERIPHERALS_PMU
/* GigaDevice::Device:GD32C10x_StdPeripherals:RCU@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_RCU
/* GigaDevice::Device:GD32C10x_StdPeripherals:RTC@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_RTC
/* GigaDevice::Device:GD32C10x_StdPeripherals:SPI_I2S@1.0.0 */
#define RTE_DEVICE_STDPERIPHERALS_SPI
/* GigaDevice::Device:GD32C10x_StdPeripherals:TIMER@1.0.0 */
Robot_V2/RTE/_Release_GD32C103CB/RTE_Components.h
+1 -1
View File
@@ -1,6 +1,6 @@
/*
* CSOLUTION generated file: DO NOT EDIT!
* Generated by: csolution version 2.12.0
* Generated by: csolution version 2.12.0+p41-gfeadfc5e
*
* Project: 'Robot_V2.Release+GD32C103CB'
* Target: 'Release+GD32C103CB'
Robot_V2/Robot_V2.cproject.yml
+12 -7
View File
@@ -14,6 +14,7 @@ project:
- file: sys/health.cpp
- file: sys/flash.h
- file: sys/flash.cpp
- file: sys/config.h
- group: device
files:
- file: device/Can.cpp
@@ -31,18 +32,20 @@ project:
files:
- file: Algorithm/Filter.cpp
- file: Algorithm/Filter.h
- file: Algorithm/DengFOC.cpp
- file: Algorithm/DengFOC.h
- file: Algorithm/FOC.cpp
- file: Algorithm/FOC.h
- file: Algorithm/pid.cpp
- file: Algorithm/pid.h
components:
- component: ARM::CMSIS:CORE
- component: Device:Startup
- component: Device:EVAL:GD32C10x
- component: Device:GD32C10x_libopt
- component: Device:GD32C10x_StdPeripherals:FMC
- component: Device:GD32C10x_StdPeripherals:ADC
- component: Device:GD32C10x_StdPeripherals:CAN
- component: Device:GD32C10x_StdPeripherals:DMA
- component: Device:GD32C10x_StdPeripherals:EXTI
- component: Device:GD32C10x_StdPeripherals:FMC
- component: Device:GD32C10x_StdPeripherals:GPIO
- component: Device:GD32C10x_StdPeripherals:MISC
- component: Device:GD32C10x_StdPeripherals:PMU
@@ -50,9 +53,7 @@ project:
- component: Device:GD32C10x_StdPeripherals:SPI_I2S
- component: Device:GD32C10x_StdPeripherals:TIMER
- component: Device:GD32C10x_StdPeripherals:USART
- component: Device:GD32C10x_libopt
- component: Device:GD32C10x_StdPeripherals:DBG
- component: Device:EVAL:GD32C10x
- component: Device:GD32C10x_StdPeripherals:FWDGT
- component: Device:GD32C10x_StdPeripherals:WWDGT
- component: Device:GD32C10x_StdPeripherals:BKP
@@ -62,6 +63,7 @@ project:
type:
- elf
- hex
- bin
- map
packs:
- pack: ARM::CMSIS
@@ -73,11 +75,14 @@ project:
- -fshort-wchar
- -D__MICROLIB
- -Wno-register
- -O3 # 添加优化级别
C:
- -D__packed=packed
- -D__packed=__attribute__((packed))
ASM:
- -D__packed=packed
- -D__packed=__attribute__((packed))
- -D__MICROLIB
Link:
- --library_type=microlib
- --entry=Reset_Handler
- --remove # 移除未使用的代码段
- --diag_suppress=L6207,L6210,L6218,L6220,L6222,L6224,L6226,L6228,L6230,L6232,L6234,L6236,L6238,L6240,L6242,L6244,L6246,L6248,L6308,L6329,L6310,L6312,L6314,L6316,L6318,L6320,L6322,L6324,L6326,L6328,L6330,L6332,L6334,L6336,L6338,L6340,L6342,L6344,L6346,L6348,L6350,L6352,L6354,L6311,L6313,L6315,L6317,L6319,L6321,L6323,L6325,L6327,L6329,L6331,L6333,L6335,L6337,L6339,L6341,L6343,L6345,L6347,L6349,L6351,L6353
Robot_V2/device/ADC.cpp
+20 -27
View File
@@ -1,29 +1,22 @@
#include "adc.h"
#include "Can.h"
#include "DengFOC.h"
#include "FOC.h"
#include "Filter.h"
#include "config.h"
#include "math.h"
#include "systick.h"
#include <cstring>
#include <gd32c10x_dma.h>
#include <stdint.h>
#include "systick.h"
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 = -1.60f;
constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
constexpr float MAX_TEMP = 150.0f;
constexpr float MIN_TEMP = -40.0f;
constexpr float MAX_CURRENT = 40.0f;
constexpr float MIN_CURRENT = -40.0f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
sampler sampler; // 采样器初始化
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
\brief configure the DMA peripheral
@@ -116,7 +109,7 @@ u8 sampler::init() {
adc_config(); // ADC初始化
delay_1ms(1);
// 初始采样
for (int i = 0; i < FILTER_SIZE + 1; i++) {
for (int i = 0; i < ADC_FILTER_SIZE + 1; i++) {
mcu_temp_samples[i] =
(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
@@ -142,7 +135,7 @@ u8 sampler::init() {
* 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f
*/
float sampler::get_mcu_temp() {
tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1);
tempora_sample = medium_filter(mcu_temp_samples, ADC_FILTER_SIZE + 1);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP or tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
@@ -158,7 +151,7 @@ float sampler::get_mcu_temp() {
* 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
*/
float sampler::get_drv_temp() {
tempora_sample = medium_filter(drv_temp_samples, FILTER_SIZE + 1);
tempora_sample = medium_filter(drv_temp_samples, ADC_FILTER_SIZE + 1);
// 检查温度读数是否在有效范围内
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
return ERROR_ADC_VAL;
@@ -177,17 +170,17 @@ float sampler::get_current(char type) {
switch (type) {
case 'U': {
// return currU_samples[current_sample_index];
tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1);
tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
break;
}
case 'V': {
// return currV_samples[current_sample_index];
tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1);
tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
break;
}
case 'W': {
// return currW_samples[current_sample_index];
tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1);
tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
break;
}
default: {
@@ -209,10 +202,10 @@ float sampler::get_current(char type) {
* 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。
*/
float sampler::get_vbus_voltage() {
tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1);
tempora_sample = medium_filter(vbus_samples, ADC_FILTER_SIZE + 1);
// 检查电压读数是否在有效范围内
if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC or
vbus_samples[FILTER_SIZE / 2] < MIN_VCC) {
if (vbus_samples[ADC_FILTER_SIZE / 2] > MAX_VCC or
vbus_samples[ADC_FILTER_SIZE / 2] < MIN_VCC) {
return ERROR_ADC_VAL;
}
return tempora_sample;
@@ -226,8 +219,8 @@ float sampler::get_vbus_voltage() {
*/
void sampler::update_mcu_temp() {
memmove(mcu_temp_samples, mcu_temp_samples + 1,
FILTER_SIZE * sizeof(float));
mcu_temp_samples[FILTER_SIZE] =
ADC_FILTER_SIZE * sizeof(float));
mcu_temp_samples[ADC_FILTER_SIZE] =
(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
}
@@ -236,9 +229,9 @@ void sampler::update_mcu_temp() {
*/
void sampler::update_drv_temp() {
memmove(drv_temp_samples, drv_temp_samples + 1,
FILTER_SIZE * sizeof(float));
ADC_FILTER_SIZE * sizeof(float));
float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
drv_temp_samples[FILTER_SIZE] =
drv_temp_samples[ADC_FILTER_SIZE] =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
}
@@ -258,7 +251,7 @@ void sampler::update_current() {
// 更新索引
current_sample_index++;
if (current_sample_index >= FILTER_SIZE - 1) {
if (current_sample_index >= ADC_FILTER_SIZE - 1) {
current_sample_index = 0;
}
// 卡尔曼滤波更新
@@ -269,6 +262,6 @@ void sampler::update_current() {
* @brief 更新供电电压
*/
void sampler::update_vbus_voltage() {
memmove(vbus_samples, vbus_samples + 1, FILTER_SIZE * sizeof(float));
vbus_samples[FILTER_SIZE] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
memmove(vbus_samples, vbus_samples + 1, ADC_FILTER_SIZE * sizeof(float));
vbus_samples[ADC_FILTER_SIZE] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
}
Robot_V2/device/Can.cpp
+182 -150
View File
@@ -1,8 +1,9 @@
#include "Can.h"
#include "DengFOC.h"
#include "FOC.h"
#include "Filter.h"
#include "adc.h"
#include "ang_sensor.h"
#include "config.h"
#include "flash.h"
#include "gd32c10x.h"
#include "gd32c10x_can.h"
@@ -19,10 +20,6 @@
#include <stdlib.h>
constexpr u16 CANX = CAN0;
constexpr u16 DEV_CAN0_ID = 0x0000;
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;
CAN_transmiter CAN0_transmiter; // CAN初始化
extern FOC_Controlroller FOC;
@@ -157,8 +154,11 @@ void can_config(void) {
/* configure CAN0 NVIC */
nvic_irq_enable(CAN0_RX0_IRQn, 1, 0);
nvic_irq_enable(CAN0_TX_IRQn, 1, 1); // 添加发送中断
/* enable can receive FIFO0 not empty interrupt */
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
/* enable can transmit mailbox empty interrupt */
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
// 使能DMA中断
nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1);
@@ -173,6 +173,7 @@ void can_config(void) {
*/
void canfd_config(void) {
can_gpio_init();
can_dma_init(); // 初始化DMA
can_parameter_struct can_parameter;
can_fdframe_struct can_fd_parameter;
can_fd_tdc_struct can_fd_tdc_parameter;
@@ -235,6 +236,9 @@ void canfd_config(void) {
nvic_irq_enable(CAN0_RX0_IRQn, 1, 0);
/* enable can receive FIFO0 not empty interrupt */
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
// 使能DMA中断
nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1);
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF);
}
/*!
@@ -458,128 +462,6 @@ void CAN_transmiter::update(void) {
v_current_union.f = sampler.get_current('V') * 1000;
}
/*!
\brief CAN 数据发送
\param id CAN ID
\param length 数据长度
\param data 数据
*/
u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) {
// 检查数据长度是否符合CAN规范
if (length > 8) {
printf("CAN data length exceeds maximum of 8 bytes\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
printf("Invalid data pointer\r\n");
return 0;
}
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式
transmit_message.tx_sfid = id; // 设置帧ID
transmit_message.tx_dlen = length; // 设置数据长度
// 复制数据到发送缓冲区
for (int i = 0; i < length && i < 8; i++) {
transmit_message.tx_data[i] = *(data + i);
}
// 将数据复制到DMA缓冲区
memcpy(can_tx_buffer, data, length);
// 配置DMA发送参数
dma_tx_parameter.number = length;
dma_init(DMA0, DMA_CH5, &dma_tx_parameter);
// 启动DMA传输
dma_channel_enable(DMA0, DMA_CH5);
// 自动选择空闲mailbox发送消息，不需要人为指定
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
// 检查是否成功获取mailbox
if (CAN_NOMAILBOX == transmit_mailbox) {
// 没有可用的mailbox，可能所有mailbox都在使用中
printf("No available mailbox for CAN transmission\r\n");
return 0;
}
return 1;
}
/*!
\brief CANFD 数据发送
\param id CAN消息ID
\param length CAN消息长度
\param data CAN消息数据
*/
u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) {
// 检查CANFD模式是否启用
if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
printf("CANFD mode not enabled\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
printf("Invalid data pointer\r\n");
return 0;
}
// 检查数据长度是否符合CANFD规范
if (length > 64) {
printf("CANFD data length exceeds maximum of 64 bytes\r\n");
return 0;
}
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
// transmit_message.tx_sfid = id; // 设置帧ID
transmit_message.tx_efid = id; // 扩展帧ID
transmit_message.tx_dlen = length; // 设置数据长度
// 设置CANFD特定参数
transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD帧格式
transmit_message.fd_brs = ENABLE; // 使能比特率切换
transmit_message.fd_esi = DISABLE; // 错误状态指示
// 复制数据到发送缓冲区
for (int i = 0; i < length && i < 64; i++) {
transmit_message.tx_data[i] = *(data + i);
}
// 将数据复制到DMA缓冲区
memcpy(can_tx_buffer, data, length);
// 配置DMA发送参数
dma_tx_parameter.number = length;
dma_init(DMA0, DMA_CH5, &dma_tx_parameter);
// 启动DMA传输
dma_channel_enable(DMA0, DMA_CH5);
// 自动选择空闲mailbox发送消息，不需要人为指定
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
// 检查是否成功获取mailbox
if (CAN_NOMAILBOX == transmit_mailbox) {
static u32 count = 0;
// 没有可用的mailbox，可能所有mailbox都在使用中
count++;
printf("No available mailbox for CANFD transmission\r\n");
if (count > 0xff and !is_sleep) {
init();
count = 0;
}
return 0;
}
return 1;
}
/**
* @brief CAN发送函数
* @retval 0: 发送失败
@@ -594,31 +476,39 @@ u8 CAN_transmiter::CAN_all_Send() {
constexpr u32 Status_FEED = 0x118;
constexpr u32 Limit_Vel_Vol_FEED = 0x119;
// 打包RT_Vel_Ang_FEED
// 打包RT_Vel_Ang_FEED - 只清零实际使用的8字节
memset(transmit_data, 0, 8);
pack_float_to_bytes(transmit_data, 0, rt_angle_union);
pack_float_to_bytes(transmit_data, 4, rt_velocity_union);
success_flag += CAN_Send(RT_Vel_Ang_FEED, 8, transmit_data);
// 打包SET_Vel_Ang_FEED
success_flag += CAN_AsyncSend(RT_Vel_Ang_FEED, 8, transmit_data);
// 打包SET_Vel_Ang_FEED - 只清零实际使用的8字节
memset(transmit_data, 0, 8);
pack_float_to_bytes(transmit_data, 0, target_angle_union);
pack_float_to_bytes(transmit_data, 4, target_velocity_union);
success_flag += CAN_Send(SET_Vel_Ang_FEED, 8, transmit_data);
// 打包Current_FEED
success_flag += CAN_AsyncSend(SET_Vel_Ang_FEED, 8, transmit_data);
// 打包Current_FEED - 只清零实际使用的6字节
memset(transmit_data, 0, 6);
pack_u16_to_bytes(transmit_data, 0, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 2, u16(w_current_union.f));
pack_u16_to_bytes(transmit_data, 4, u16(v_current_union.f));
success_flag += CAN_Send(Current_FEED, 6, transmit_data);
success_flag += CAN_AsyncSend(Current_FEED, 6, transmit_data);
// 打包Status_FEED
// 打包Status_FEED - 只清零实际使用的8字节
memset(transmit_data, 0, 8);
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);
pack_u16_to_bytes(transmit_data, 6, Power_Voltage);
success_flag += CAN_Send(Status_FEED, 8, transmit_data);
// 打包Limit_Vel_Vol_FEED
success_flag += CAN_AsyncSend(Status_FEED, 8, transmit_data);
// 打包Limit_Vel_Vol_FEED - 只清零实际使用的6字节
memset(transmit_data, 0, 6);
pack_float_to_bytes(transmit_data, 0, limit_velocity_union);
pack_u16_to_bytes(transmit_data, 4, limit_voltage);
success_flag += CAN_Send(Limit_Vel_Vol_FEED, 6, transmit_data);
success_flag += CAN_AsyncSend(Limit_Vel_Vol_FEED, 6, transmit_data);
return success_flag;
}
@@ -631,9 +521,8 @@ u8 CAN_transmiter::CAN_all_Send() {
u8 CAN_transmiter::CANFD_all_Send() {
u32 success_flag = 0;
constexpr u32 STATUS_ID = 0x00010100;
constexpr u32 CONFIG_ID = 0x00010101;
// 打包status数据
// 打包status数据 - 只清零实际使用的32字节
memset(transmit_data, 0, 32);
pack_u16_to_bytes(transmit_data, 0, mcu_temp);
pack_u16_to_bytes(transmit_data, 2, motor_temp);
pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
@@ -647,15 +536,16 @@ u8 CAN_transmiter::CANFD_all_Send() {
pack_float_to_bytes(transmit_data, 13, rt_angle_union);
pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
pack_float_to_bytes(transmit_data, 21, testfloat_union);
success_flag += CANFD_Send(STATUS_ID, 32, transmit_data);
success_flag += CANFD_AsyncSend(CANFD_STATUS_ID, 32, transmit_data);
// 打包config数据
// 打包config数据 - 只清零实际使用的16字节
memset(transmit_data, 0, 16);
transmit_data[0] = controller_Mode;
pack_u16_to_bytes(transmit_data, 1, limit_voltage);
pack_float_to_bytes(transmit_data, 3, limit_velocity_union);
pack_float_to_bytes(transmit_data, 7, target_angle_union);
pack_float_to_bytes(transmit_data, 11, target_velocity_union);
success_flag += CANFD_Send(CONFIG_ID, 16, transmit_data);
success_flag += CANFD_AsyncSend(CANFD_CONFIG_ID, 16, transmit_data);
return success_flag;
}
@@ -669,9 +559,9 @@ void CAN_transmiter::communication_check() {
// CAN0 发送数据标志
if (SET == can0_send_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
CAN0_transmiter.update();
update();
// CAN0_transmiter.CAN_all_Send();
CAN0_transmiter.CANFD_all_Send();
CANFD_all_Send();
can0_send_flag = RESET;
}
@@ -681,7 +571,7 @@ void CAN_transmiter::communication_check() {
// for (i = 0; i < receive_message.rx_dlen; i++) {
// printf(" %02x", receive_message.rx_data[i]);
// }
if (receive_message.rx_efid == 0x00010200) {
if (receive_message.rx_efid == CANFD_CMD_ID) {
FOC.control_mode = receive_message.rx_data[0];
if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 !=
FOC.limit_voltage) {
@@ -701,7 +591,7 @@ void CAN_transmiter::communication_check() {
target_velocity_union.f =
unpack_bytes_to_float(receive_message.rx_data, 11).f;
FOC.target_velocity = target_velocity_union.f;
} else if (receive_message.rx_efid == 0x00010301) {
} else if (receive_message.rx_efid == CANFD_RESET_ID) {
u8 first_byte = receive_message.rx_data[0];
u8 eigth_byte = receive_message.rx_data[7];
if (first_byte & 0x01) {
@@ -722,7 +612,8 @@ void CAN_transmiter::communication_check() {
prepare_deep_sleep(true);
// CAN_Sleep();
}
} else if (receive_message.rx_efid == CANFD_BOOT_ID) {
jumpToBOOT();
} else {
printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
}
@@ -800,7 +691,6 @@ void DMA0_Channel4_IRQHandler(void) {
if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_FTF)) {
// 清除DMA传输完成中断标志
dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G);
// 将DMA缓冲区的数据复制到接收消息结构体
for (int i = 0; i < CAN0_transmiter.receive_message.rx_dlen && i < 8;
i++) {
@@ -815,7 +705,149 @@ void DMA0_Channel4_IRQHandler(void) {
*/
void DMA0_Channel5_IRQHandler(void) {
if (dma_interrupt_flag_get(DMA0, DMA_CH5, DMA_INT_FLAG_FTF)) {
// DMA传输完成，可以在此处执行后续处理
// 清除DMA传输完成中断标志
dma_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_G);
dma_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_FTF);
// 可以在此添加后续处理逻辑，如发送下一个数据包等
}
}
/**
* @brief 异步CAN数据发送(使用DMA和中断机制)
* @param id CAN消息ID
* @param length CAN消息长度
* @param data CAN消息数据
*/
u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) {
// 检查数据长度是否符合CAN规范
if (length > 8) {
printf("CAN data length exceeds maximum of 8 bytes\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
printf("Invalid data pointer\r\n");
return 0;
}
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式
transmit_message.tx_sfid = id; // 设置帧ID
transmit_message.tx_dlen = length; // 设置数据长度
// 复制数据到发送缓冲区
for (int i = 0; i < length && i < 8; i++) {
transmit_message.tx_data[i] = *(data + i);
}
// 自动选择空闲mailbox发送消息
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
// 检查是否成功获取mailbox
if (transmit_mailbox == CAN_NOMAILBOX) {
// 没有可用的mailbox，启用发送中断以便在邮箱可用时重试
printf("No available mailbox for CAN transmission, enabling TX "
"interrupt\r\n");
// 可以设置一个标志位，当邮箱可用时发送
// 在实际应用中，可以维护一个待发送队列
return 0;
}
// 启用CAN发送中断以检测传输完成
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
return 1;
}
/**
* @brief 异步CANFD数据发送(使用DMA和中断机制)
* @param id CAN消息ID
* @param length CAN消息长度
* @param data CAN消息数据
*/
u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
u8 valid_length = length;
// 检查CANFD模式是否启用
if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
printf("CANFD mode not enabled\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
printf("Invalid data pointer\r\n");
return 0;
}
// 检查数据长度是否符合CANFD规范
if (length > 8) {
if (length > 8 and length < 13) {
valid_length = 12;
} else if (length > 12 and length < 17) {
valid_length = 16;
} else if (length > 16 and length < 21) {
valid_length = 20;
} else if (length > 20 and length < 24) {
valid_length = 24;
} else if (length > 28 and length < 33) {
valid_length = 32;
} else if (length > 32 and length < 49) {
valid_length = 48;
} else if (length > 48 and length < 65) {
valid_length = 64;
} else {
printf("CANFD data length error\r\n");
return 0;
}
}
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
transmit_message.tx_efid = id; // 扩展帧ID
transmit_message.tx_dlen = valid_length; // 设置数据长度
// 设置CANFD特定参数
transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD帧格式
transmit_message.fd_brs = ENABLE; // 使能比特率切换
transmit_message.fd_esi = DISABLE; // 错误状态指示
// 复制数据到发送缓冲区
for (int i = 0; i < valid_length; i++) {
if (i < length) {
transmit_message.tx_data[i] = *(data + i);
} else {
transmit_message.tx_data[i] = 0;
}
}
// 自动选择空闲mailbox发送消息
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
// 检查是否成功获取mailbox
if (transmit_mailbox == CAN_NOMAILBOX) {
// 没有可用的mailbox，启用发送中断以便在邮箱可用时重试
printf("No available mailbox for CANFD transmission, enabling TX "
"interrupt\r\n");
return 0;
}
// 启用CAN发送中断以检测传输完成
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
return 1;
}
// 添加CAN传输中断处理函数
void CAN0_TX_IRQHandler(void) {
// 检查是否有发送邮箱空闲
if (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET ||
can_flag_get(CAN0, CAN_FLAG_TME1) != RESET ||
can_flag_get(CAN0, CAN_FLAG_TME2) != RESET) {
// 邮箱已空闲，可以发送新消息
// 可以在这里处理待发送队列中的消息
// 清除发送中断标志
can_flag_clear(CAN0, CAN_FLAG_TME0);
can_flag_clear(CAN0, CAN_FLAG_TME1);
can_flag_clear(CAN0, CAN_FLAG_TME2);
}
}
Robot_V2/device/Can.h
+3 -2
View File
@@ -60,6 +60,7 @@ void can_dma_init(void); // CAN DMA初始化
void CAN0_RX0_IRQHandler(void); // CAN接收中断处理函数
void DMA0_Channel4_IRQHandler(void); // DMA通道4中断处理函数(CAN接收)
void DMA0_Channel5_IRQHandler(void); // DMA通道5中断处理函数(CAN发送)
void jumpToBOOT();
void can_loopback_init(void);
void can_loopback_test(void);
@@ -97,8 +98,8 @@ class CAN_transmiter {
void update(); // 更新数据
u8 CAN_Receive(); // CAN接收
u8 CANFD_Receive(); // CANFD接收
u8 CAN_Send(u32 ID, u8 length, u8 *data); // CAN发送
u8 CANFD_Send(u32 ID, u8 length, u8 *data); // CANFD发送
u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data); // CAN异步发送(使用DMA)
u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA)
u8 CAN_all_Send(); // CAN发送
u8 CANFD_all_Send(); // CANFD发送
Robot_V2/device/Timer.cpp
+10 -20
View File
@@ -1,6 +1,6 @@
#include "Timer.h"
#include "Can.h"
#include "DengFOC.h"
#include "FOC.h"
#include "Timer.h"
#include "gd32c10x.h"
#include "gd32c10x_gpio.h"
@@ -12,16 +12,6 @@
#include <stdint.h>
#include <stdio.h>
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
constexpr int TIMER1_CH0_PULSE = 0;
constexpr int TIMER1_CH1_PULSE = 0;
constexpr int TIMER1_CH2_PULSE = 0;
constexpr int TIMER1_CH3_COMPARE_VALUE =
(TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断
volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
extern u16 adc_value[7];
@@ -46,7 +36,7 @@ void nvic_config(void) {
}
/**
* @brief 配置定时器2
* @brief 配置定时器2-任务管理
* @return void
*
*配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务
@@ -87,7 +77,7 @@ void TIMER2_config(void) {
}
/**
* @brief 配置TIMER2中断
* @brief 配置TIMER2-任务管理中断
* @return void
*/
void User_timer_init(void) {
@@ -98,7 +88,7 @@ void User_timer_init(void) {
}
/**
* @brief TIMER2中断处理函数
* @brief TIMER2-任务管理中断处理函数
* @return void
*/
void TIMER2_IRQHandler(void) {
@@ -174,7 +164,7 @@ void TIMER2_IRQHandler(void) {
}
/**
* @brief TIMER1 GPIO配置函数
* @brief TIMER1-PWM GPIO配置函数
*/
void timer1_gpio_config(void) {
// 时钟使能
@@ -198,7 +188,7 @@ void timer1_gpio_config(void) {
}
/**
* @brief TIMER1 配置函数
* @brief TIMER1-PWM 配置函数
*/
void timer1_pwm_config(void) {
timer_parameter_struct timer_initpara;
@@ -242,11 +232,11 @@ void timer1_pwm_config(void) {
TIMER_OC_MODE_PWM0); // CH3也设为PWM模式
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
TIMER1_CH0_PULSE);
TIMER1_DEFAULT_PULSE);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1,
TIMER1_CH1_PULSE);
TIMER1_DEFAULT_PULSE);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
TIMER1_CH2_PULSE);
TIMER1_DEFAULT_PULSE);
// 设置CH3比较值为PWM周期x%位置
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_3,
TIMER1_CH3_COMPARE_VALUE);
@@ -262,7 +252,7 @@ void timer1_pwm_config(void) {
timer_enable(TIMER1);
}
// 添加TIMER1中断处理函数
// 添加TIMER1-PWM中断处理函数
/**
* @brief TIMER1中断处理函数
* @return void
Robot_V2/device/Timer.h
-4
View File
@@ -16,10 +16,6 @@ void timer1_pwm_config(void);
void timer1_gpio_config(void);
void TIMER1_IRQHandler(void); // 添加TIMER1中断处理函数声明
/* 获取系统运行时间 */
uint64_t get_system_runtime_us(void);
#ifdef __cplusplus
}
#endif
Robot_V2/device/adc.h
+8 -8
View File
@@ -6,16 +6,16 @@ extern "C" {
#endif
#include "stdint.h"
constexpr u8 FILTER_SIZE = 6;
constexpr u8 BLOCK_SIZE = FILTER_SIZE * sizeof(float);
#include "config.h"
class sampler {
private:
float vbus_samples[FILTER_SIZE + 1];
float currU_samples[FILTER_SIZE + 1];
float currV_samples[FILTER_SIZE + 1];
float currW_samples[FILTER_SIZE + 1];
float mcu_temp_samples[FILTER_SIZE + 1];
float drv_temp_samples[FILTER_SIZE + 1];
float vbus_samples[ADC_FILTER_SIZE + 1];
float currU_samples[ADC_FILTER_SIZE + 1];
float currV_samples[ADC_FILTER_SIZE + 1];
float currW_samples[ADC_FILTER_SIZE + 1];
float mcu_temp_samples[ADC_FILTER_SIZE + 1];
float drv_temp_samples[ADC_FILTER_SIZE + 1];
u8 current_sample_index;
float tempora_sample;
Robot_V2/device/ang_sensor.cpp
+12 -27
View File
@@ -1,31 +1,20 @@
#include "ang_sensor.h"
#include "Can.h"
#include "Filter.h"
#include "core_cmFunc.h"
#include "gd32c10x_can.h"
#include "flash.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include "initial.h"
#include "flash.h"
#include <cstdio>
#include <cstring>
#include "Can.h"
#include <stdint.h>
#define ANG_SENS_DEBUG 1
#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
#define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
constexpr u16 DEFAULT_COMMAND = 0x0000;
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 u8 SIZE_FLOAT = sizeof(float);
// DMA传输完成标志
volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
@@ -34,7 +23,7 @@ angle_sensor KTH7823; // 角度传感器初始化
extern FlashEEPROM flashEEP;
extern EncoderAngleKalman Angle_kalman;
extern EncoderVelocityKalman Vel_kalman;
extern CAN_transmiter CAN0_transmiter; // CAN初始化
extern CAN_transmiter CAN0_transmiter; // CAN初始化
/**
* @brief SPI DMA initialization
@@ -199,10 +188,9 @@ void KTH7823_spi_init(void) {
spi_enable(SPI0);
}
/**
* @ 角度传感器类 初始化
* @return 0:初始化成功 1:初始化失败
* @ 角度传感器类 初始化
* @return 0:初始化成功 1:初始化失败
*/
u8 angle_sensor::init(void) {
KTH7823_gpio_init(); // GPIO初始化
@@ -211,17 +199,16 @@ u8 angle_sensor::init(void) {
return 0;
}
/**
* @ 角度传感器类 设置当前角度
* @param angle 输出级角度值
* @return 0成功/1失败
* @ 角度传感器类 设置当前角度
* @param angle 输出级角度值
* @return 0成功/1失败
*/
u8 angle_sensor::angle_set(float angle){
m_cumulativeAngle = angle*REDUCTION_RATIO; // 从Flash读取上次保存的角度
u8 angle_sensor::angle_set(float angle) {
m_cumulativeAngle = angle * REDUCTION_RATIO; // 从Flash读取上次保存的角度
m_firstUpdate = true;
return 0;
}
/**
* @brief 读取传感器数据
* @return 传感器数据
@@ -235,19 +222,17 @@ inline float angle_sensor::read_sensorAngle(void) {
if (result == 0) {
// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
float angle_deg = (float)raw_data * ANGLE_FACTOR;
float angle_deg = (float)raw_data * ANGLESENS_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");
}
#endif
return (float)FAULT_RECEIVE;
}
Robot_V2/device/ang_sensor.h
+1 -1
View File
@@ -5,12 +5,12 @@ extern "C" {
#endif
#include "gd32c10x_eval.h"
#include "stdio.h"
#include "config.h"
u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
uint16_t *rx_data);
constexpr u16 VEL_FILTER_SIZE = 4;
class angle_sensor {
public:
Robot_V2/main.cpp
+48 -50
View File
@@ -1,16 +1,16 @@
#include "main.h"
#include "Can.h"
#include "DengFOC.h"
#include "FOC.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "core_cm4.h"
#include "core_cmFunc.h"
#include "flash.h"
#include "health.h"
#include "initial.h"
#include "motor.h"
#include "sys/config.h"
#include "sys/flash.h"
#include "sys/initial.h"
#include "systick.h"
#include <cstdio>
@@ -28,35 +28,14 @@ TaskScheduler scheduler = {0};
volatile u32 system_runtime_us = 0; // 电机驱动计算轮次
volatile u32 cnt_test = 0; // 负载检测
int main(void) {
systick_config(); // 系统时钟初始化
SystemCoreClockUpdate(); // 更新系统时钟变量
watchdog_init(50); // 看门狗初始化
unused_pin_init(); // 未使用引脚初始化
USART0_init(); // 调试串口初始化
motor_init(); // 电机驱动初始化
if (get_reset_cause() ==
RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠
printf("continue sleep\r\n");
watchdog_init(5000); // 看门狗初始化
delay_1ms(1);
prepare_deep_sleep(false);
}
User_timer_init(); // 任务Timer初始化
u8 init_status = 0;
init_status += KTH7823.init(); // 角度传感器初始化
init_status += sampler.init(); // 采样器初始化
init_status +=
flashEEP.init(); // EEPROM初始化，必须放在角度卡尔曼之前，角度传感器之后
init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
init_status += Vel_kalman.init(0.0f); // 速度滤波初始化
init_status += CAN0_transmiter.init(); // CAN初始化
u8 init_status = hardware_init();
while (init_status) { // 初始化检测
std::printf("初始化失败\r\n");
}
OTA_initial(); // OTA初始化,A/B分区处理
angsens_MG_check(); // 角度传感器磁感应强度测试
while (1) {
__disable_irq();
u32 currentTasks =
@@ -69,6 +48,32 @@ int main(void) {
return 0;
}
u8 hardware_init(void) {
__enable_irq();
systick_config(); // 系统时钟初始化
watchdog_init(50); // 看门狗初始化
unused_pin_init(); // 未使用引脚初始化
USART0_init(); // 调试串口初始化
motor_init(); // 电机驱动初始化
if (get_reset_cause() ==
RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠
printf("continue sleep\r\n");
watchdog_init(5000); // 看门狗初始化
delay_1ms(3000);
prepare_deep_sleep(false);
}
User_timer_init(); // 任务Timer初始化
u8 init_status = 0;
init_status += KTH7823.init(); // 角度传感器初始化
init_status += sampler.init(); // 采样器初始化
flashEEP.init(); // EEPROM初始化，必须放在角度卡尔曼之前，角度传感器之后
init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
init_status += Vel_kalman.init(0.0f); // 速度滤波初始化
init_status += CAN0_transmiter.init(); // CAN初始化
return init_status;
}
void do_task(u32 currentTasks) {
// 按照优先级和重要性处理任务
if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
@@ -83,29 +88,22 @@ void do_task(u32 currentTasks) {
}
if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
system_runtime_us++;
switch (FOC.control_mode) {
case 0: {
FOC.drv_hold(0.95); // 占空比置0
break;
}
case 1: {
// FOC.angle_control(FOC.target_angle); // 角度闭环控制
FOC.angle_control_with_velocity_limit(FOC.target_angle); // 角度闭环控制，带速度限制
break;
}
case 2: {
FOC.velocity_control(FOC.target_velocity); // 速度闭环控制
break;
}
case 3: {
// FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制
FOC.current_control(FOC.limit_velocity); // 电流闭环控制
break;
}
case 4: {
FOC.velocity_open_loop(); // 速度开环控制
break;
}
// 函数指针数组，包含各种控制模式对应的函数
static FOCFunctionPtr control_functions[] = {
&FOC_Controlroller::drv_hold, // 0: 占空比置
&FOC_Controlroller::
angle_control_with_velocity_limit, // 1:
// 角度闭环控制，带速度限制
&FOC_Controlroller::velocity_control, // 2: 速度闭环控制
&FOC_Controlroller::current_control, // 3: 电流闭环控制
&FOC_Controlroller::velocity_open_loop // 4: 速度开环控制
};
// 边界检查，防止数组越界
if (FOC.control_mode >= 0 && FOC.control_mode < 5) {
// 调用对应的控制函数
(FOC.*control_functions[FOC.control_mode])();
}
}
if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值
Robot_V2/main.h
+1
View File
@@ -37,5 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "stdint.h"
void do_task(u32 currentTasks);
u8 hardware_init(void);
#endif /* MAIN_H */
Robot_V2/sys/config.h
+88
View File
@@ -0,0 +1,88 @@
#ifndef __CONFIG_H
#define __CONFIG_H
#include <stdint.h>
// 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4
// #define ARM_MATH_CM4
#define EEP_DEBUG 1
// BOOT 相关
constexpr u32 BOOTLOADER_START_ADDR = 0x08000000; // BOOT起始地址
constexpr u32 OTA_CONFIG_NUM = 6;
constexpr u32 n_APP_LATEST_AVAL_ADDR = 0x0801D000;
constexpr u32 n_APP_BACKUP_AVAL_ADDR = 0x0801D004;
constexpr u32 n_APP_LATEST_EN_ADDR = 0x0801D008;
constexpr u32 n_APP_BACKUP_EN_ADDR = 0x0801D00C;
constexpr u32 n_APP_LATEST_CHECK_ADDR = 0x0801D010;
constexpr u32 n_APP_BACKUP_CHECK_ADDR = 0x0801D014;
constexpr u32 OTA_NOT_AVAL = 0xFFFFFFFF;
constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用
// 参数配置
constexpr u8 PP = 11;
constexpr float DIR = 1.0;
constexpr u16 REDUCTION_RATIO = 25;
constexpr float CURRENT_OFFSET = -1.615f; // 当前电流传感器的零点偏移
constexpr float HOLD_PWM_DUTY = 0.95f;
constexpr u32 CANFD_BOOT_ID = 0x00000000;
constexpr u32 CANFD_STATUS_ID = 0x00010100;
constexpr u32 CANFD_CONFIG_ID = 0x00010101;
constexpr u32 CANFD_CMD_ID = 0x00010200;
constexpr u32 CANFD_RESET_ID = 0x00010301;
// 常量定义
constexpr u8 SIZE_FLOAT = sizeof(float);
constexpr u8 SIZE_U32 = sizeof(uint32_t);
constexpr float ADC_MAX = 65536.0f;
constexpr float PI = 3.14159265358979323846f;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * PI;
constexpr float DEG2RAD = 1.0f / 360.0f * 2 * PI;
constexpr float RAD2DEG = 360.0f / 2 * PI;
constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
constexpr u8 DEFAULT_VALUE = 0;
constexpr u8 CONFIG_SAVED_NUM = 6;
// 三角函数快速计算LUT
constexpr u16 LUT_SIZE = 1024;
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
// CAN相关
constexpr u16 DEV_CAN0_ID = 0x0000;
constexpr u16 DEV_CAN0_MASK = 0x0000;
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
// ADC
constexpr u8 ADC_FILTER_SIZE = 6;
constexpr u8 BLOCK_SIZE = ADC_FILTER_SIZE * SIZE_FLOAT;
constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float MAX_TEMP = 150.0f;
constexpr float MIN_TEMP = -40.0f;
constexpr float MAX_CURRENT = 40.0f;
constexpr float MIN_CURRENT = -40.0f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;
// ang_sensor
constexpr u16 VEL_FILTER_SIZE = 4;
constexpr u16 DEFAULT_COMMAND = 0x0000;
constexpr u16 FAULT_RECEIVE = 0xFFFF;
constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
constexpr float ANGLE_WRAP_VALUE = 360.0f;
constexpr float RAD_FACTOR = 1.0f / ADC_MAX * 2.0f * PI;
constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
// TIMER2-任务调度
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us
// TIMER1-PWM
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
constexpr int TIMER1_DEFAULT_PULSE = 0;
constexpr int TIMER1_CH3_COMPARE_VALUE =
(TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断
#endif
Robot_V2/sys/flash.cpp
+226 -51
View File
@@ -1,26 +1,28 @@
#include "flash.h"
#include "DengFOC.h"
#include "FOC.h"
#include "ang_sensor.h"
#include "config.h"
#include "gd32c10x.h"
#include "gd32c10x_crc.h"
#include "gd32c10x_fmc.h"
#include "math.h"
#include <Can.h>
#include <Filter.h>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <stdint.h>
FlashEEPROM flashEEP = FlashEEPROM();
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC; // FOC控制
constexpr u8 SIZE_U32 = sizeof(uint32_t);
constexpr u8 SIZE_FLOAT = sizeof(float);
constexpr u8 DEFAULT_VALUE = 0;
constexpr u8 NUM_SAVED = 6;
#define EEPDEBUG 0
inline fmc_state_enum write_ADDRESS(u32 address, u32 data) {
fmc_unlock();
fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
// 确保使用对齐地址
fmc_state_enum fmc_status = fmc_word_program(address, data);
fmc_lock();
return fmc_status;
}
// FlashEEPROM类实现
FlashEEPROM::FlashEEPROM() {
// 构造函数可以为空，或者做一些初始化工作
@@ -56,8 +58,8 @@ u8 FlashEEPROM::init() {
u8 FlashEEPROM::read_saved_system_config(void) {
// 读取保存的参数
u32 CRC_NUM = 0x0000;
FlashEepromStatus status_read[NUM_SAVED + 1] = {FLASH_EEPROM_OK};
float data[NUM_SAVED] = {0.0f};
FlashEepromStatus status_read[CONFIG_SAVED_NUM + 1] = {FLASH_EEPROM_OK};
float data[CONFIG_SAVED_NUM] = {0.0f};
// 尝试从Flash读取参数，如果不存在则使用默认值
status_read[1] = readFloat(ADDR_CONTROL_MODE, &data[0]);
status_read[2] = readFloat(ADDR_VOLTAGE_LIMIT, &data[1]);
@@ -67,7 +69,7 @@ u8 FlashEEPROM::read_saved_system_config(void) {
status_read[6] = readFloat(ADDR_TARGET_VEL, &data[5]);
// 读取失败则使用默认值
for (u8 i = 1; i < NUM_SAVED + 1; i++) {
for (u8 i = 1; i < CONFIG_SAVED_NUM + 1; i++) {
if (status_read[i] ==
FLASH_EEPROM_DATA_NOT_FOUND) { // 首次读取，不保存报错
data[i - 1] = DEFAULT_VALUE;
@@ -99,13 +101,13 @@ u8 FlashEEPROM::read_saved_system_config(void) {
}
crc_data_register_reset(); // 确保CRC计算从初始状态开始
CRC_NUM = crc_block_data_calculate((uint32_t *)data, NUM_SAVED);
CRC_NUM = crc_block_data_calculate((uint32_t *)data, CONFIG_SAVED_NUM);
#if EEPDEBUG
#if EEP_DEBUG
printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
#endif
if (CRC_NUM != *((u32 *)next_address)) { // CRC错误
for (u8 i = 0; i < NUM_SAVED; i++) {
for (u8 i = 0; i < CONFIG_SAVED_NUM; i++) {
data[i] = DEFAULT_VALUE;
}
EEPROMStatus = FLASH_EEPROM_CRC_ERROR;
@@ -185,7 +187,7 @@ FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const {
u32 float_data = data_word & DATA_FLOAT_MASK;
// 使用内存拷贝将处理后的数据解释为float
memcpy(data, &float_data, SIZE_FLOAT);
#if EEPDEBUG
#if EEP_DEBUG
printf("read:0x%x=%f\r\n", address, *data);
#endif
return FLASH_EEPROM_OK;
@@ -218,7 +220,7 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
if ((data_read & DATA_ADDR_MASK) == (u32)virtualAddr) { // 虚拟地址相同
if ((data_read & DATA_U16_MASK) ==
(data & DATA_U16_MASK)) { // 数据相同
#if EEPDEBUG
#if EEP_DEBUG
data_read = data_read & DATA_U16_MASK;
u16 float_data_read = *((u16 *)&data_read);
printf("Found data in:0x%x=%u\r\n", address, data_read);
@@ -249,19 +251,14 @@ FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
if (next_address < EEPROM_END_ADDRESS) {
u32 data_word = ((u32)data << 16) | (u32)virtualAddr;
// 使用GD32库函数进行安全编程
fmc_unlock();
fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
fmc_state_enum fmc_status =
fmc_word_program(next_address, data_word); // 写入数据
fmc_lock();
fmc_state_enum fmc_status = write_ADDRESS(next_address, data_word);
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_WRITE_ERROR;
}
return FLASH_EEPROM_OK;
} else { // 空间不足，需要格式化页面
#if EEPDEBUG
#if EEP_DEBUG
printf("no enough space\r\n");
#endif
FlashEepromStatus status = format();
@@ -301,7 +298,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
(u32)virtualAddr) { // 虚拟地址相同
if ((data_word_read & DATA_FLOAT_MASK) ==
(data_word & DATA_FLOAT_MASK)) { // 数据相同
#if EEPDEBUG
#if EEP_DEBUG
data_word_read = data_word_read & DATA_FLOAT_MASK;
float float_data_read = *((float *)&data_word_read);
printf("Found data in:0x%x=%f\r\n", address, float_data_read);
@@ -330,15 +327,8 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
// 如果有足够空间，直接写入
if (next_address < EEPROM_END_ADDRESS) {
// 使用GD32库函数进行安全编程
fmc_unlock();
fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
// 确保使用对齐地址
fmc_state_enum fmc_status = fmc_word_program(next_address, data_word);
fmc_lock();
#if EEPDEBUG
fmc_state_enum fmc_status = write_ADDRESS(next_address, data_word);
#if EEP_DEBUG
u32 data_word_c = *((u32 *)next_address);
data_word_c = data_word_c & DATA_FLOAT_MASK;
float float_data = *((float *)&data_word_c);
@@ -350,7 +340,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
return FLASH_EEPROM_OK;
} else {
// 空间不足，需要格式化页面
#if EEPDEBUG
#if EEP_DEBUG
printf("no enough space\r\n");
#endif
FlashEepromStatus status = format();
@@ -370,7 +360,7 @@ FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
*/
FlashEepromStatus FlashEEPROM::format() {
fmc_state_enum fmc_status;
#if EEPDEBUG
#if EEP_DEBUG
printf("擦除页面\r\n");
#endif
@@ -395,9 +385,10 @@ FlashEepromStatus FlashEEPROM::format() {
*/
FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
u32 CRC_NUM = 0x0000;
float data[NUM_SAVED] = {(float)FOC.control_mode, FOC.limit_voltage,
FOC.limit_velocity, FOC.rt_angle,
FOC.target_angle, FOC.target_velocity};
float data[CONFIG_SAVED_NUM] = {
(float)FOC.control_mode, FOC.limit_voltage,
FOC.limit_velocity, FOC.rt_angle,
FOC.target_angle, FOC.target_velocity};
// 如果系统配置标记已保存，则无需再次保存
if (system_config_saved) {
@@ -445,10 +436,10 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
}
// 创建用于CRC计算的模拟数据数组，模拟实际存储到Flash中的数据格式
u32 crc_data_words[NUM_SAVED];
u32 crc_data_words[CONFIG_SAVED_NUM];
// 按照writeFloat函数的方式处理数据
for (u8 i = 0; i < NUM_SAVED; i++) {
for (u8 i = 0; i < CONFIG_SAVED_NUM; i++) {
// 将完整的float数据复制到32位变量中
memcpy(&crc_data_words[i], &data[i], SIZE_FLOAT);
// 去除最后2位精度
@@ -456,7 +447,7 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
}
crc_data_register_reset(); // 确保CRC计算从初始状态开始
CRC_NUM = crc_block_data_calculate(crc_data_words, NUM_SAVED);
CRC_NUM = crc_block_data_calculate(crc_data_words, CONFIG_SAVED_NUM);
writeCRC(CRC_NUM, saved);
EEPROMStatus = FLASH_EEPROM_OK;
@@ -501,14 +492,10 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
if (*((u32 *)next_address) != CRC_NUM) {
// 如果有足够空间，直接写入crc值
if (next_address > EEPROM_START_ADDRESS) {
fmc_unlock();
fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM);
fmc_lock();
#if EEPDEBUG
fmc_state_enum fmc_status = write_ADDRESS(next_address, CRC_NUM);
#if EEP_DEBUG
printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
#endif
// 检查写入是否成功
if (fmc_status != FMC_READY) {
EEPROMStatus = fmc_status;
@@ -516,7 +503,7 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
}
} else {
// 空间不足，需要格式化页面
#if EEPDEBUG
#if EEP_DEBUG
printf("no enough space\r\n");
#endif
FlashEepromStatus status = format();
@@ -539,4 +526,192 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
EEPROMStatus = FLASH_EEPROM_CRC_ERROR; // 设置EEPROM错误状态
return FLASH_EEPROM_CRC_ERROR;
}
}
u8 OTA_SET(u32 addr, bool enable) {
fmc_state_enum fmc_status;
u32 temp_val[OTA_CONFIG_NUM];
u32 data = enable ? OTA_NOT_AVAL : OTA_AVAL;
volatile u32 *flash_ptr = (volatile u32 *)addr;
temp_val[0] = *flash_ptr;
if (temp_val[0] == data) { // 如果相同
return FLASH_EEPROM_OK; // 成功
}
fmc_unlock();
// 如果不同，且要写0，直接写
if (data == OTA_AVAL) {
fmc_status = write_ADDRESS(addr, data);
fmc_lock();
if (fmc_status == FMC_READY) {
return FLASH_EEPROM_OK; // 成功可以直接退出
}
}
// 如果不同或写入出错，要写1，需要重置，然后再写
flash_ptr = (volatile u32 *)n_APP_LATEST_AVAL_ADDR;
temp_val[0] = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR;
temp_val[1] = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR;
temp_val[2] = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_BACKUP_EN_ADDR;
temp_val[3] = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR;
temp_val[4] = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR;
temp_val[5] = *flash_ptr;
#if EEP_DEBUG
printf("OTA_SET\r\n");
printf("0x%x_0x%x_0x%x_0x%x\r\n", temp_val[0], temp_val[1], temp_val[2],
temp_val[3]);
#endif
fmc_status = fmc_page_erase(n_APP_LATEST_AVAL_ADDR); // 擦除页
if (fmc_status != FMC_READY) {
return FLASH_EEPROM_ERASE_ERROR; // 擦除错误退出
}
for (int i = 0; i < OTA_CONFIG_NUM; i++) {
if (temp_val[i] == OTA_AVAL and
n_APP_LATEST_AVAL_ADDR + 4 * i != addr) {
fmc_status = write_ADDRESS(n_APP_LATEST_AVAL_ADDR + 4 * i,
OTA_AVAL); // 把需要写入0的写入0
if (fmc_status != FMC_READY) {
fmc_lock();
return FLASH_EEPROM_WRITE_ERROR; // 写入错误退出
}
}
}
fmc_lock();
return FLASH_EEPROM_OK;
}
/**
* @brief 跳转应用处理模块
* @param 无
* @retval 无
*/
void jumpToBOOT() {
OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活
// 1. 关闭所有中断
__disable_irq();
for (int i = 0; i < 8; i++) {
NVIC->ICER[i] = 0xFFFFFFFF;
}
// 2. 重置外设（特别是CAN，防止冲突）
can_deinit(CAN0);
// 3. 重置系统时钟到默认状态
rcu_system_clock_source_config(RCU_CKSYSSRC_IRC8M);
// 4. 关闭一些关键外设时钟
rcu_periph_clock_disable(RCU_GPIOA);
rcu_periph_clock_disable(RCU_GPIOB);
rcu_periph_clock_disable(RCU_GPIOC);
rcu_periph_clock_disable(RCU_USART0);
rcu_periph_clock_disable(RCU_CAN0);
rcu_periph_clock_disable(RCU_TIMER0);
rcu_periph_clock_disable(RCU_DMA0);
// 5. 设置中断向量表偏移到应用程序起始地址
SCB->VTOR = BOOTLOADER_START_ADDR;
// 6. 设置MSP并跳转
uint32_t msp_value = *(__IO uint32_t *)BOOTLOADER_START_ADDR;
uint32_t reset_handler = *(__IO uint32_t *)(BOOTLOADER_START_ADDR + 4);
typedef void (*AppEntryFunc)(void);
AppEntryFunc app_entry = (AppEntryFunc)reset_handler;
__set_MSP(msp_value);
// 7. 跳转
app_entry();
// 如果程序执行到这里，说明跳转失败
printf("Jump to application failed!\r\n");
}
/**
* @brief 将0x08011000地址后0xC000大小区域内的数据搬运到0x08050000地址
* @param None
* @retval fmc_state_enum: 操作状态
*/
fmc_state_enum move_app_data() {
// 源地址: 0x08011000
// 目标地址: 0x08050000
// 数据大小: 0xC000 (49152字节)
uint32_t src_addr = 0x08011000;
uint32_t dst_addr = 0x08050000;
uint32_t size = 0xC000; // 48KB
fmc_state_enum fmc_status = FMC_READY;
// 计算源数据的CRC
rcu_periph_clock_enable(RCU_CRC);
crc_deinit();
crc_data_register_reset();
// 临时缓冲区用于计算CRC
uint32_t word_count = size / 4;
uint32_t src_crc, dst_crc;
// 计算源数据CRC
src_crc = crc_block_data_calculate((uint32_t *)src_addr, word_count);
// 关闭全局中断以防止Flash操作期间的中断干扰
__disable_irq();
fmc_unlock();
// 首先擦除目标地址的Flash页面
// 计算需要擦除的页面数量（GD32C103的Flash页大小为1KB）
uint32_t page_size = 1024; // 1KB per page
uint32_t num_pages = (size + page_size - 1) / page_size; // 向上取整
for (uint32_t i = 0; i < num_pages; i++) {
uint32_t page_addr = dst_addr + i * page_size;
// 擦除页面
fmc_status = fmc_page_erase(page_addr);
if (fmc_status != FMC_READY) {
fmc_lock();
__enable_irq(); // 重新开启中断
return fmc_status;
}
}
// 复制数据 - 按字（32位）进行复制以提高效率
for (uint32_t offset = 0; offset < size; offset += 4) {
uint32_t src_data = *(volatile uint32_t *)(src_addr + offset);
// 检查源地址是否为有效的Flash数据（非0xFFFFFFFF表示有数据）
if (src_data != 0xFFFFFFFF) {
fmc_status = fmc_word_program(dst_addr + offset, src_data);
if (fmc_status != FMC_READY) {
fmc_lock();
__enable_irq(); // 重新开启中断
return fmc_status;
}
}
}
fmc_lock();
// 重新开启全局中断
__enable_irq();
// 计算目标数据的CRC
crc_data_register_reset();
dst_crc = crc_block_data_calculate((uint32_t *)dst_addr, word_count);
// 比较源数据和目标数据的CRC
if (src_crc != dst_crc) {
// 如果CRC不匹配，可以选择返回错误状态
// 这里我们返回一个特殊值表示CRC校验失败
return (fmc_state_enum)0xFF; // 自定义错误码表示CRC校验失败
}
// 设置备份应用可用标志
OTA_SET(n_APP_BACKUP_AVAL_ADDR, OTA_AVAL);
OTA_SET(n_APP_BACKUP_CHECK_ADDR, OTA_AVAL);
return fmc_status;
}
Robot_V2/sys/flash.h
+5
View File
@@ -2,6 +2,8 @@
#define __FLASH_H
#include <stdint.h>
#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用
// 修改后 - 强制4字节对齐
#define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐
@@ -69,4 +71,7 @@ class FlashEEPROM {
};
void flash_eeprom_example_main(void);
u8 OTA_SET(u32 addr, bool enable);
fmc_state_enum move_app_data(void);
#endif /* __FLASH_H */
Robot_V2/sys/health.cpp
+4 -2
View File
@@ -1,9 +1,11 @@
#include "health.h"
#include "config.h"
#include <stdint.h>
#include "Can.h"
#include "DengFOC.h"
#include "FOC.h"
#include "adc.h"
#include "flash.h"
#include <stdint.h>
extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
Robot_V2/sys/initial.cpp
+20 -1
View File
@@ -1,7 +1,10 @@
#include "initial.h"
#include "config.h"
#include "flash.h"
#include "gd32c10x.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_rcu.h"
#include "gd32c10x_fmc.h"
#include <cstdio>
#include <stdint.h>
#include <stdio.h>
@@ -27,7 +30,6 @@ u8 watchdog_init(u32 time) {
return status;
}
int fputc(int ch, FILE *f) throw() {
usart_data_transmit(EVAL_COM0, (uint8_t)ch);
while (RESET == usart_flag_get(EVAL_COM0, USART_FLAG_TBE))
@@ -91,3 +93,20 @@ u16 get_reset_cause(void) {
// printf("cause:%d\r\n", cause);
return cause;
}
void OTA_initial(void) {
// 确认当前APP可用
OTA_SET(n_APP_LATEST_CHECK_ADDR, true);
volatile u32 *flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR;
u32 back_aval = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_BACKUP_CHECK_ADDR;
u32 back_check = *flash_ptr;
// 如果BACK不可用
if (back_aval == OTA_NOT_AVAL) {
move_app_data();
} else if (back_aval == OTA_AVAL &&
back_check == OTA_NOT_AVAL) { // BACK已刷写，但未检查
move_app_data();
}
}
Robot_V2/sys/initial.h
+11 -13
View File
@@ -5,23 +5,11 @@
extern "C" {
#endif
#include "config.h"
#include "gd32c10x_eval.h"
#include "stdint.h"
#include <stdio.h>
constexpr u16 REDUCTION_RATIO = 25;
constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
constexpr float RAD2DEG = 360.0f / 6.28318530718f;
void rcu_config(void);
static volatile uint32_t timer_delay_count = 0; // volatile 很重要！
int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现
u8 USART0_init(void);
u8 watchdog_init(u32 time);
u16 get_reset_cause(void);
void unused_pin_init();
// 任务标志定义
typedef enum {
TASK_MOTOR_CONTROL = 0,
@@ -37,6 +25,16 @@ typedef enum {
TASK_COUNT
} SystemTask;
void rcu_config(void);
static volatile uint32_t timer_delay_count = 0; // volatile 很重要！
int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现
u8 USART0_init(void);
u8 watchdog_init(u32 time);
u16 get_reset_cause(void);
void unused_pin_init();
void OTA_initial(void);
// 任务管理结构
typedef struct {
volatile u32 taskFlags; // 任务标志位(每个位代表一个任务)
Robot_V2/sys/systick.cpp
+6 -1
View File
@@ -83,7 +83,6 @@ void delay_1ms(uint32_t count) {
delay = count;
while (0U != delay) {
fwdgt_counter_reload();
fwdgt_counter_reload();
}
}
@@ -177,4 +176,10 @@ void EXTI5_9_IRQHandler(void) {
printf("MCU wake up\r\n");
system_soft_reset();
}
}
void OTA_initial(){
}