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/sys/config.h

@@ -3,7 +3,7 @@
 #include <stdint.h>
 
 // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4
-// #define ARM_MATH_CM4
+#define ARM_MATH_CM4
 
 #define EEP_DEBUG 1
 #define OTA_DEBUG 1
@@ -39,9 +39,9 @@ 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 ANGLE_FACTOR = 25.0f / 180.0f * PI;
+constexpr float DEG2RAD = 1.0f / 180.0f * PI;
+constexpr float RAD2DEG = 180.0f / PI;	   // 修正RAD2DEG定义，应该是180/π
 constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
 constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
 constexpr u8 DEFAULT_VALUE = 0;
@@ -89,7 +89,8 @@ 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
+constexpr int TIMER2_PERIOD = 49;	  // 1MHz / 50 = 50us
+constexpr float TIMER2_FREQUENCY = 1 / 5e-5;
 
 // TIMER1-PWM
 constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz

Robot_V2/sys/flash.h

@@ -1,8 +1,8 @@
 #ifndef __FLASH_H
 #define __FLASH_H
 
+#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用
 #include <stdint.h>
-#include "gd32c10x.h"  // 添加GD32主头文件以确保外设函数声明可用
 
 // 修改后 - 强制4字节对齐
 #define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐
@@ -71,7 +71,7 @@ class FlashEEPROM {
 };
 
 void flash_eeprom_example_main(void);
-u8 OTA_SET(u32 addr, u32 data) ;
+u8 OTA_SET(u32 addr, u32 data);
 fmc_state_enum move_app_data(void);
 
 #endif /* __FLASH_H */

Robot_V2/sys/initial.cpp

@@ -1,4 +1,5 @@
 #include "initial.h"
+#include "Can.h"
 #include "config.h"
 #include "flash.h"
 #include "gd32c10x.h"
@@ -7,7 +8,6 @@
 #include "gd32c10x_rcu.h"
 #include <cstdio>
 #include <stdint.h>
-#include "can.h"
 #include <stdio.h>
 
 extern CAN_transmiter CAN0_transmiter; // CAN初始化
@@ -97,9 +97,13 @@ u16 get_reset_cause(void) {
 }
 
 void OTA_initial(void) {
+
 	// 确认当前APP可用
 	OTA_SET(n_APP_LATEST_CHECK_ADDR, OTA_AVAL);
 	OTA_SET(n_APP_LATEST_EN_ADDR, OTA_AVAL);
+	u8 transmit_data[2] = {0x0A, 0xAC};
+	// 确认APP可用
+	CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data);
 
 	volatile u32 *flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR;
 	u32 back_aval = *flash_ptr;
@@ -108,9 +112,14 @@ void OTA_initial(void) {
 
 	// 如果BACK不可用
 	if (back_aval == OTA_NOT_AVAL) {
+		std::printf("notaval\r\n");
 		move_app_data();
 	} else if (back_aval == OTA_AVAL &&
 			   back_check == OTA_NOT_AVAL) { // BACK已刷写，但未检查
+		std::printf("aval\r\n");
 		move_app_data();
 	}
+	else {
+		printf("check\r\n");
+	}
 }

Robot_V2/sys/systick.cpp

@@ -34,9 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "systick.h"
 #include "Can.h"
+#include "core_cmFunc.h"
 #include "flash.h"
 #include "gd32c10x.h"
 #include "gd32c10x_exti.h"
+#include "initial.h"
 #include "motor.h"
 #include <cstdio>
 
@@ -157,6 +159,7 @@ void prepare_deep_sleep(bool save_data) {
 	// 确保中断标志已被清除
 	exti_interrupt_flag_clear(EXTI_8);
 
+	watchdog_init(5000); // 看门狗初始化
 	// 进入深度睡眠
 	pmu_to_deepsleepmode(PMU_LDO_LOWPOWER, WFI_CMD);
 }
@@ -177,9 +180,3 @@ void EXTI5_9_IRQHandler(void) {
 		system_soft_reset();
 	}
 }
-
-
-void OTA_initial(){
-	
-
-}