|
|
@@ -1,25 +1,25 @@
|
|
|
#include <string.h>
|
|
|
#include "libs/task.h"
|
|
|
-#include "hal/pwm.h"
|
|
|
-#include "hal/hal.h"
|
|
|
+#include "bsp/bsp.h"
|
|
|
#include "foc/foc.h"
|
|
|
#include "foc/park_clark.h"
|
|
|
#include "foc/svpwm.h"
|
|
|
#include "foc/foc_task.h"
|
|
|
#include "foc/phase_current.h"
|
|
|
+#include "foc/hall_speed.h"
|
|
|
|
|
|
static void charge_cap_timer_handler(timer_t *t);
|
|
|
static u32 foc_main_task_handler(void);
|
|
|
|
|
|
static timer_t charge_cap_timer = {.handler = charge_cap_timer_handler};
|
|
|
static motor_foc_t m_foc;
|
|
|
-u16 _hall_table[] = {0xFFFF, 292, 47, 1, 180, 227, 113, 0xFFFF};
|
|
|
|
|
|
void foc_init(void) {
|
|
|
/* init pwm hardware timer */
|
|
|
m_foc.state = IDLE;
|
|
|
m_foc.gate_output = false;
|
|
|
- memcpy(m_foc.hall_table, _hall_table, sizeof(_hall_table));
|
|
|
+
|
|
|
+ hall_sensor_init();
|
|
|
|
|
|
PWM_TimerInit();
|
|
|
|
|
|
@@ -28,21 +28,16 @@ void foc_init(void) {
|
|
|
foc_hall_detect(6.0f, (u16 *)m_foc.hall_table);
|
|
|
}
|
|
|
|
|
|
-void foc_motor_spin(dq_t *dq_v, float angle) {
|
|
|
- alpha_beta_t alphabeta;
|
|
|
- phase_time_t phase_time;
|
|
|
- u8 sector = 0xFF;
|
|
|
- Rev_Park(dq_v, degree_2_pi(angle), &alphabeta);
|
|
|
- svpwm(&alphabeta, MAX_VBUS, FOC_PWM_period/2, &phase_time, §or);
|
|
|
- PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, FOC_PWM_period/2);
|
|
|
-}
|
|
|
-
|
|
|
int foc_hall_detect(float current, u16 *hall_table){
|
|
|
- dq_t dq_v = {.d = 0.0f, .q = 0.0f};
|
|
|
foc_start_pwm(true);
|
|
|
+ m_foc.override_p.is_override_theta = true;
|
|
|
+ m_foc.override_p.theta = 0.0f;
|
|
|
+ m_foc.override_p.is_override_v_dq = true;
|
|
|
+ m_foc.override_p.v_dq.d = .0f;
|
|
|
+ m_foc.override_p.v_dq.q = .0f;
|
|
|
+
|
|
|
for (int i = 0;i < 1000;i++) {
|
|
|
- dq_v.d = (float)i * current / 1000.0f;
|
|
|
- foc_motor_spin(&dq_v, 0);
|
|
|
+ m_foc.override_p.v_dq.d = (float)i * current / 1000.0f;
|
|
|
task_udelay(1000);
|
|
|
}
|
|
|
float sin_hall[8];
|
|
|
@@ -55,10 +50,10 @@ int foc_hall_detect(float current, u16 *hall_table){
|
|
|
// Forwards
|
|
|
for (int i = 0;i < 3;i++) {
|
|
|
for (int j = 0;j < 360;j++) {
|
|
|
- foc_motor_spin(&dq_v, j);
|
|
|
+ m_foc.override_p.theta = j;
|
|
|
task_udelay(10 * 1000);
|
|
|
|
|
|
- int hall = HALL_Read(7);
|
|
|
+ int hall = get_hall_stat(7);
|
|
|
float s, c;
|
|
|
normal_sincosf(degree_2_pi(j), &s, &c);
|
|
|
sin_hall[hall] += s;
|
|
|
@@ -70,10 +65,10 @@ int foc_hall_detect(float current, u16 *hall_table){
|
|
|
// Reverse
|
|
|
for (int i = 0;i < 3;i++) {
|
|
|
for (int j = 360;j >= 0;j--) {
|
|
|
- foc_motor_spin(&dq_v, j);
|
|
|
+ m_foc.override_p.theta = j;
|
|
|
task_udelay(10 * 1000);
|
|
|
|
|
|
- int hall = HALL_Read(7);
|
|
|
+ int hall = get_hall_stat(7);
|
|
|
float s, c;
|
|
|
normal_sincosf(degree_2_pi(j), &s, &c);
|
|
|
sin_hall[hall] += s;
|
|
|
@@ -81,6 +76,9 @@ int foc_hall_detect(float current, u16 *hall_table){
|
|
|
hall_iterations[hall]++;
|
|
|
}
|
|
|
}
|
|
|
+ foc_start_pwm(false);
|
|
|
+ m_foc.override_p.is_override_theta = false;
|
|
|
+ m_foc.override_p.is_override_v_dq = false;
|
|
|
int fails = 0;
|
|
|
for(int i = 0;i < 8;i++) {
|
|
|
if (hall_iterations[i] > 30) {
|
|
|
@@ -92,7 +90,6 @@ int foc_hall_detect(float current, u16 *hall_table){
|
|
|
fails++;
|
|
|
}
|
|
|
}
|
|
|
- foc_start_pwm(false);
|
|
|
return fails == 2;
|
|
|
}
|
|
|
|
|
|
@@ -129,9 +126,6 @@ void foc_pwm_up_handler(void){
|
|
|
phase_current_adc_triger(&m_foc.current_samp);
|
|
|
}
|
|
|
|
|
|
-void hall_detect_handler(void) {
|
|
|
-
|
|
|
-}
|
|
|
|
|
|
void current_sample_handler(void) {
|
|
|
foc_task(&m_foc);
|