feat(FOC): 集成ARM数学库优化FOC算法实现

- 引入arm_math.h库以使用ARM优化的数学函数
- 将角度归一化从弧度制改为角度制，使用360.0f替代2*PI
- 使用arm_sin_cos_f32替代查找表实现正弦余弦计算
- 移除注释中的ARM Clark和Park变换函数调用
- 修正机械角度计算中的时间常量为TIMER2_FREQUENCY倒数
- 移除传感器角度到弧度的转换，保持原始角度值
- 激活电流环控制中的Iq/Id计算并发送CAN数据

Robot_V2/main.cpp

@@ -9,10 +9,11 @@
 #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 <arm_math.h>
+#include <cmath>
 #include <cstdio>
 #include <stdint.h>
 extern CAN_transmiter CAN0_transmiter;		// CAN初始化
@@ -25,8 +26,8 @@ extern FOC_Controlroller FOC;				// FOC控制
 extern FlashEEPROM flashEEP;				// EEPROM类
 
 TaskScheduler scheduler = {0};
-volatile u32 system_runtime_us = 0; // 电机驱动计算轮次
-volatile u32 cnt_test = 0;			// 负载检测
+volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次
+volatile u32 cnt_test = 0;			 // 负载检测
 int main(void) {
 	u8 init_status = hardware_init();
 
@@ -35,9 +36,8 @@ int main(void) {
 	}
 	OTA_initial(); //  OTA初始化,A/B分区处理
 
-	watchdog_init(50);	// 看门狗初始化
 	angsens_MG_check(); // 角度传感器磁感应强度测试
-
+	watchdog_init(50);	// 看门狗初始化
 	while (1) {
 		__disable_irq();
 		u32 currentTasks =
@@ -60,7 +60,6 @@ u8 hardware_init(void) {
 		RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠
 		printf("continue sleep\r\n");
 		watchdog_init(5000); // 看门狗初始化
-		delay_1ms(3000);
 		prepare_deep_sleep(false);
 	}
 	User_timer_init(); // 任务Timer初始化
@@ -76,6 +75,7 @@ u8 hardware_init(void) {
 }
 
 void do_task(u32 currentTasks) {
+	cnt_test++;
 	// 按照优先级和重要性处理任务
 	if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
 		sampler.update_current();
@@ -88,7 +88,7 @@ void do_task(u32 currentTasks) {
 		FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
 	}
 	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
-		system_runtime_us++;
+		motor_runtime_50us++;
 
 		// 函数指针数组，包含各种控制模式对应的函数
 		static FOCFunctionPtr control_functions[] = {
@@ -122,8 +122,8 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
 		power_manage();
 	}
-	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
-		// CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
+	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {	  // CAN通信
+		CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
 		cnt_test = 0;
 		CAN0_transmiter.can0_send_flag = SET;
 		CAN0_transmiter.communication_check();