MC100/Applications/foc/motor/current_2_3_shut.c

#include <math.h>
#include "bsp/bsp_driver.h"
#include "foc/motor/current.h"
#include "foc/core/controller.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_AB;
	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();
	}
	sys_debug("offset %d, %d, %d\n", g_cs.adc_offset_a, g_cs.adc_offset_b, g_cs.adc_offset_c);
}

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;
		}

	}
	return true;
}

#define ADC_OFF_1 (ADC_FULL_MAX/2 + 100)
#define ADC_OFF_2 (ADC_FULL_MAX/2 - 100)
bool phase_curr_offset_check(void) {
	if ((g_cs.adc_offset_a > ADC_OFF_1) || (g_cs.adc_offset_b > ADC_OFF_1) || (g_cs.adc_offset_c > ADC_OFF_1)) {
		mc_crit_err_add_s16(FOC_CRIT_CURR_OFF_Err, MAX(g_cs.adc_offset_a, MAX(g_cs.adc_offset_c, g_cs.adc_offset_b)));
		return true;
	}
	if ((g_cs.adc_offset_a < ADC_OFF_2) || (g_cs.adc_offset_b < ADC_OFF_2) || (g_cs.adc_offset_c < ADC_OFF_2)) {
		mc_crit_err_add_s16(FOC_CRIT_CURR_OFF_Err, min(g_cs.adc_offset_a, 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);

	if (cs->c_phases == PHASE_AB) {
		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
		cs->adc_ib = (phase_current2 - cs->adc_offset_b);
		iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;
		iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof;
		iABC[2] = -(iABC[0] + iABC[1]);
	}else if (cs->c_phases == PHASE_BC) {
		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
		iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof;
		iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
		iABC[0] = -(iABC[1] + iABC[2]);
	}else { //PHASE_AC
		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
		iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;
		iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
		iABC[1] = -(iABC[0] + iABC[2]);
	}
}

void phase_current_point(void *p){
	foc_out_t *out = p;
	current_samp_t *cs = &g_cs;
#ifdef CONFIG_THREE_SHUNT
	if (out->low_duty == out->duty[0]) {
		if (out->mid_duty == out->duty[1]) {
			cs->c_phases = PHASE_AB;
		}else if (out->mid_duty == out->duty[2]) {
			cs->c_phases = PHASE_AC;
		}else {
			//keep unchanged
		}
	}else if (out->low_duty == out->duty[1]) {
		if (out->mid_duty == out->duty[0]) {
			cs->c_phases = PHASE_AB;
		}else if (out->mid_duty == out->duty[2]) {
			cs->c_phases = PHASE_BC;
		}else {
			//keep unchanged
		}
	}else { // out->low_duty == out->duty[2]
		if (out->mid_duty == out->duty[0]) {
			cs->c_phases = PHASE_AC;
		}else if (out->mid_duty == out->duty[1]) {
			cs->c_phases = PHASE_BC;
		}else {
			//keep unchanged
		}
	}
#else // two shunt
	cs->c_phases = PHASE_AB;
#endif
	out->sample1 = FOC_PWM_Half_Period - 1;
	out->sample2 = FOC_PWM_Half_Period + 1;
	out->sample_phase = cs->c_phases;
}

void phase_current_adc_triger(void){
	adc_enable_ext_trigger();
}