|
|
@@ -0,0 +1,170 @@
|
|
|
+#include <math.h>
|
|
|
+#include "bsp/adc.h"
|
|
|
+#include "bsp/pwm.h"
|
|
|
+#include "foc/motor/current.h"
|
|
|
+#include "foc/core/PMSM_FOC_Core.h"
|
|
|
+#include "foc/mc_error.h"
|
|
|
+#include "libs/utils.h"
|
|
|
+#include "libs/logger.h"
|
|
|
+#include "math/fast_math.h"
|
|
|
+current_samp_t g_cs;
|
|
|
+
|
|
|
+#define NB_OFFSET_SAMPLES 32
|
|
|
+#define SENSOR_SAMPLES 10000
|
|
|
+
|
|
|
+void phase_current_init(void) {
|
|
|
+ current_samp_t *cs = &g_cs;
|
|
|
+ cs->sample_count = NB_OFFSET_SAMPLES + 1;
|
|
|
+ cs->adc_ia = 0;
|
|
|
+ cs->adc_ib = 0;
|
|
|
+ cs->adc_ic = 0;
|
|
|
+}
|
|
|
+
|
|
|
+void phase_current_offset_calibrate(void){
|
|
|
+ g_cs.adc_offset_a = 0;
|
|
|
+ g_cs.adc_offset_b = 0;
|
|
|
+ g_cs.adc_offset_c = 0;
|
|
|
+
|
|
|
+ phase_current_init();
|
|
|
+ g_cs.c_phases = PHASE_BC;
|
|
|
+ g_cs.c_ignore_phase = IGNORE_NONE;
|
|
|
+ adc_current_sample_config(g_cs.c_phases);
|
|
|
+ g_cs.is_calibrating_offset = true;
|
|
|
+}
|
|
|
+
|
|
|
+void phase_current_calibrate_wait(void) {
|
|
|
+ while(g_cs.is_calibrating_offset || g_cs.is_calibrating_sensor) {
|
|
|
+ wdog_reload();
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void phase_current_sensor_start_calibrate(float calibrate_current) {
|
|
|
+ bool calibrate = false;
|
|
|
+ if (calibrate_current > 0) {
|
|
|
+ calibrate = true;
|
|
|
+ g_cs.sensor_samples_1 = 0;
|
|
|
+ g_cs.sensor_samples_2 = 0;
|
|
|
+ g_cs.sample_count = SENSOR_SAMPLES + 1;
|
|
|
+ g_cs.calibrate_current = calibrate_current;
|
|
|
+ }
|
|
|
+
|
|
|
+ g_cs.is_calibrating_sensor = calibrate;
|
|
|
+}
|
|
|
+
|
|
|
+bool phase_current_sensor_do_calibrate(void) {
|
|
|
+ current_samp_t *cs = &g_cs;
|
|
|
+ if (!cs->is_calibrating_sensor) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ s32 phase_current1, phase_current2;
|
|
|
+ adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);
|
|
|
+ if(cs->c_phases == PHASE_BC) {
|
|
|
+ if (cs->sample_count > 0) {
|
|
|
+ cs->sample_count--;
|
|
|
+ if (cs->sample_count <= SENSOR_SAMPLES) {
|
|
|
+ cs->sensor_samples_1 += (phase_current1 - cs->adc_offset_b);
|
|
|
+ cs->sensor_samples_2 += (phase_current2 - cs->adc_offset_c);
|
|
|
+ }
|
|
|
+ }else {
|
|
|
+ cs->sensor_samples_1 = cs->sensor_samples_1 / (float)SENSOR_SAMPLES;
|
|
|
+ cs->sensor_samples_2 = cs->sensor_samples_2 / (float)SENSOR_SAMPLES;
|
|
|
+ cs->sensor_k1 = g_cs.calibrate_current/cs->sensor_samples_1;
|
|
|
+ cs->sensor_k2 = g_cs.calibrate_current/cs->sensor_samples_2;
|
|
|
+ cs->sensor_k1 = ABS(cs->sensor_k1);
|
|
|
+ cs->sensor_k2 = ABS(cs->sensor_k2);
|
|
|
+ cs->is_calibrating_sensor = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return cs->is_calibrating_sensor;
|
|
|
+}
|
|
|
+
|
|
|
+bool phase_current_offset(void) {
|
|
|
+ current_samp_t *cs = &g_cs;
|
|
|
+ if (!cs->is_calibrating_offset) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ s32 phase_current1 = 0 , phase_current2 = 0;
|
|
|
+ adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);
|
|
|
+ if (cs->sample_count == (NB_OFFSET_SAMPLES + 1)) {
|
|
|
+ cs->sample_count --;
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ if (cs->sample_count > 0) {
|
|
|
+ cs->sample_count--;
|
|
|
+ if (cs->c_phases == PHASE_AB && cs->sample_count >= 0) {
|
|
|
+ cs->adc_offset_a += phase_current1;
|
|
|
+ cs->adc_offset_b += phase_current2;
|
|
|
+ if (cs->sample_count == 0) {
|
|
|
+ cs->adc_offset_a = cs->adc_offset_a / NB_OFFSET_SAMPLES;
|
|
|
+ cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (cs->c_phases == PHASE_BC && cs->sample_count >= 0) {
|
|
|
+ cs->adc_offset_c += phase_current2;
|
|
|
+ cs->adc_offset_b += phase_current1;
|
|
|
+ if (cs->sample_count == 0) {
|
|
|
+ cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
|
|
|
+ cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }else {
|
|
|
+ if (cs->c_phases == PHASE_AB) {
|
|
|
+ cs->c_phases = PHASE_BC;
|
|
|
+ phase_current_init();
|
|
|
+ adc_current_sample_config(cs->c_phases);
|
|
|
+ }else {
|
|
|
+ cs->is_calibrating_offset = false;
|
|
|
+ sys_debug("offset %d, %d, %d\n", g_cs.adc_offset_a, g_cs.adc_offset_b, g_cs.adc_offset_c);
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+bool phase_curr_offset_check(void) {
|
|
|
+ if ((g_cs.adc_offset_b > ADC_FULL_MAX/2 + 100) || (g_cs.adc_offset_c > ADC_FULL_MAX/2 + 100)) {
|
|
|
+ err_add_record(FOC_CRIT_CURR_OFF_Err, MAX(g_cs.adc_offset_c, g_cs.adc_offset_b));
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ if ((g_cs.adc_offset_b < ADC_FULL_MAX/2 - 100) || (g_cs.adc_offset_c < ADC_FULL_MAX/2 - 100)) {
|
|
|
+ err_add_record(FOC_CRIT_CURR_OFF_Err, min(g_cs.adc_offset_c, g_cs.adc_offset_b));
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
+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);
|
|
|
+
|
|
|
+ cs->adc_ib = (phase_current1 - cs->adc_offset_b);
|
|
|
+ cs->adc_ic = (phase_current2 - cs->adc_offset_c);
|
|
|
+
|
|
|
+#ifdef CONFIG_PWM_UV_SWAP
|
|
|
+ iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof1;
|
|
|
+ iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof2;
|
|
|
+ iABC[0] = -(iABC[1] + iABC[2]);
|
|
|
+#else
|
|
|
+ iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof1;
|
|
|
+ iABC[0] = -cs->adc_ic * ADC_TO_CURR_ceof2;
|
|
|
+ iABC[2] = -(iABC[1] + iABC[0]);
|
|
|
+#endif
|
|
|
+}
|
|
|
+
|
|
|
+void phase_current_point(void *p){
|
|
|
+ FOC_OutP *out = p;
|
|
|
+ current_samp_t *cs = &g_cs;
|
|
|
+
|
|
|
+ cs->c_phases = PHASE_BC;
|
|
|
+ out->n_Sample1 = FOC_PWM_Half_Period - 1;
|
|
|
+ out->n_Sample2 = FOC_PWM_Half_Period + 1;
|
|
|
+ out->n_CPhases = cs->c_phases;
|
|
|
+}
|
|
|
+
|
|
|
+void phase_current_adc_triger(void){
|
|
|
+ adc_enable_ext_trigger();
|
|
|
+}
|
|
|
+
|
huhui