feat(FOC): 集成ARM数学库优化FOC算法实现
- 引入arm_math.h库以使用ARM优化的数学函数 - 将角度归一化从弧度制改为角度制，使用360.0f替代2*PI - 使用arm_sin_cos_f32替代查找表实现正弦余弦计算 - 移除注释中的ARM Clark和Park变换函数调用 - 修正机械角度计算中的时间常量为TIMER2_FREQUENCY倒数 - 移除传感器角度到弧度的转换，保持原始角度值 - 激活电流环控制中的Iq/Id计算并发送CAN数据
This commit is contained in:
+24
-22
@@ -6,11 +6,11 @@
|
||||
#include "config.h"
|
||||
#include "motor.h"
|
||||
#include "pid.h"
|
||||
#include <arm_math.h>
|
||||
#include <cmath>
|
||||
#include <gd32c10x.h>
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
FOC_Controlroller FOC; // FOC控制初始化
|
||||
extern sampler sampler;
|
||||
extern angle_sensor KTH7823;
|
||||
@@ -22,16 +22,16 @@ extern CAN_transmiter CAN0_transmiter;
|
||||
#define constrain(value, min, max, target_min, target_max) \
|
||||
((value - min) / (max - min) * (target_max - target_min))
|
||||
|
||||
/**
|
||||
/**@
|
||||
* @brief 归一化角度
|
||||
*
|
||||
* @param angle
|
||||
* @return float
|
||||
*/
|
||||
inline float _normalizeAngle(float angle) {
|
||||
float a = fmod(angle, 2 * PI);
|
||||
float a = fmod(angle, 360.0f);
|
||||
// 取余运算可以用于归一化，列出特殊值例子算便知
|
||||
return a >= 0 ? a : (a + 2 * PI);
|
||||
return a >= 0 ? a : (a + 360.0f);
|
||||
}
|
||||
|
||||
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
|
||||
@@ -40,16 +40,19 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
|
||||
// 计算电气角度
|
||||
float angle_el = _normalizeAngle(shaft_angle * PP);
|
||||
|
||||
float I_alpha, I_beta;
|
||||
// Clarke变换 (使用三相电流)
|
||||
float I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
|
||||
(1.0f / 3.0f) * current_c;
|
||||
float I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
|
||||
// arm_clarke_f32(current_a, current_b, &I_alpha, &I_beta);
|
||||
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
|
||||
(1.0f / 3.0f) * current_c;
|
||||
I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
|
||||
|
||||
// Park变换
|
||||
float sin_el, cos_el;
|
||||
cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
|
||||
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
|
||||
float I_q = I_beta * cos_el - I_alpha * sin_el;
|
||||
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||||
float I_q, I_d;
|
||||
I_q = I_beta * cos_el - I_alpha * sin_el;
|
||||
// arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el);
|
||||
|
||||
return I_q * 1000.0f; // 返回Iq值，乘以1000以转换为mA
|
||||
}
|
||||
@@ -73,8 +76,7 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
|
||||
|
||||
// 帕克逆变换 (直接计算，避免函数调用开销)
|
||||
float sin_el, cos_el;
|
||||
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
|
||||
cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
|
||||
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||||
float Ualpha = Ud * cos_el - Uq * sin_el;
|
||||
float Ubeta = Ud * sin_el + Uq * cos_el;
|
||||
|
||||
@@ -93,7 +95,6 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
|
||||
duty_b = duty_b * voltage_exchange;
|
||||
duty_c = duty_c * voltage_exchange;
|
||||
Set_3Pwm(duty_a, duty_b, duty_c);
|
||||
// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -156,8 +157,8 @@ void FOC_Controlroller::update_power(void) {
|
||||
float FOC_Controlroller::velocity_open_loop() {
|
||||
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
|
||||
// 预计算常量 1e-4 = 100微秒控制周期
|
||||
shaft_angle =
|
||||
_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);
|
||||
shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR *
|
||||
1.0 / TIMER2_FREQUENCY);
|
||||
|
||||
// 因为在开环控制中，Ud始终为0，只控制Uq
|
||||
const float Uq = voltage_power_supply / 2.0f;
|
||||
@@ -195,7 +196,7 @@ float FOC_Controlroller::angle_control() {
|
||||
float Uq = angle_loop(target_angle - rt_angle);
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
// 设置相电压
|
||||
setPhaseVoltage(Uq, 0.0f);
|
||||
return shaft_angle;
|
||||
@@ -224,7 +225,7 @@ float FOC_Controlroller::angle_control_with_velocity_limit() {
|
||||
// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
|
||||
// 设置相电压
|
||||
setPhaseVoltage(Uq, 0.0f);
|
||||
@@ -243,12 +244,13 @@ float FOC_Controlroller::velocity_control() {
|
||||
float Uq = vel_loop((target_velocity - rt_velocity) * 25);
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
// 设置相电压
|
||||
setPhaseVoltage(Uq, 0.0f);
|
||||
// CAN0_transmiter.testfloat_union.f =
|
||||
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'));
|
||||
CAN0_transmiter.testfloat_union.f =
|
||||
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'));
|
||||
|
||||
return rt_velocity;
|
||||
}
|
||||
|
||||
@@ -259,7 +261,7 @@ float FOC_Controlroller::current_control() {
|
||||
float Uq = current_loop(target_current - Iq);
|
||||
CAN0_transmiter.testfloat_union.f = Iq;
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
// 设置相电压
|
||||
setPhaseVoltage(Uq, 0.0f);
|
||||
return 1;
|
||||
|
||||
+5
-348
@@ -45,15 +45,15 @@ class FOC_Controlroller {
|
||||
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
|
||||
|
||||
PIDController vel_loop =
|
||||
PIDController{0.028f, 0.2f, 1e-4, 1000000, voltage_power_supply}; // 12V
|
||||
PIDController{0.01f, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
|
||||
PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000,
|
||||
voltage_power_supply}; // 12V
|
||||
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.12f, 3.0f, 5e-4, 1000000, voltage_power_supply};
|
||||
PIDController ang_inVelAngloop =
|
||||
PIDController{30.0f, 0.0f, 1e-3, 1000000, 999};
|
||||
PIDController{100.0f, 0.0f, 5e-4, 1000000, 999};
|
||||
|
||||
void setPhaseVoltage(float Uq, float Ud);
|
||||
float cal_Iq_Id(float current_a, float current_b, float current_c);
|
||||
@@ -62,351 +62,8 @@ class FOC_Controlroller {
|
||||
// 定义函数指针类型
|
||||
typedef float (FOC_Controlroller::*FOCFunctionPtr)(void);
|
||||
|
||||
float _normalizeAngle(float angle);
|
||||
inline float _normalizeAngle(float angle);
|
||||
inline float _electricalAngle();
|
||||
|
||||
__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,
|
||||
0.073565f, 0.079682f, 0.085797f, 0.091909f, 0.098017f, 0.104122f,
|
||||
0.110222f, 0.116319f, 0.122411f, 0.128498f, 0.134581f, 0.140658f,
|
||||
0.146730f, 0.152797f, 0.158858f, 0.164913f, 0.170962f, 0.177004f,
|
||||
0.183040f, 0.189069f, 0.195090f, 0.201105f, 0.207111f, 0.213110f,
|
||||
0.219101f, 0.225084f, 0.231058f, 0.237024f, 0.242980f, 0.248928f,
|
||||
0.254866f, 0.260794f, 0.266713f, 0.272621f, 0.278520f, 0.284408f,
|
||||
0.290285f, 0.296151f, 0.302006f, 0.307850f, 0.313682f, 0.319502f,
|
||||
0.325310f, 0.331106f, 0.336890f, 0.342661f, 0.348419f, 0.354164f,
|
||||
0.359895f, 0.365613f, 0.371317f, 0.377007f, 0.382683f, 0.388345f,
|
||||
0.393992f, 0.399624f, 0.405241f, 0.410843f, 0.416430f, 0.422000f,
|
||||
0.427555f, 0.433094f, 0.438616f, 0.444122f, 0.449611f, 0.455084f,
|
||||
0.460539f, 0.465976f, 0.471397f, 0.476799f, 0.482184f, 0.487550f,
|
||||
0.492898f, 0.498228f, 0.503538f, 0.508830f, 0.514103f, 0.519356f,
|
||||
0.524590f, 0.529804f, 0.534998f, 0.540171f, 0.545325f, 0.550458f,
|
||||
0.555570f, 0.560662f, 0.565732f, 0.570781f, 0.575808f, 0.580814f,
|
||||
0.585798f, 0.590760f, 0.595699f, 0.600616f, 0.605511f, 0.610383f,
|
||||
0.615232f, 0.620057f, 0.624859f, 0.629638f, 0.634393f, 0.639124f,
|
||||
0.643832f, 0.648514f, 0.653173f, 0.657807f, 0.662416f, 0.667000f,
|
||||
0.671559f, 0.676093f, 0.680601f, 0.685084f, 0.689541f, 0.693971f,
|
||||
0.698376f, 0.702755f, 0.707107f, 0.711432f, 0.715731f, 0.720003f,
|
||||
0.724247f, 0.728464f, 0.732654f, 0.736817f, 0.740951f, 0.745058f,
|
||||
0.749136f, 0.753187f, 0.757209f, 0.761202f, 0.765167f, 0.769103f,
|
||||
0.773010f, 0.776888f, 0.780737f, 0.784557f, 0.788346f, 0.792107f,
|
||||
0.795837f, 0.799537f, 0.803208f, 0.806848f, 0.810457f, 0.814036f,
|
||||
0.817585f, 0.821103f, 0.824589f, 0.828045f, 0.831470f, 0.834863f,
|
||||
0.838225f, 0.841555f, 0.844854f, 0.848120f, 0.851355f, 0.854558f,
|
||||
0.857729f, 0.860867f, 0.863973f, 0.867046f, 0.870087f, 0.873095f,
|
||||
0.876070f, 0.879012f, 0.881921f, 0.884797f, 0.887640f, 0.890449f,
|
||||
0.893224f, 0.895966f, 0.898674f, 0.901349f, 0.903989f, 0.906596f,
|
||||
0.909168f, 0.911706f, 0.914210f, 0.916679f, 0.919114f, 0.921514f,
|
||||
0.923880f, 0.926210f, 0.928506f, 0.930767f, 0.932993f, 0.935184f,
|
||||
0.937339f, 0.939459f, 0.941544f, 0.943593f, 0.945607f, 0.947586f,
|
||||
0.949528f, 0.951435f, 0.953306f, 0.955141f, 0.956940f, 0.958703f,
|
||||
0.960431f, 0.962121f, 0.963776f, 0.965394f, 0.966976f, 0.968522f,
|
||||
0.970031f, 0.971504f, 0.972940f, 0.974339f, 0.975702f, 0.977028f,
|
||||
0.978317f, 0.979570f, 0.980785f, 0.981964f, 0.983105f, 0.984210f,
|
||||
0.985278f, 0.986308f, 0.987301f, 0.988258f, 0.989177f, 0.990058f,
|
||||
0.990903f, 0.991710f, 0.992480f, 0.993212f, 0.993907f, 0.994565f,
|
||||
0.995185f, 0.995767f, 0.996313f, 0.996820f, 0.997290f, 0.997723f,
|
||||
0.998118f, 0.998476f, 0.998795f, 0.999078f, 0.999322f, 0.999529f,
|
||||
0.999699f, 0.999831f, 0.999925f, 0.999981f, 1.000000f, 0.999981f,
|
||||
0.999925f, 0.999831f, 0.999699f, 0.999529f, 0.999322f, 0.999078f,
|
||||
0.998795f, 0.998476f, 0.998118f, 0.997723f, 0.997290f, 0.996820f,
|
||||
0.996313f, 0.995767f, 0.995185f, 0.994565f, 0.993907f, 0.993212f,
|
||||
0.992480f, 0.991710f, 0.990903f, 0.990058f, 0.989177f, 0.988258f,
|
||||
0.987301f, 0.986308f, 0.985278f, 0.984210f, 0.983105f, 0.981964f,
|
||||
0.980785f, 0.979570f, 0.978317f, 0.977028f, 0.975702f, 0.974339f,
|
||||
0.972940f, 0.971504f, 0.970031f, 0.968522f, 0.966976f, 0.965394f,
|
||||
0.963776f, 0.962121f, 0.960431f, 0.958703f, 0.956940f, 0.955141f,
|
||||
0.953306f, 0.951435f, 0.949528f, 0.947586f, 0.945607f, 0.943593f,
|
||||
0.941544f, 0.939459f, 0.937339f, 0.935184f, 0.932993f, 0.930767f,
|
||||
0.928506f, 0.926210f, 0.923880f, 0.921514f, 0.919114f, 0.916679f,
|
||||
0.914210f, 0.911706f, 0.909168f, 0.906596f, 0.903989f, 0.901349f,
|
||||
0.898674f, 0.895966f, 0.893224f, 0.890449f, 0.887640f, 0.884797f,
|
||||
0.881921f, 0.879012f, 0.876070f, 0.873095f, 0.870087f, 0.867046f,
|
||||
0.863973f, 0.860867f, 0.857729f, 0.854558f, 0.851355f, 0.848120f,
|
||||
0.844854f, 0.841555f, 0.838225f, 0.834863f, 0.831470f, 0.828045f,
|
||||
0.824589f, 0.821103f, 0.817585f, 0.814036f, 0.810457f, 0.806848f,
|
||||
0.803208f, 0.799537f, 0.795837f, 0.792107f, 0.788346f, 0.784557f,
|
||||
0.780737f, 0.776888f, 0.773010f, 0.769103f, 0.765167f, 0.761202f,
|
||||
0.757209f, 0.753187f, 0.749136f, 0.745058f, 0.740951f, 0.736817f,
|
||||
0.732654f, 0.728464f, 0.724247f, 0.720003f, 0.715731f, 0.711432f,
|
||||
0.707107f, 0.702755f, 0.698376f, 0.693971f, 0.689541f, 0.685084f,
|
||||
0.680601f, 0.676093f, 0.671559f, 0.667000f, 0.662416f, 0.657807f,
|
||||
0.653173f, 0.648514f, 0.643832f, 0.639124f, 0.634393f, 0.629638f,
|
||||
0.624859f, 0.620057f, 0.615232f, 0.610383f, 0.605511f, 0.600616f,
|
||||
0.595699f, 0.590760f, 0.585798f, 0.580814f, 0.575808f, 0.570781f,
|
||||
0.565732f, 0.560662f, 0.555570f, 0.550458f, 0.545325f, 0.540171f,
|
||||
0.534998f, 0.529804f, 0.524590f, 0.519356f, 0.514103f, 0.508830f,
|
||||
0.503538f, 0.498228f, 0.492898f, 0.487550f, 0.482184f, 0.476799f,
|
||||
0.471397f, 0.465976f, 0.460539f, 0.455084f, 0.449611f, 0.444122f,
|
||||
0.438616f, 0.433094f, 0.427555f, 0.422000f, 0.416430f, 0.410843f,
|
||||
0.405241f, 0.399624f, 0.393992f, 0.388345f, 0.382683f, 0.377007f,
|
||||
0.371317f, 0.365613f, 0.359895f, 0.354164f, 0.348419f, 0.342661f,
|
||||
0.336890f, 0.331106f, 0.325310f, 0.319502f, 0.313682f, 0.307850f,
|
||||
0.302006f, 0.296151f, 0.290285f, 0.284408f, 0.278520f, 0.272621f,
|
||||
0.266713f, 0.260794f, 0.254866f, 0.248928f, 0.242980f, 0.237024f,
|
||||
0.231058f, 0.225084f, 0.219101f, 0.213110f, 0.207111f, 0.201105f,
|
||||
0.195090f, 0.189069f, 0.183040f, 0.177004f, 0.170962f, 0.164913f,
|
||||
0.158858f, 0.152797f, 0.146730f, 0.140658f, 0.134581f, 0.128498f,
|
||||
0.122411f, 0.116319f, 0.110222f, 0.104122f, 0.098017f, 0.091909f,
|
||||
0.085797f, 0.079682f, 0.073565f, 0.067444f, 0.061321f, 0.055195f,
|
||||
0.049068f, 0.042938f, 0.036807f, 0.030675f, 0.024541f, 0.018407f,
|
||||
0.012272f, 0.006136f, 0.000000f, -0.006136f, -0.012272f, -0.018407f,
|
||||
-0.024541f, -0.030675f, -0.036807f, -0.042938f, -0.049068f, -0.055195f,
|
||||
-0.061321f, -0.067444f, -0.073565f, -0.079682f, -0.085797f, -0.091909f,
|
||||
-0.098017f, -0.104122f, -0.110222f, -0.116319f, -0.122411f, -0.128498f,
|
||||
-0.134581f, -0.140658f, -0.146730f, -0.152797f, -0.158858f, -0.164913f,
|
||||
-0.170962f, -0.177004f, -0.183040f, -0.189069f, -0.195090f, -0.201105f,
|
||||
-0.207111f, -0.213110f, -0.219101f, -0.225084f, -0.231058f, -0.237024f,
|
||||
-0.242980f, -0.248928f, -0.254866f, -0.260794f, -0.266713f, -0.272621f,
|
||||
-0.278520f, -0.284408f, -0.290285f, -0.296151f, -0.302006f, -0.307850f,
|
||||
-0.313682f, -0.319502f, -0.325310f, -0.331106f, -0.336890f, -0.342661f,
|
||||
-0.348419f, -0.354164f, -0.359895f, -0.365613f, -0.371317f, -0.377007f,
|
||||
-0.382683f, -0.388345f, -0.393992f, -0.399624f, -0.405241f, -0.410843f,
|
||||
-0.416430f, -0.422000f, -0.427555f, -0.433094f, -0.438616f, -0.444122f,
|
||||
-0.449611f, -0.455084f, -0.460539f, -0.465976f, -0.471397f, -0.476799f,
|
||||
-0.482184f, -0.487550f, -0.492898f, -0.498228f, -0.503538f, -0.508830f,
|
||||
-0.514103f, -0.519356f, -0.524590f, -0.529804f, -0.534998f, -0.540171f,
|
||||
-0.545325f, -0.550458f, -0.555570f, -0.560662f, -0.565732f, -0.570781f,
|
||||
-0.575808f, -0.580814f, -0.585798f, -0.590760f, -0.595699f, -0.600616f,
|
||||
-0.605511f, -0.610383f, -0.615232f, -0.620057f, -0.624859f, -0.629638f,
|
||||
-0.634393f, -0.639124f, -0.643832f, -0.648514f, -0.653173f, -0.657807f,
|
||||
-0.662416f, -0.667000f, -0.671559f, -0.676093f, -0.680601f, -0.685084f,
|
||||
-0.689541f, -0.693971f, -0.698376f, -0.702755f, -0.707107f, -0.711432f,
|
||||
-0.715731f, -0.720003f, -0.724247f, -0.728464f, -0.732654f, -0.736817f,
|
||||
-0.740951f, -0.745058f, -0.749136f, -0.753187f, -0.757209f, -0.761202f,
|
||||
-0.765167f, -0.769103f, -0.773010f, -0.776888f, -0.780737f, -0.784557f,
|
||||
-0.788346f, -0.792107f, -0.795837f, -0.799537f, -0.803208f, -0.806848f,
|
||||
-0.810457f, -0.814036f, -0.817585f, -0.821103f, -0.824589f, -0.828045f,
|
||||
-0.831470f, -0.834863f, -0.838225f, -0.841555f, -0.844854f, -0.848120f,
|
||||
-0.851355f, -0.854558f, -0.857729f, -0.860867f, -0.863973f, -0.867046f,
|
||||
-0.870087f, -0.873095f, -0.876070f, -0.879012f, -0.881921f, -0.884797f,
|
||||
-0.887640f, -0.890449f, -0.893224f, -0.895966f, -0.898674f, -0.901349f,
|
||||
-0.903989f, -0.906596f, -0.909168f, -0.911706f, -0.914210f, -0.916679f,
|
||||
-0.919114f, -0.921514f, -0.923880f, -0.926210f, -0.928506f, -0.930767f,
|
||||
-0.932993f, -0.935184f, -0.937339f, -0.939459f, -0.941544f, -0.943593f,
|
||||
-0.945607f, -0.947586f, -0.949528f, -0.951435f, -0.953306f, -0.955141f,
|
||||
-0.956940f, -0.958703f, -0.960431f, -0.962121f, -0.963776f, -0.965394f,
|
||||
-0.966976f, -0.968522f, -0.970031f, -0.971504f, -0.972940f, -0.974339f,
|
||||
-0.975702f, -0.977028f, -0.978317f, -0.979570f, -0.980785f, -0.981964f,
|
||||
-0.983105f, -0.984210f, -0.985278f, -0.986308f, -0.987301f, -0.988258f,
|
||||
-0.989177f, -0.990058f, -0.990903f, -0.991710f, -0.992480f, -0.993212f,
|
||||
-0.993907f, -0.994565f, -0.995185f, -0.995767f, -0.996313f, -0.996820f,
|
||||
-0.997290f, -0.997723f, -0.998118f, -0.998476f, -0.998795f, -0.999078f,
|
||||
-0.999322f, -0.999529f, -0.999699f, -0.999831f, -0.999925f, -0.999981f,
|
||||
-1.000000f, -0.999981f, -0.999925f, -0.999831f, -0.999699f, -0.999529f,
|
||||
-0.999322f, -0.999078f, -0.998795f, -0.998476f, -0.998118f, -0.997723f,
|
||||
-0.997290f, -0.996820f, -0.996313f, -0.995767f, -0.995185f, -0.994565f,
|
||||
-0.993907f, -0.993212f, -0.992480f, -0.991710f, -0.990903f, -0.990058f,
|
||||
-0.989177f, -0.988258f, -0.987301f, -0.986308f, -0.985278f, -0.984210f,
|
||||
-0.983105f, -0.981964f, -0.980785f, -0.979570f, -0.978317f, -0.977028f,
|
||||
-0.975702f, -0.974339f, -0.972940f, -0.971504f, -0.970031f, -0.968522f,
|
||||
-0.966976f, -0.965394f, -0.963776f, -0.962121f, -0.960431f, -0.958703f,
|
||||
-0.956940f, -0.955141f, -0.953306f, -0.951435f, -0.949528f, -0.947586f,
|
||||
-0.945607f, -0.943593f, -0.941544f, -0.939459f, -0.937339f, -0.935184f,
|
||||
-0.932993f, -0.930767f, -0.928506f, -0.926210f, -0.923880f, -0.921514f,
|
||||
-0.919114f, -0.916679f, -0.914210f, -0.911706f, -0.909168f, -0.906596f,
|
||||
-0.903989f, -0.901349f, -0.898674f, -0.895966f, -0.893224f, -0.890449f,
|
||||
-0.887640f, -0.884797f, -0.881921f, -0.879012f, -0.876070f, -0.873095f,
|
||||
-0.870087f, -0.867046f, -0.863973f, -0.860867f, -0.857729f, -0.854558f,
|
||||
-0.851355f, -0.848120f, -0.844854f, -0.841555f, -0.838225f, -0.834863f,
|
||||
-0.831470f, -0.828045f, -0.824589f, -0.821103f, -0.817585f, -0.814036f,
|
||||
-0.810457f, -0.806848f, -0.803208f, -0.799537f, -0.795837f, -0.792107f,
|
||||
-0.788346f, -0.784557f, -0.780737f, -0.776888f, -0.773010f, -0.769103f,
|
||||
-0.765167f, -0.761202f, -0.757209f, -0.753187f, -0.749136f, -0.745058f,
|
||||
-0.740951f, -0.736817f, -0.732654f, -0.728464f, -0.724247f, -0.720003f,
|
||||
-0.715731f, -0.711432f, -0.707107f, -0.702755f, -0.698376f, -0.693971f,
|
||||
-0.689541f, -0.685084f, -0.680601f, -0.676093f, -0.671559f, -0.667000f,
|
||||
-0.662416f, -0.657807f, -0.653173f, -0.648514f, -0.643832f, -0.639124f,
|
||||
-0.634393f, -0.629638f, -0.624859f, -0.620057f, -0.615232f, -0.610383f,
|
||||
-0.605511f, -0.600616f, -0.595699f, -0.590760f, -0.585798f, -0.580814f,
|
||||
-0.575808f, -0.570781f, -0.565732f, -0.560662f, -0.555570f, -0.550458f,
|
||||
-0.545325f, -0.540171f, -0.534998f, -0.529804f, -0.524590f, -0.519356f,
|
||||
-0.514103f, -0.508830f, -0.503538f, -0.498228f, -0.492898f, -0.487550f,
|
||||
-0.482184f, -0.476799f, -0.471397f, -0.465976f, -0.460539f, -0.455084f,
|
||||
-0.449611f, -0.444122f, -0.438616f, -0.433094f, -0.427555f, -0.422000f,
|
||||
-0.416430f, -0.410843f, -0.405241f, -0.399624f, -0.393992f, -0.388345f,
|
||||
-0.382683f, -0.377007f, -0.371317f, -0.365613f, -0.359895f, -0.354164f,
|
||||
-0.348419f, -0.342661f, -0.336890f, -0.331106f, -0.325310f, -0.319502f,
|
||||
-0.313682f, -0.307850f, -0.302006f, -0.296151f, -0.290285f, -0.284408f,
|
||||
-0.278520f, -0.272621f, -0.266713f, -0.260794f, -0.254866f, -0.248928f,
|
||||
-0.242980f, -0.237024f, -0.231058f, -0.225084f, -0.219101f, -0.213110f,
|
||||
-0.207111f, -0.201105f, -0.195090f, -0.189069f, -0.183040f, -0.177004f,
|
||||
-0.170962f, -0.164913f, -0.158858f, -0.152797f, -0.146730f, -0.140658f,
|
||||
-0.134581f, -0.128498f, -0.122411f, -0.116319f, -0.110222f, -0.104122f,
|
||||
-0.098017f, -0.091909f, -0.085797f, -0.079682f, -0.073565f, -0.067444f,
|
||||
-0.061321f, -0.055195f, -0.049068f, -0.042938f, -0.036807f, -0.030675f,
|
||||
-0.024541f, -0.018407f, -0.012272f, -0.006136f};
|
||||
__attribute__((aligned(4))) const float cos_lut[LUT_SIZE] = {
|
||||
1.000000f, 0.999981f, 0.999925f, 0.999831f, 0.999699f, 0.999529f,
|
||||
0.999322f, 0.999078f, 0.998795f, 0.998476f, 0.998118f, 0.997723f,
|
||||
0.997290f, 0.996820f, 0.996313f, 0.995767f, 0.995185f, 0.994565f,
|
||||
0.993907f, 0.993212f, 0.992480f, 0.991710f, 0.990903f, 0.990058f,
|
||||
0.989177f, 0.988258f, 0.987301f, 0.986308f, 0.985278f, 0.984210f,
|
||||
0.983105f, 0.981964f, 0.980785f, 0.979570f, 0.978317f, 0.977028f,
|
||||
0.975702f, 0.974339f, 0.972940f, 0.971504f, 0.970031f, 0.968522f,
|
||||
0.966976f, 0.965394f, 0.963776f, 0.962121f, 0.960431f, 0.958703f,
|
||||
0.956940f, 0.955141f, 0.953306f, 0.951435f, 0.949528f, 0.947586f,
|
||||
0.945607f, 0.943593f, 0.941544f, 0.939459f, 0.937339f, 0.935184f,
|
||||
0.932993f, 0.930767f, 0.928506f, 0.926210f, 0.923880f, 0.921514f,
|
||||
0.919114f, 0.916679f, 0.914210f, 0.911706f, 0.909168f, 0.906596f,
|
||||
0.903989f, 0.901349f, 0.898674f, 0.895966f, 0.893224f, 0.890449f,
|
||||
0.887640f, 0.884797f, 0.881921f, 0.879012f, 0.876070f, 0.873095f,
|
||||
0.870087f, 0.867046f, 0.863973f, 0.860867f, 0.857729f, 0.854558f,
|
||||
0.851355f, 0.848120f, 0.844854f, 0.841555f, 0.838225f, 0.834863f,
|
||||
0.831470f, 0.828045f, 0.824589f, 0.821103f, 0.817585f, 0.814036f,
|
||||
0.810457f, 0.806848f, 0.803208f, 0.799537f, 0.795837f, 0.792107f,
|
||||
0.788346f, 0.784557f, 0.780737f, 0.776888f, 0.773010f, 0.769103f,
|
||||
0.765167f, 0.761202f, 0.757209f, 0.753187f, 0.749136f, 0.745058f,
|
||||
0.740951f, 0.736817f, 0.732654f, 0.728464f, 0.724247f, 0.720003f,
|
||||
0.715731f, 0.711432f, 0.707107f, 0.702755f, 0.698376f, 0.693971f,
|
||||
0.689541f, 0.685084f, 0.680601f, 0.676093f, 0.671559f, 0.667000f,
|
||||
0.662416f, 0.657807f, 0.653173f, 0.648514f, 0.643832f, 0.639124f,
|
||||
0.634393f, 0.629638f, 0.624859f, 0.620057f, 0.615232f, 0.610383f,
|
||||
0.605511f, 0.600616f, 0.595699f, 0.590760f, 0.585798f, 0.580814f,
|
||||
0.575808f, 0.570781f, 0.565732f, 0.560662f, 0.555570f, 0.550458f,
|
||||
0.545325f, 0.540171f, 0.534998f, 0.529804f, 0.524590f, 0.519356f,
|
||||
0.514103f, 0.508830f, 0.503538f, 0.498228f, 0.492898f, 0.487550f,
|
||||
0.482184f, 0.476799f, 0.471397f, 0.465976f, 0.460539f, 0.455084f,
|
||||
0.449611f, 0.444122f, 0.438616f, 0.433094f, 0.427555f, 0.422000f,
|
||||
0.416430f, 0.410843f, 0.405241f, 0.399624f, 0.393992f, 0.388345f,
|
||||
0.382683f, 0.377007f, 0.371317f, 0.365613f, 0.359895f, 0.354164f,
|
||||
0.348419f, 0.342661f, 0.336890f, 0.331106f, 0.325310f, 0.319502f,
|
||||
0.313682f, 0.307850f, 0.302006f, 0.296151f, 0.290285f, 0.284408f,
|
||||
0.278520f, 0.272621f, 0.266713f, 0.260794f, 0.254866f, 0.248928f,
|
||||
0.242980f, 0.237024f, 0.231058f, 0.225084f, 0.219101f, 0.213110f,
|
||||
0.207111f, 0.201105f, 0.195090f, 0.189069f, 0.183040f, 0.177004f,
|
||||
0.170962f, 0.164913f, 0.158858f, 0.152797f, 0.146730f, 0.140658f,
|
||||
0.134581f, 0.128498f, 0.122411f, 0.116319f, 0.110222f, 0.104122f,
|
||||
0.098017f, 0.091909f, 0.085797f, 0.079682f, 0.073565f, 0.067444f,
|
||||
0.061321f, 0.055195f, 0.049068f, 0.042938f, 0.036807f, 0.030675f,
|
||||
0.024541f, 0.018407f, 0.012272f, 0.006136f, 0.000000f, -0.006136f,
|
||||
-0.012272f, -0.018407f, -0.024541f, -0.030675f, -0.036807f, -0.042938f,
|
||||
-0.049068f, -0.055195f, -0.061321f, -0.067444f, -0.073565f, -0.079682f,
|
||||
-0.085797f, -0.091909f, -0.098017f, -0.104122f, -0.110222f, -0.116319f,
|
||||
-0.122411f, -0.128498f, -0.134581f, -0.140658f, -0.146730f, -0.152797f,
|
||||
-0.158858f, -0.164913f, -0.170962f, -0.177004f, -0.183040f, -0.189069f,
|
||||
-0.195090f, -0.201105f, -0.207111f, -0.213110f, -0.219101f, -0.225084f,
|
||||
-0.231058f, -0.237024f, -0.242980f, -0.248928f, -0.254866f, -0.260794f,
|
||||
-0.266713f, -0.272621f, -0.278520f, -0.284408f, -0.290285f, -0.296151f,
|
||||
-0.302006f, -0.307850f, -0.313682f, -0.319502f, -0.325310f, -0.331106f,
|
||||
-0.336890f, -0.342661f, -0.348419f, -0.354164f, -0.359895f, -0.365613f,
|
||||
-0.371317f, -0.377007f, -0.382683f, -0.388345f, -0.393992f, -0.399624f,
|
||||
-0.405241f, -0.410843f, -0.416430f, -0.422000f, -0.427555f, -0.433094f,
|
||||
-0.438616f, -0.444122f, -0.449611f, -0.455084f, -0.460539f, -0.465976f,
|
||||
-0.471397f, -0.476799f, -0.482184f, -0.487550f, -0.492898f, -0.498228f,
|
||||
-0.503538f, -0.508830f, -0.514103f, -0.519356f, -0.524590f, -0.529804f,
|
||||
-0.534998f, -0.540171f, -0.545325f, -0.550458f, -0.555570f, -0.560662f,
|
||||
-0.565732f, -0.570781f, -0.575808f, -0.580814f, -0.585798f, -0.590760f,
|
||||
-0.595699f, -0.600616f, -0.605511f, -0.610383f, -0.615232f, -0.620057f,
|
||||
-0.624859f, -0.629638f, -0.634393f, -0.639124f, -0.643832f, -0.648514f,
|
||||
-0.653173f, -0.657807f, -0.662416f, -0.667000f, -0.671559f, -0.676093f,
|
||||
-0.680601f, -0.685084f, -0.689541f, -0.693971f, -0.698376f, -0.702755f,
|
||||
-0.707107f, -0.711432f, -0.715731f, -0.720003f, -0.724247f, -0.728464f,
|
||||
-0.732654f, -0.736817f, -0.740951f, -0.745058f, -0.749136f, -0.753187f,
|
||||
-0.757209f, -0.761202f, -0.765167f, -0.769103f, -0.773010f, -0.776888f,
|
||||
-0.780737f, -0.784557f, -0.788346f, -0.792107f, -0.795837f, -0.799537f,
|
||||
-0.803208f, -0.806848f, -0.810457f, -0.814036f, -0.817585f, -0.821103f,
|
||||
-0.824589f, -0.828045f, -0.831470f, -0.834863f, -0.838225f, -0.841555f,
|
||||
-0.844854f, -0.848120f, -0.851355f, -0.854558f, -0.857729f, -0.860867f,
|
||||
-0.863973f, -0.867046f, -0.870087f, -0.873095f, -0.876070f, -0.879012f,
|
||||
-0.881921f, -0.884797f, -0.887640f, -0.890449f, -0.893224f, -0.895966f,
|
||||
-0.898674f, -0.901349f, -0.903989f, -0.906596f, -0.909168f, -0.911706f,
|
||||
-0.914210f, -0.916679f, -0.919114f, -0.921514f, -0.923880f, -0.926210f,
|
||||
-0.928506f, -0.930767f, -0.932993f, -0.935184f, -0.937339f, -0.939459f,
|
||||
-0.941544f, -0.943593f, -0.945607f, -0.947586f, -0.949528f, -0.951435f,
|
||||
-0.953306f, -0.955141f, -0.956940f, -0.958703f, -0.960431f, -0.962121f,
|
||||
-0.963776f, -0.965394f, -0.966976f, -0.968522f, -0.970031f, -0.971504f,
|
||||
-0.972940f, -0.974339f, -0.975702f, -0.977028f, -0.978317f, -0.979570f,
|
||||
-0.980785f, -0.981964f, -0.983105f, -0.984210f, -0.985278f, -0.986308f,
|
||||
-0.987301f, -0.988258f, -0.989177f, -0.990058f, -0.990903f, -0.991710f,
|
||||
-0.992480f, -0.993212f, -0.993907f, -0.994565f, -0.995185f, -0.995767f,
|
||||
-0.996313f, -0.996820f, -0.997290f, -0.997723f, -0.998118f, -0.998476f,
|
||||
-0.998795f, -0.999078f, -0.999322f, -0.999529f, -0.999699f, -0.999831f,
|
||||
-0.999925f, -0.999981f, -1.000000f, -0.999981f, -0.999925f, -0.999831f,
|
||||
-0.999699f, -0.999529f, -0.999322f, -0.999078f, -0.998795f, -0.998476f,
|
||||
-0.998118f, -0.997723f, -0.997290f, -0.996820f, -0.996313f, -0.995767f,
|
||||
-0.995185f, -0.994565f, -0.993907f, -0.993212f, -0.992480f, -0.991710f,
|
||||
-0.990903f, -0.990058f, -0.989177f, -0.988258f, -0.987301f, -0.986308f,
|
||||
-0.985278f, -0.984210f, -0.983105f, -0.981964f, -0.980785f, -0.979570f,
|
||||
-0.978317f, -0.977028f, -0.975702f, -0.974339f, -0.972940f, -0.971504f,
|
||||
-0.970031f, -0.968522f, -0.966976f, -0.965394f, -0.963776f, -0.962121f,
|
||||
-0.960431f, -0.958703f, -0.956940f, -0.955141f, -0.953306f, -0.951435f,
|
||||
-0.949528f, -0.947586f, -0.945607f, -0.943593f, -0.941544f, -0.939459f,
|
||||
-0.937339f, -0.935184f, -0.932993f, -0.930767f, -0.928506f, -0.926210f,
|
||||
-0.923880f, -0.921514f, -0.919114f, -0.916679f, -0.914210f, -0.911706f,
|
||||
-0.909168f, -0.906596f, -0.903989f, -0.901349f, -0.898674f, -0.895966f,
|
||||
-0.893224f, -0.890449f, -0.887640f, -0.884797f, -0.881921f, -0.879012f,
|
||||
-0.876070f, -0.873095f, -0.870087f, -0.867046f, -0.863973f, -0.860867f,
|
||||
-0.857729f, -0.854558f, -0.851355f, -0.848120f, -0.844854f, -0.841555f,
|
||||
-0.838225f, -0.834863f, -0.831470f, -0.828045f, -0.824589f, -0.821103f,
|
||||
-0.817585f, -0.814036f, -0.810457f, -0.806848f, -0.803208f, -0.799537f,
|
||||
-0.795837f, -0.792107f, -0.788346f, -0.784557f, -0.780737f, -0.776888f,
|
||||
-0.773010f, -0.769103f, -0.765167f, -0.761202f, -0.757209f, -0.753187f,
|
||||
-0.749136f, -0.745058f, -0.740951f, -0.736817f, -0.732654f, -0.728464f,
|
||||
-0.724247f, -0.720003f, -0.715731f, -0.711432f, -0.707107f, -0.702755f,
|
||||
-0.698376f, -0.693971f, -0.689541f, -0.685084f, -0.680601f, -0.676093f,
|
||||
-0.671559f, -0.667000f, -0.662416f, -0.657807f, -0.653173f, -0.648514f,
|
||||
-0.643832f, -0.639124f, -0.634393f, -0.629638f, -0.624859f, -0.620057f,
|
||||
-0.615232f, -0.610383f, -0.605511f, -0.600616f, -0.595699f, -0.590760f,
|
||||
-0.585798f, -0.580814f, -0.575808f, -0.570781f, -0.565732f, -0.560662f,
|
||||
-0.555570f, -0.550458f, -0.545325f, -0.540171f, -0.534998f, -0.529804f,
|
||||
-0.524590f, -0.519356f, -0.514103f, -0.508830f, -0.503538f, -0.498228f,
|
||||
-0.492898f, -0.487550f, -0.482184f, -0.476799f, -0.471397f, -0.465976f,
|
||||
-0.460539f, -0.455084f, -0.449611f, -0.444122f, -0.438616f, -0.433094f,
|
||||
-0.427555f, -0.422000f, -0.416430f, -0.410843f, -0.405241f, -0.399624f,
|
||||
-0.393992f, -0.388345f, -0.382683f, -0.377007f, -0.371317f, -0.365613f,
|
||||
-0.359895f, -0.354164f, -0.348419f, -0.342661f, -0.336890f, -0.331106f,
|
||||
-0.325310f, -0.319502f, -0.313682f, -0.307850f, -0.302006f, -0.296151f,
|
||||
-0.290285f, -0.284408f, -0.278520f, -0.272621f, -0.266713f, -0.260794f,
|
||||
-0.254866f, -0.248928f, -0.242980f, -0.237024f, -0.231058f, -0.225084f,
|
||||
-0.219101f, -0.213110f, -0.207111f, -0.201105f, -0.195090f, -0.189069f,
|
||||
-0.183040f, -0.177004f, -0.170962f, -0.164913f, -0.158858f, -0.152797f,
|
||||
-0.146730f, -0.140658f, -0.134581f, -0.128498f, -0.122411f, -0.116319f,
|
||||
-0.110222f, -0.104122f, -0.098017f, -0.091909f, -0.085797f, -0.079682f,
|
||||
-0.073565f, -0.067444f, -0.061321f, -0.055195f, -0.049068f, -0.042938f,
|
||||
-0.036807f, -0.030675f, -0.024541f, -0.018407f, -0.012272f, -0.006136f,
|
||||
-0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f,
|
||||
0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f,
|
||||
0.073565f, 0.079682f, 0.085797f, 0.091909f, 0.098017f, 0.104122f,
|
||||
0.110222f, 0.116319f, 0.122411f, 0.128498f, 0.134581f, 0.140658f,
|
||||
0.146730f, 0.152797f, 0.158858f, 0.164913f, 0.170962f, 0.177004f,
|
||||
0.183040f, 0.189069f, 0.195090f, 0.201105f, 0.207111f, 0.213110f,
|
||||
0.219101f, 0.225084f, 0.231058f, 0.237024f, 0.242980f, 0.248928f,
|
||||
0.254866f, 0.260794f, 0.266713f, 0.272621f, 0.278520f, 0.284408f,
|
||||
0.290285f, 0.296151f, 0.302006f, 0.307850f, 0.313682f, 0.319502f,
|
||||
0.325310f, 0.331106f, 0.336890f, 0.342661f, 0.348419f, 0.354164f,
|
||||
0.359895f, 0.365613f, 0.371317f, 0.377007f, 0.382683f, 0.388345f,
|
||||
0.393992f, 0.399624f, 0.405241f, 0.410843f, 0.416430f, 0.422000f,
|
||||
0.427555f, 0.433094f, 0.438616f, 0.444122f, 0.449611f, 0.455084f,
|
||||
0.460539f, 0.465976f, 0.471397f, 0.476799f, 0.482184f, 0.487550f,
|
||||
0.492898f, 0.498228f, 0.503538f, 0.508830f, 0.514103f, 0.519356f,
|
||||
0.524590f, 0.529804f, 0.534998f, 0.540171f, 0.545325f, 0.550458f,
|
||||
0.555570f, 0.560662f, 0.565732f, 0.570781f, 0.575808f, 0.580814f,
|
||||
0.585798f, 0.590760f, 0.595699f, 0.600616f, 0.605511f, 0.610383f,
|
||||
0.615232f, 0.620057f, 0.624859f, 0.629638f, 0.634393f, 0.639124f,
|
||||
0.643832f, 0.648514f, 0.653173f, 0.657807f, 0.662416f, 0.667000f,
|
||||
0.671559f, 0.676093f, 0.680601f, 0.685084f, 0.689541f, 0.693971f,
|
||||
0.698376f, 0.702755f, 0.707107f, 0.711432f, 0.715731f, 0.720003f,
|
||||
0.724247f, 0.728464f, 0.732654f, 0.736817f, 0.740951f, 0.745058f,
|
||||
0.749136f, 0.753187f, 0.757209f, 0.761202f, 0.765167f, 0.769103f,
|
||||
0.773010f, 0.776888f, 0.780737f, 0.784557f, 0.788346f, 0.792107f,
|
||||
0.795837f, 0.799537f, 0.803208f, 0.806848f, 0.810457f, 0.814036f,
|
||||
0.817585f, 0.821103f, 0.824589f, 0.828045f, 0.831470f, 0.834863f,
|
||||
0.838225f, 0.841555f, 0.844854f, 0.848120f, 0.851355f, 0.854558f,
|
||||
0.857729f, 0.860867f, 0.863973f, 0.867046f, 0.870087f, 0.873095f,
|
||||
0.876070f, 0.879012f, 0.881921f, 0.884797f, 0.887640f, 0.890449f,
|
||||
0.893224f, 0.895966f, 0.898674f, 0.901349f, 0.903989f, 0.906596f,
|
||||
0.909168f, 0.911706f, 0.914210f, 0.916679f, 0.919114f, 0.921514f,
|
||||
0.923880f, 0.926210f, 0.928506f, 0.930767f, 0.932993f, 0.935184f,
|
||||
0.937339f, 0.939459f, 0.941544f, 0.943593f, 0.945607f, 0.947586f,
|
||||
0.949528f, 0.951435f, 0.953306f, 0.955141f, 0.956940f, 0.958703f,
|
||||
0.960431f, 0.962121f, 0.963776f, 0.965394f, 0.966976f, 0.968522f,
|
||||
0.970031f, 0.971504f, 0.972940f, 0.974339f, 0.975702f, 0.977028f,
|
||||
0.978317f, 0.979570f, 0.980785f, 0.981964f, 0.983105f, 0.984210f,
|
||||
0.985278f, 0.986308f, 0.987301f, 0.988258f, 0.989177f, 0.990058f,
|
||||
0.990903f, 0.991710f, 0.992480f, 0.993212f, 0.993907f, 0.994565f,
|
||||
0.995185f, 0.995767f, 0.996313f, 0.996820f, 0.997290f, 0.997723f,
|
||||
0.998118f, 0.998476f, 0.998795f, 0.999078f, 0.999322f, 0.999529f,
|
||||
0.999699f, 0.999831f, 0.999925f, 0.999981f};
|
||||
|
||||
#endif
|
||||
@@ -1,17 +1,17 @@
|
||||
#include "Filter.h"
|
||||
#include "Can.h"
|
||||
#include "ang_sensor.h"
|
||||
#include "config.h"
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
#include <stdint.h>
|
||||
#include "config.h"
|
||||
|
||||
extern angle_sensor KTH7823;
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
extern u32 system_runtime_us;
|
||||
extern u32 motor_runtime_50us;
|
||||
EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
|
||||
EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
|
||||
|
||||
|
||||
/*!
|
||||
\brief 中值滤波器函数
|
||||
\param[in] values 待滤波的数据数组
|
||||
@@ -126,10 +126,10 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
|
||||
\retval 当前角度估计值 (degree)
|
||||
*/
|
||||
float EncoderAngleKalman::getVelocity() const {
|
||||
static u32 last_us100_counter = system_runtime_us;
|
||||
float vel = (kalman.getState() - last_angle) * 1e4 *
|
||||
(system_runtime_us - last_us100_counter);
|
||||
last_us100_counter = system_runtime_us;
|
||||
static u32 last_us50_counter = motor_runtime_50us;
|
||||
float vel = (kalman.getState() - last_angle) * TIMER2_FREQUENCY *
|
||||
(motor_runtime_50us - last_us50_counter);
|
||||
last_us50_counter = motor_runtime_50us;
|
||||
return vel;
|
||||
}
|
||||
|
||||
|
||||
@@ -59,6 +59,7 @@ project:
|
||||
- component: Device:GD32C10x_StdPeripherals:BKP
|
||||
- component: Device:GD32C10x_StdPeripherals:CRC
|
||||
- component: Device:GD32C10x_StdPeripherals:RTC
|
||||
- component: CMSIS:DSP
|
||||
output:
|
||||
type:
|
||||
- elf
|
||||
@@ -76,6 +77,7 @@ project:
|
||||
- -D__MICROLIB
|
||||
- -Wno-register
|
||||
- -O3 # 添加优化级别
|
||||
- -DARM_MATH_CM4 # Cortex-M4核心定义
|
||||
C:
|
||||
- -D__packed=__attribute__((packed))
|
||||
ASM:
|
||||
@@ -86,3 +88,4 @@ project:
|
||||
- --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
|
||||
# 移除了 __TARGET_FPU_VFP，因为它不是链接器参数
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
#include "Can.h"
|
||||
#include "FOC.h"
|
||||
#include "Filter.h"
|
||||
#include "arm_math.h"
|
||||
#include "config.h"
|
||||
#include "math.h"
|
||||
#include "systick.h"
|
||||
|
||||
@@ -444,6 +444,7 @@ u8 CAN_transmiter::init() {
|
||||
void CAN_transmiter::update(void) {
|
||||
target_angle_union.f = FOC.target_angle;
|
||||
target_velocity_union.f = FOC.target_velocity;
|
||||
target_current_union.f = FOC.target_current;
|
||||
limit_velocity_union.f = FOC.limit_velocity;
|
||||
limit_voltage = FOC.limit_voltage * 1000;
|
||||
mcu_temp = sampler.get_mcu_temp() * 100;
|
||||
@@ -591,6 +592,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;
|
||||
FOC.target_current = FOC.target_velocity * 10.0f;
|
||||
} 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];
|
||||
|
||||
@@ -87,9 +87,10 @@ class CAN_transmiter {
|
||||
u8 Target_Control_Mode = 0; // 目标控制模式
|
||||
u8 EEPROMStatus = 0; // EEPROM状态码
|
||||
float_union_t target_velocity_union = {0}; // 目标速度
|
||||
float_union_t u_current_union = {0}; // U相电流
|
||||
float_union_t w_current_union = {0}; // W相电流
|
||||
float_union_t v_current_union = {0}; // V相电流
|
||||
float_union_t target_current_union = {0};
|
||||
float_union_t u_current_union = {0}; // U相电流
|
||||
float_union_t w_current_union = {0}; // W相电流
|
||||
float_union_t v_current_union = {0}; // V相电流
|
||||
|
||||
can_trasnmit_message_struct transmit_message; // 发送消息结构体
|
||||
u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态
|
||||
|
||||
+56
-48
@@ -12,7 +12,6 @@
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
|
||||
extern u16 adc_value[7];
|
||||
extern FOC_Controlroller FOC;
|
||||
@@ -96,66 +95,75 @@ void TIMER2_IRQHandler(void) {
|
||||
if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
|
||||
// 清除更新中断标志
|
||||
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
|
||||
// 每100us执行一次的操作
|
||||
scheduler.taskFlags |= (1 << TASK_ANGLE_UPDATE) |
|
||||
(1 << TASK_VELOCITY_UPDATE) |
|
||||
(1 << TASK_MOTOR_CONTROL);
|
||||
|
||||
switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
|
||||
case 0: {
|
||||
scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
|
||||
break;
|
||||
}
|
||||
case 1: {
|
||||
scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
|
||||
break;
|
||||
}
|
||||
case 2: {
|
||||
scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
|
||||
break;
|
||||
}
|
||||
case 3: {
|
||||
break;
|
||||
}
|
||||
case 9: {
|
||||
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
|
||||
break;
|
||||
}
|
||||
}
|
||||
// 每50us执行一次的操作 - 电机控制
|
||||
scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
|
||||
scheduler.taskFlags |=
|
||||
(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
|
||||
|
||||
total_us100_counter++;
|
||||
if (total_us100_counter > 9) {
|
||||
total_us100_counter = 0;
|
||||
// 可以添加每1ms执行的代码，无负载均衡
|
||||
static u8 ms1_counter = 0;
|
||||
// 使用标志位控制，每100us执行一次其他任务
|
||||
static bool isEvenCall = false;
|
||||
isEvenCall = !isEvenCall;
|
||||
|
||||
switch (ms1_counter) { // 每10ms执行的代码，负载均衡
|
||||
if (isEvenCall) {
|
||||
// 每100us执行一次的操作 - 保持原有频率
|
||||
|
||||
switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
|
||||
case 0: {
|
||||
scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
|
||||
scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
|
||||
break;
|
||||
}
|
||||
case 1: {
|
||||
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
|
||||
scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
|
||||
break;
|
||||
}
|
||||
case 2: {
|
||||
scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
|
||||
break;
|
||||
}
|
||||
case 3: {
|
||||
break;
|
||||
}
|
||||
case 9: {
|
||||
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
|
||||
break;
|
||||
}
|
||||
}
|
||||
ms1_counter++;
|
||||
if (ms1_counter > 9) {
|
||||
ms1_counter = 0;
|
||||
// 可以添加每10ms执行的代码，无负载均衡
|
||||
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执行的代码
|
||||
|
||||
total_us100_counter++;
|
||||
if (total_us100_counter > 9) {
|
||||
total_us100_counter = 0;
|
||||
// 可以添加每1ms执行的代码，无负载均衡
|
||||
static u8 ms1_counter = 0;
|
||||
|
||||
switch (ms1_counter) { // 每10ms执行的代码，负载均衡
|
||||
case 0: {
|
||||
scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
|
||||
break;
|
||||
}
|
||||
case 1: {
|
||||
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
|
||||
break;
|
||||
}
|
||||
case 2: {
|
||||
break;
|
||||
}
|
||||
}
|
||||
ms1_counter++;
|
||||
if (ms1_counter > 9) {
|
||||
ms1_counter = 0;
|
||||
// 可以添加每10ms执行的代码，无负载均衡
|
||||
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执行的代码
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -253,7 +253,7 @@ float angle_sensor::update(void) {
|
||||
float delta = current_angle_deg - m_lastAngle;
|
||||
|
||||
// 处理角度绕回情况（0度和360度之间的跳变）
|
||||
// 如果变化超过180度，说明发生了绕回
|
||||
// 如果变化超过300度，说明发生了绕回
|
||||
if (delta > ANGLE_WRAP_THRESHOLD) {
|
||||
// 从接近360度绕回到接近0度
|
||||
delta -= ANGLE_WRAP_VALUE;
|
||||
|
||||
+9
-9
@@ -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();
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 */
|
||||
@@ -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");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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(){
|
||||
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user