|
@@ -5,7 +5,7 @@
|
|
|
#include "foc/core/PMSM_FOC_Core.h"
|
|
#include "foc/core/PMSM_FOC_Core.h"
|
|
|
#include "libs/utils.h"
|
|
#include "libs/utils.h"
|
|
|
#include "libs/logger.h"
|
|
#include "libs/logger.h"
|
|
|
-#include "math/fix_math.h"
|
|
|
|
|
|
|
+#include "math/fast_math.h"
|
|
|
static current_samp_t g_cs;
|
|
static current_samp_t g_cs;
|
|
|
|
|
|
|
|
#define NB_OFFSET_SAMPLES 32
|
|
#define NB_OFFSET_SAMPLES 32
|
|
@@ -43,6 +43,7 @@ void phase_current_start_cali(void){
|
|
|
phase_current_init();
|
|
phase_current_init();
|
|
|
g_cs.is_calibrating_offset = true;
|
|
g_cs.is_calibrating_offset = true;
|
|
|
g_cs.c_phases = PHASE_AB;
|
|
g_cs.c_phases = PHASE_AB;
|
|
|
|
|
+ g_cs.c_ignore_phase = IGNORE_NONE;
|
|
|
adc_current_sample_config(g_cs.c_phases);
|
|
adc_current_sample_config(g_cs.c_phases);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
@@ -94,6 +95,7 @@ bool phase_current_offset(void) {
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
+#if 0
|
|
|
void phase_current_get(float *iABC){
|
|
void phase_current_get(float *iABC){
|
|
|
current_samp_t *cs = &g_cs;
|
|
current_samp_t *cs = &g_cs;
|
|
|
s32 phase_current1, phase_current2;
|
|
s32 phase_current1, phase_current2;
|
|
@@ -124,51 +126,6 @@ void phase_current_get(float *iABC){
|
|
|
iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
|
|
iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-#if 0
|
|
|
|
|
-void phase_current_point(void *p){
|
|
|
|
|
- FOC_OutP *out = p;
|
|
|
|
|
- current_samp_t *cs = &g_cs;
|
|
|
|
|
- u32 low_side_low_duty = FOC_PWM_Half_Period - out->n_lowDuty;
|
|
|
|
|
- u32 low_side_mid_duty = FOC_PWM_Half_Period - out->n_midDuty;
|
|
|
|
|
- cs->sector = out->n_Sector;
|
|
|
|
|
- out->n_Sample1 = FOC_PWM_Half_Period + 1;
|
|
|
|
|
- out->n_Sample2 = FOC_PWM_Half_Period + 1;
|
|
|
|
|
- s16 potest = 7;
|
|
|
|
|
- /*底边开mos的时间是2倍的 low_side_low_duty(一个周期)*/
|
|
|
|
|
- if (low_side_low_duty * 2 >= TSampleMIN) { //可以采样
|
|
|
|
|
- if (low_side_low_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
|
|
|
|
|
- out->n_Sample1 = FOC_PWM_Half_Period - 1;
|
|
|
|
|
- cs->sector = SECTOR_1;
|
|
|
|
|
- potest = 1;
|
|
|
|
|
- }else {
|
|
|
|
|
- u32 Samp_p = out->n_lowDuty + TSampleBefore;
|
|
|
|
|
- if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样,需要配置PWM0模式
|
|
|
|
|
- out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
|
|
|
|
|
- potest = 2;
|
|
|
|
|
- }else {
|
|
|
|
|
- out->n_Sample1 = Samp_p;
|
|
|
|
|
- potest = 3;
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- }else if (low_side_mid_duty * 2 >= TSampleMIN){
|
|
|
|
|
- if (low_side_mid_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
|
|
|
|
|
- out->n_Sample1 = FOC_PWM_Half_Period - 1;
|
|
|
|
|
- potest = 4;
|
|
|
|
|
- }else {
|
|
|
|
|
- u32 Samp_p = out->n_midDuty + TSampleBefore;
|
|
|
|
|
- if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样,需要配置PWM0模式
|
|
|
|
|
- out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
|
|
|
|
|
- potest = 5;
|
|
|
|
|
- }else {
|
|
|
|
|
- out->n_Sample1 = Samp_p;
|
|
|
|
|
- potest = 6;
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- plot_1data16(potest);
|
|
|
|
|
- out->n_Sector = cs->sector;
|
|
|
|
|
-}
|
|
|
|
|
-#else
|
|
|
|
|
static __inline__ s16 get_mid_duty(u32 max_hi_side, FOC_OutP *out) {
|
|
static __inline__ s16 get_mid_duty(u32 max_hi_side, FOC_OutP *out) {
|
|
|
u32 min_low_side = FOC_PWM_Half_Period - max_hi_side;
|
|
u32 min_low_side = FOC_PWM_Half_Period - max_hi_side;
|
|
|
if (min_low_side >= MAX(TSampleBefore, TADC)) { //可以采样
|
|
if (min_low_side >= MAX(TSampleBefore, TADC)) { //可以采样
|
|
@@ -219,6 +176,210 @@ void phase_current_point(void *p){
|
|
|
}
|
|
}
|
|
|
out->n_CPhases = cs->c_phases;
|
|
out->n_CPhases = cs->c_phases;
|
|
|
}
|
|
}
|
|
|
|
|
+#else
|
|
|
|
|
+#define LOW_FP_COEF 1.0f
|
|
|
|
|
+void phase_current_get(float *iABC){
|
|
|
|
|
+ current_samp_t *cs = &g_cs;
|
|
|
|
|
+ s32 phase_current1, phase_current2;
|
|
|
|
|
+
|
|
|
|
|
+ adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);
|
|
|
|
|
+
|
|
|
|
|
+ if (cs->c_phases == PHASE_AB) {
|
|
|
|
|
+ /* Current on Phase C is not accessible */
|
|
|
|
|
+ /* Ia = PhaseAOffset - ADC converted value) */
|
|
|
|
|
+ cs->adc_ia = (phase_current1 - cs->adc_offset_a);
|
|
|
|
|
+ cs->adc_ib = (phase_current2 - cs->adc_offset_b);
|
|
|
|
|
+
|
|
|
|
|
+ if (cs->c_ignore_phase == IGNORE_NONE) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
|
|
|
|
|
+ LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
|
|
|
|
|
+ LowPass_Filter(cs->adc_ic_filter, -(cs->adc_ia + cs->adc_ib), LOW_FP_COEF);
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_A) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
|
|
|
|
|
+ cs->adc_ia = cs->adc_ia_filter;
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_B) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
|
|
|
|
|
+ cs->adc_ib = cs->adc_ib_filter;
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_ALL) {
|
|
|
|
|
+ cs->adc_ia = cs->adc_ia_filter;
|
|
|
|
|
+ cs->adc_ib = cs->adc_ib_filter;
|
|
|
|
|
+ }
|
|
|
|
|
+ cs->adc_ic = -(cs->adc_ia + cs->adc_ib);
|
|
|
|
|
+ }else if (cs->c_phases == PHASE_BC) {
|
|
|
|
|
+ /* Current on Phase A is not accessible */
|
|
|
|
|
+ /* Ib = PhaseBOffset - ADC converted value) */
|
|
|
|
|
+ cs->adc_ib = (phase_current1 - cs->adc_offset_b);
|
|
|
|
|
+ cs->adc_ic = (phase_current2 - cs->adc_offset_c);
|
|
|
|
|
+
|
|
|
|
|
+ if (cs->c_ignore_phase == IGNORE_NONE) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
|
|
|
|
|
+ LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
|
|
|
|
|
+ LowPass_Filter(cs->adc_ia_filter, -(cs->adc_ib + cs->adc_ic), LOW_FP_COEF);
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_B) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
|
|
|
|
|
+ cs->adc_ib = cs->adc_ib_filter;
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_C) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
|
|
|
|
|
+ cs->adc_ic = cs->adc_ic_filter;
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_ALL) {
|
|
|
|
|
+ cs->adc_ib = cs->adc_ib_filter;
|
|
|
|
|
+ cs->adc_ic = cs->adc_ic_filter;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ cs->adc_ia = -(cs->adc_ib + cs->adc_ic);
|
|
|
|
|
+ }else if (cs->c_phases == PHASE_AC) {
|
|
|
|
|
+ /* Current on Phase B is not accessible */
|
|
|
|
|
+ /* Ia = PhaseAOffset - ADC converted value) */
|
|
|
|
|
+ cs->adc_ia = (phase_current1 - cs->adc_offset_a);
|
|
|
|
|
+ cs->adc_ic = (phase_current2 - cs->adc_offset_c);
|
|
|
|
|
+ if (cs->c_ignore_phase == IGNORE_NONE) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
|
|
|
|
|
+ LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
|
|
|
|
|
+ LowPass_Filter(cs->adc_ib_filter, -(cs->adc_ia + cs->adc_ic), LOW_FP_COEF);
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_A) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
|
|
|
|
|
+ cs->adc_ia = cs->adc_ia_filter;
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_C) {
|
|
|
|
|
+ LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
|
|
|
|
|
+ cs->adc_ic = cs->adc_ic_filter;
|
|
|
|
|
+ }else if (cs->c_ignore_phase == IGNORE_ALL) {
|
|
|
|
|
+ cs->adc_ia = cs->adc_ia_filter;
|
|
|
|
|
+ cs->adc_ic = cs->adc_ic_filter;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
|
|
|
|
|
+ }
|
|
|
|
|
+ iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;
|
|
|
|
|
+ iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof;
|
|
|
|
|
+ iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void phase_current_point(void *p){
|
|
|
|
|
+ FOC_OutP *out = p;
|
|
|
|
|
+ current_samp_t *cs = &g_cs;
|
|
|
|
|
+ u16 *duty = out->n_Duty;
|
|
|
|
|
+ s16 test_sample = 0;
|
|
|
|
|
+ u16 max_hi_side = MAX(duty[0], MAX(duty[1], duty[2]));
|
|
|
|
|
+ u16 min_low_side = FOC_PWM_Half_Period - max_hi_side;
|
|
|
|
|
+
|
|
|
|
|
+ out->n_Sample1 = FOC_PWM_Half_Period + 1;
|
|
|
|
|
+ out->n_Sample2 = FOC_PWM_Half_Period + 1;
|
|
|
|
|
+
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_NONE;
|
|
|
|
|
+
|
|
|
|
|
+ if (min_low_side > TSampleMIN) {
|
|
|
|
|
+ u16 Samp_p = max_hi_side + TSampleBefore;
|
|
|
|
|
+ if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样,需要配置PWM0模式
|
|
|
|
|
+ out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
|
|
|
|
|
+ test_sample = 1;
|
|
|
|
|
+ }else {
|
|
|
|
|
+ out->n_Sample1 = Samp_p;
|
|
|
|
|
+ test_sample = 2;
|
|
|
|
|
+ }
|
|
|
|
|
+ cs->c_phases = PHASE_AB;
|
|
|
|
|
+ }else {
|
|
|
|
|
+ u16 lowA = FOC_PWM_Half_Period - duty[0];
|
|
|
|
|
+ u16 lowB = FOC_PWM_Half_Period - duty[1];
|
|
|
|
|
+ u16 lowC = FOC_PWM_Half_Period - duty[2];
|
|
|
|
|
+ u16 t2, t3, t_pointer, m = 0;
|
|
|
|
|
+ if (max_hi_side == duty[0]) { //A 相底边开通时间最短
|
|
|
|
|
+ cs->c_phases = PHASE_BC;
|
|
|
|
|
+ if (lowB > lowC) {
|
|
|
|
|
+ t2 = (lowC - lowA)/2;
|
|
|
|
|
+ t3 = (lowB - lowC)/2;
|
|
|
|
|
+ t_pointer = duty[2]; //通过C计算
|
|
|
|
|
+ }else {
|
|
|
|
|
+ t2 = (lowB - lowA)/2;
|
|
|
|
|
+ t3 = (lowC - lowB)/2;
|
|
|
|
|
+ t_pointer = duty[1]; //通过B计算
|
|
|
|
|
+ m = 1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (t2 > TSampleMIN) {
|
|
|
|
|
+ out->n_Sample1 = t_pointer + TSampleBefore;
|
|
|
|
|
+ test_sample = 3;
|
|
|
|
|
+ }else if (t3 > TSampleMIN) {
|
|
|
|
|
+ if (m == 0) {
|
|
|
|
|
+ out->n_Sample1 = duty[1] + TSampleBefore;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_C;
|
|
|
|
|
+ test_sample = 4;
|
|
|
|
|
+ }else {
|
|
|
|
|
+ out->n_Sample1 = duty[2] + TSampleBefore;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_B;
|
|
|
|
|
+ test_sample = 5;
|
|
|
|
|
+ }
|
|
|
|
|
+ }else {
|
|
|
|
|
+ out->n_Sample1 = FOC_PWM_Half_Period - 1;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_ALL;
|
|
|
|
|
+ test_sample = 6;
|
|
|
|
|
+ }
|
|
|
|
|
+ }else if (max_hi_side == duty[1]) { //B 相底边开通时间最短
|
|
|
|
|
+ cs->c_phases = PHASE_AC;
|
|
|
|
|
+ if (lowA > lowC) {
|
|
|
|
|
+ t2 = (lowC - lowB)/2;
|
|
|
|
|
+ t3 = (lowA - lowC)/2;
|
|
|
|
|
+ t_pointer = duty[2]; //通过C计算
|
|
|
|
|
+ }else {
|
|
|
|
|
+ t2 = (lowA - lowB)/2;
|
|
|
|
|
+ t3 = (lowC - lowA)/2;
|
|
|
|
|
+ t_pointer = duty[0]; //通过A计算
|
|
|
|
|
+ m = 1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (t2 > TSampleMIN) {
|
|
|
|
|
+ out->n_Sample1 = t_pointer + TSampleBefore;
|
|
|
|
|
+ test_sample = 7;
|
|
|
|
|
+ }else if (t3 > TSampleMIN) {
|
|
|
|
|
+ if (m == 0) {
|
|
|
|
|
+ out->n_Sample1 = duty[0] + TSampleBefore;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_C;
|
|
|
|
|
+ test_sample = 8;
|
|
|
|
|
+ }else {
|
|
|
|
|
+ out->n_Sample1 = duty[2] + TSampleBefore;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_A;
|
|
|
|
|
+ test_sample = 9;
|
|
|
|
|
+ }
|
|
|
|
|
+ }else {
|
|
|
|
|
+ out->n_Sample1 = FOC_PWM_Half_Period - 1;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_ALL;
|
|
|
|
|
+ test_sample = 10;
|
|
|
|
|
+ }
|
|
|
|
|
+ }else { //C 相底边开通时间最短
|
|
|
|
|
+ cs->c_phases = PHASE_AB;
|
|
|
|
|
+ if (lowA > lowB) {
|
|
|
|
|
+ t2 = (lowB - lowC)/2;
|
|
|
|
|
+ t3 = (lowA - lowB)/2;
|
|
|
|
|
+ t_pointer = duty[1]; //通过B计算
|
|
|
|
|
+ }else {
|
|
|
|
|
+ t2 = (lowA - lowC)/2;
|
|
|
|
|
+ t3 = (lowB - lowA)/2;
|
|
|
|
|
+ t_pointer = duty[0]; //通过A计算
|
|
|
|
|
+ m = 1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (t2 > TSampleMIN) {
|
|
|
|
|
+ out->n_Sample1 = t_pointer + TSampleBefore;
|
|
|
|
|
+ test_sample = 11;
|
|
|
|
|
+ }else if (t3 > TSampleMIN) {
|
|
|
|
|
+ if (m == 0) {
|
|
|
|
|
+ out->n_Sample1 = duty[0] + TSampleBefore;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_B;
|
|
|
|
|
+ test_sample = 12;
|
|
|
|
|
+ }else {
|
|
|
|
|
+ out->n_Sample1 = duty[1] + TSampleBefore;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_A;
|
|
|
|
|
+ test_sample = 13;
|
|
|
|
|
+ }
|
|
|
|
|
+ }else {
|
|
|
|
|
+ out->n_Sample1 = FOC_PWM_Half_Period - 1;
|
|
|
|
|
+ cs->c_ignore_phase = IGNORE_ALL;
|
|
|
|
|
+ test_sample = 14;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+ out->n_CPhases = cs->c_phases;
|
|
|
|
|
+ out->test_sample = test_sample;
|
|
|
|
|
+}
|
|
|
|
|
|
|
|
#endif
|
|
#endif
|
|
|
|
|
|