MC100/Applications/foc/mc_config.c

#include "foc/mc_config.h"
#include "libs/utils.h"
#include "libs/crc16.h"
#include "app/nv_storage.h"

mc_config conf;

void mc_conf_default(void);

static u8 *conf_buff = NULL;
static int conf_len = 0;
static int conf_idx = 0;

bool mc_conf_begin_recv(int len) {
	if (conf_buff) {
		os_free(conf_buff);
		conf_buff = NULL;
	}
	if (len > sizeof(mc_config)) {
		return false;
	}
	conf_len = len;
	conf_idx = 0;
	conf_buff = os_alloc(len);
	return conf_buff != NULL;
}

bool mc_conf_recv_data(u8 *buff, int offset, int len) {
	if (offset != conf_idx || conf_buff == NULL) {
		return false;
	}
	if (offset + len > conf_len) {
		return false;
	}
	memcpy(conf_buff + offset, buff, len);
	conf_idx = offset + len;
	return true;
}

bool mc_conf_finish_recv(u16 crc) {
	if (crc != crc16_get(conf_buff, conf_len)) {
		return false;
	}
	mc_conf_decode_configs();
	
	conf.crc = crc16_get((u8 *)&conf, (u8 *)&conf.crc - (u8 *)&conf);

	mc_conf_save();

	if (conf_buff) {
		os_free(conf_buff);
		conf_buff = NULL;
	}
	return true;
}
static u16 crc16;
bool mc_conf_begin_send(void) {
	if (conf_buff) {
		return true;
	}
	
	conf_idx = 0;
	conf_buff = os_alloc(sizeof(mc_config));
	if (conf_buff) {
		conf_len = mc_conf_encode_configs(conf_buff);
		crc16 = crc16_get(conf_buff, conf_len);
	}
	return conf_buff != NULL;
}


int mc_conf_send_data(u8 *buff, int offset, int len) {
	if (offset == 0 && !mc_conf_begin_send()) {
		return -2;
	}
	if (conf_len == conf_idx) {
		return 0;
	}
	if (offset != conf_idx || conf_buff == NULL) {
		return -1;
	}
	len = min(len, conf_len - conf_idx);
	memcpy(buff, conf_buff + offset, len);
	conf_idx = (offset + len);
	return len;
}

int mc_conf_finish_send(u8 *buff) {
	if (conf_buff) {
		os_free(conf_buff);
		conf_buff = NULL;
	}
	encode_u16(buff, crc16);
	return sizeof(u16);
}

void mc_conf_save(void) {
	nv_write_config_block(0, (u8 *)&conf, sizeof(conf));
	nv_write_config_block(1, (u8 *)&conf, sizeof(conf));
}

void mc_conf_load(void) {
	mc_config temp;
	nv_read_config_block(0, (u8 *)&temp, sizeof(temp));
	u16 crc16 = crc16_get((u8 *)&temp, (u8 *)&temp.crc - (u8 *)&temp);
	bool idx0_success = crc16 == temp.crc;
	if (idx0_success) {
		memcpy(&conf, &temp, sizeof(temp));
	}
	nv_read_config_block(1, (u8 *)&temp, sizeof(temp));
	crc16 = crc16_get((u8 *)&temp, (u8 *)&temp.crc - (u8 *)&temp);
	if (crc16 == temp.crc && !idx0_success) {
		memcpy(&conf, &temp, sizeof(temp));
		nv_write_config_block(0, (u8 *)&temp, sizeof(temp));
	}else if (crc16 != temp.crc) {
		if (idx0_success) {
			nv_write_config_block(1, (u8 *)&conf, sizeof(conf));
		}else {
			mc_conf_default();
			nv_write_config_block(0, (u8 *)&conf, sizeof(conf));
			nv_write_config_block(1, (u8 *)&conf, sizeof(conf));
		}
	}
}

#define Gear_torque(l, t) conf.g_n[l].torque[t] = CONFIG_Gear##l##_Torque##t
#define Gear_Config_default(l) \
	do { \
		conf.g_n[l].max_speed = CONFIG_Gear##l##_MaxSpeed;\
		conf.g_n[l].max_idc = CONFIG_Gear##l##_MaxIdc;\
		conf.g_n[l].max_trq = CONFIG_Gear##l##_MaxTorque;\
		conf.g_n[l].zero_accl = CONFIG_Gear##l##_ZeroAccl;\
		conf.g_n[l].accl_time = CONFIG_Gear##l##_NormalAccl;\
		Gear_torque(l,0);Gear_torque(l,1);Gear_torque(l,2);Gear_torque(l,3);Gear_torque(l,4); \
		Gear_torque(l,5);Gear_torque(l,6);Gear_torque(l,7);Gear_torque(l,8);Gear_torque(l,9); \
	}while(0);

#define GearLow_torque(l, t) conf.g_n[l].torque[t] = CONFIG_GearLow##l##_Torque##t
#define GearLow_Config_default(l) \
		do { \
			conf.g_l[l].max_speed = CONFIG_GearLow##l##_MaxSpeed;\
			conf.g_l[l].max_idc = CONFIG_GearLow##l##_MaxIdc;\
			conf.g_l[l].max_trq = CONFIG_GearLow##l##_MaxTorque;\
			conf.g_l[l].zero_accl = CONFIG_GearLow##l##_ZeroAccl;\
			conf.g_l[l].accl_time = CONFIG_GearLow##l##_NormalAccl;\
			GearLow_torque(l,0);GearLow_torque(l,1);GearLow_torque(l,2);GearLow_torque(l,3);GearLow_torque(l,4); \
			GearLow_torque(l,5);GearLow_torque(l,6);GearLow_torque(l,7);GearLow_torque(l,8);GearLow_torque(l,9); \
		}while(0);


#define Limter_Config_default(item, type, n) \
	do { \
		conf.item[n].enter_pointer = CONFIG_Protect_##type##_Level##n##_Entry;\
		conf.item[n].exit_pointer = CONFIG_Protect_##type##_Level##n##_Exit;\
		conf.item[n].limit_value = CONFIG_Protect_##type##_Level##n##_Value;\
	}while(0);

#define EnReco_Config_default(n) \
	do {\
		conf.e_r[n].torque = CONFIG_EnergyRecovery_Level##n##_Torque;\
		conf.e_r[n].time = CONFIG_EnergyRecovery_Level##n##_Time;\
	}while(0);

void mc_conf_default(void) {
	conf.version = 0;
	conf.m.poles = CONFIG_Motor_Poles;
	conf.m.ld = CONFIG_Motor_Ld;
	conf.m.lq = CONFIG_Motor_Lq;
	conf.m.rs = CONFIG_Motor_Rs;
	conf.m.nor_pll_band = CONFIG_Motor_PLLBand;
	conf.m.epm_pll_band = CONFIG_Motor_EpmPLL;
	conf.m.pos_pll_band = CONFIG_Motor_PosPLL;
	conf.m.velocity_w = CONFIG_Motor_VelocityW;
	conf.m.wheel_c = CONFIG_Motor_VelocityC;
	conf.m.gear_ratio = CONFIG_Motor_GearRatio;
	conf.m.max_fw_id = CONFIG_Motor_MaxFwDCurr;

	conf.c.max_dc_vol = CONFIG_Foc_MaxDCVol;
	conf.c.min_dc_vol = CONFIG_Foc_MinDCVol;
	conf.c.max_phase_curr = CONFIG_Foc_MaxPhaseCurr;
	conf.c.max_torque = CONFIG_Foc_MaxTorque;
	conf.c.max_rpm = CONFIG_Foc_MaxRPM;
	conf.c.max_epm_rpm = CONFIG_Foc_MaxEPMRPM;
	conf.c.max_epm_torque = CONFIG_Foc_MaxEPMTorque;
	conf.c.max_epm_back_rpm = CONFIG_Foc_MaxEPMRPMBk;
	conf.c.max_epm_back_torque = CONFIG_Foc_MaxEPMTorqueBk;
	conf.c.max_ebrk_torque = CONFIG_Foc_MaxEbrkTorque;
	conf.c.max_idc = CONFIG_Foc_MaxIDC;
	conf.c.dq_loop_bandwith = CONFIG_Foc_CurrCtrlBandWith;
	conf.c.thro_start_vol = CONFIG_Foc_ThroStartVol;
	conf.c.thro_end_vol = CONFIG_Foc_ThroEndVol;
	conf.c.thro_max_vol = CONFIG_Foc_ThroMaxVol;
	conf.c.thro_min_vol = CONFIG_Foc_ThroMinVol;
	conf.c.thro_dec_time = CONFIG_Foc_ThroDecTime;

	conf.c.pid[PID_IQ_ID].kp = (float)conf.c.dq_loop_bandwith * 2.0f * 3.14f * conf.m.ld;
	conf.c.pid[PID_IQ_ID].ki = conf.m.rs / conf.m.ld;
	conf.c.pid[PID_IQ_ID].kd = 0;

	conf.c.pid[PID_IQ_ID].kp = (float)conf.c.dq_loop_bandwith * 2.0f * 3.14f * conf.m.lq;
	conf.c.pid[PID_IQ_ID].ki = conf.m.rs / conf.m.lq;
	conf.c.pid[PID_IQ_ID].kd = 0;

	conf.c.pid[PID_VelLim_ID].kp = CONFIG_Foc_PID_VelLim_Kp;
	conf.c.pid[PID_VelLim_ID].ki = CONFIG_Foc_PID_VelLim_Ki;
	conf.c.pid[PID_VelLim_ID].kd = CONFIG_Foc_PID_VelLim_Kd;

	conf.c.pid[PID_Vel_ID].kp = CONFIG_Foc_PID_VelCtrl_Kp;
	conf.c.pid[PID_Vel_ID].ki = CONFIG_Foc_PID_VelCtrl_Ki;
	conf.c.pid[PID_Vel_ID].kd = CONFIG_Foc_PID_VelCtrl_Kd;

	conf.c.pid[PID_AutoHold_ID].kp = CONFIG_Foc_PID_Autohold_Kp;
	conf.c.pid[PID_AutoHold_ID].kp = CONFIG_Foc_PID_Autohold_Ki;
	conf.c.pid[PID_AutoHold_ID].ki = CONFIG_Foc_PID_Autohold_Kd;

	conf.c.pid[PID_IDCLim_ID].kp = CONFIG_Foc_PID_IDCLim_Kp;
	conf.c.pid[PID_IDCLim_ID].ki = CONFIG_Foc_PID_IDCLim_Ki;
	conf.c.pid[PID_IDCLim_ID].kd = CONFIG_Foc_PID_IDCLim_Kd;

	conf.s.auto_hold = CONFIG_Settings_AutoHold;
	conf.s.brk_shut_power = CONFIG_Settings_BrkShutPower;
	conf.s.tcs_enable = CONFIG_Settings_TcsEnable;

	Gear_Config_default(0);
	Gear_Config_default(1);
	Gear_Config_default(2);
	Gear_Config_default(3);
	GearLow_Config_default(0);
	GearLow_Config_default(1);
	GearLow_Config_default(2);
	GearLow_Config_default(3);

	Limter_Config_default(p_mot, Motor, 0);
	Limter_Config_default(p_mot, Motor, 1);
	Limter_Config_default(p_mot, Motor,2);

	Limter_Config_default(p_mos, MosFet, 0);
	Limter_Config_default(p_mos, MosFet, 1);
	Limter_Config_default(p_mos, MosFet, 2);

	conf.p_vol.enter_pointer = CONFIG_Protect_Voltage_Level0_Entry;
	conf.p_vol.exit_pointer = CONFIG_Protect_Voltage_Level0_Exit;
	conf.p_vol.limit_value = CONFIG_Protect_Voltage_Level0_Value;

	EnReco_Config_default(0);
	EnReco_Config_default(1);
	EnReco_Config_default(2);
	EnReco_Config_default(3);
	EnReco_Config_default(4);
	EnReco_Config_default(5);
	EnReco_Config_default(6);
	EnReco_Config_default(7);
	EnReco_Config_default(8);
	EnReco_Config_default(9);

	conf.crc = crc16_get((u8 *)&conf, (u8 *)&conf.crc - (u8 *)&conf);
}


void mc_conf_init(void) {
	mc_conf_load();
}

int mc_conf_decode_motor(u8 *buff) {
	u8 *ori = buff;
	conf.m.poles = decode_u8(buff++);
	conf.m.ld = decode_float(buff);buff += 4;
	conf.m.lq = decode_float(buff);buff += 4;
	conf.m.rs = decode_float(buff);buff += 4;
	conf.m.flux = decode_float(buff);buff +=4;
	conf.m.nor_pll_band = (float)decode_u16(buff); buff += 2;
	conf.m.epm_pll_band = (float)decode_u16(buff); buff += 2;
	conf.m.pos_pll_band = (float)decode_u16(buff); buff += 2;
	conf.m.velocity_w = decode_u16(buff);buff += 2;
	conf.m.wheel_c = decode_u16(buff);buff += 2;
	conf.m.gear_ratio = decode_float(buff);buff += 4;
	conf.m.max_fw_id = decode_u16(buff); buff += 2;
	return buff - ori;
}

int mc_conf_encode_motor(u8 *buff) {
	u8 *ori = buff;
	encode_u8(buff++, conf.m.poles);
	encode_float(buff, conf.m.ld);buff += 4;
	encode_float(buff, conf.m.lq);buff += 4;
	encode_float(buff, conf.m.rs);buff += 4;
	encode_float(buff, conf.m.flux);buff +=4;
	encode_u16(buff, (u16)conf.m.nor_pll_band); buff += 2;
	encode_u16(buff, (u16)conf.m.epm_pll_band); buff += 2;
	encode_u16(buff, (u16)conf.m.pos_pll_band); buff += 2;
	encode_u16(buff, conf.m.velocity_w);buff += 2;
	encode_u16(buff, conf.m.wheel_c);buff += 2;
	encode_float(buff, conf.m.gear_ratio);buff += 4;
	encode_u16(buff, conf.m.max_fw_id); buff += 4;
	return buff - ori;
}


int mc_conf_decode_pid(pid_t *pid, u8 *buff) {
	u8 *ori = buff;
	pid->kp = decode_float(buff);buff += 4;
	pid->ki = decode_float(buff);buff += 4;
	pid->kd = decode_float(buff);buff += 4;
	return buff - ori;
}

int mc_conf_encode_pid(pid_t *pid, u8 *buff) {
	u8 *ori = buff;
	encode_float(buff, pid->kp);buff += 4;
	encode_float(buff, pid->ki);buff += 4;
	encode_float(buff, pid->kd);buff += 4;
	return buff - ori;
}

int mc_conf_decode_controler(u8 *buff) {
	u8 *ori = buff;
	conf.c.max_dc_vol = decode_s16(buff);buff += 2;
	conf.c.min_dc_vol = decode_s16(buff);buff += 2;
	conf.c.max_phase_curr = decode_s16(buff);buff += 2;
	conf.c.max_torque = decode_s16(buff);buff += 2;
	conf.c.max_rpm = decode_s16(buff);buff += 2;
	conf.c.max_epm_rpm = decode_s16(buff);buff += 2;
	conf.c.max_epm_torque = decode_s16(buff);buff += 2;
	conf.c.max_epm_back_rpm = decode_s16(buff);buff += 2;
	conf.c.max_epm_back_torque = decode_s16(buff);buff += 2;
	conf.c.max_ebrk_torque = decode_s16(buff);buff += 2;
	conf.c.max_idc = decode_s16(buff);buff += 2;
	conf.c.dq_loop_bandwith = decode_s16(buff);buff += 2;
	conf.c.thro_start_vol = decode_float(buff);buff += 4;
	conf.c.thro_end_vol = decode_float(buff);buff += 4;
	conf.c.thro_max_vol = decode_float(buff);buff += 4;
	conf.c.thro_min_vol = decode_float(buff);buff += 4;
	conf.c.thro_dec_time = decode_u16(buff);buff += 2;
	buff += mc_conf_decode_pid(&conf.c.pid[PID_VelLim_ID], buff);
	buff += mc_conf_decode_pid(&conf.c.pid[PID_Vel_ID], buff);
	buff += mc_conf_decode_pid(&conf.c.pid[PID_AutoHold_ID], buff);
	buff += mc_conf_decode_pid(&conf.c.pid[PID_IDCLim_ID], buff);
	return buff - ori;
}

int mc_conf_encode_controler(u8 *buff) {
	u8 *ori = buff;
	encode_s16(buff, conf.c.max_dc_vol);buff += 2;
	encode_s16(buff, conf.c.min_dc_vol);buff += 2;
	encode_s16(buff, conf.c.max_phase_curr);buff += 2;
	encode_s16(buff, conf.c.max_torque);buff += 2;
	encode_s16(buff, conf.c.max_rpm);buff += 2;
	encode_s16(buff, conf.c.max_epm_rpm);buff += 2;
	encode_s16(buff, conf.c.max_epm_torque);buff += 2;
	encode_s16(buff, conf.c.max_epm_back_rpm);buff += 2;
	encode_s16(buff, conf.c.max_epm_back_torque);buff += 2;
	encode_s16(buff, conf.c.max_ebrk_torque);buff += 2;
	encode_s16(buff, conf.c.max_idc);buff += 2;
	encode_s16(buff, conf.c.dq_loop_bandwith);buff += 2;
	encode_float(buff, conf.c.thro_start_vol);buff += 4;
	encode_float(buff, conf.c.thro_end_vol);buff += 4;
	encode_float(buff, conf.c.thro_max_vol);buff += 4;
	encode_float(buff, conf.c.thro_min_vol);buff += 4;
	encode_u16(buff, conf.c.thro_dec_time);buff += 2;
	buff += mc_conf_encode_pid(&conf.c.pid[PID_VelLim_ID], buff);
	buff += mc_conf_encode_pid(&conf.c.pid[PID_Vel_ID], buff);
	buff += mc_conf_encode_pid(&conf.c.pid[PID_AutoHold_ID], buff);
	buff += mc_conf_encode_pid(&conf.c.pid[PID_IDCLim_ID], buff);
	return buff - ori;
}


int mc_conf_decode_setting(u8 *buff) {
	u8 *ori = buff;
	conf.s.auto_hold = decode_u8(buff++)==1?true:false;
	conf.s.brk_shut_power = decode_u8(buff++)==1?true:false;
	conf.s.tcs_enable = decode_u8(buff++)==1?true:false;
	return buff - ori;
}

int mc_conf_encode_setting(u8 *buff) {
	u8 *ori = buff;
	encode_u8(buff++, conf.s.auto_hold?1:0);
	encode_u8(buff++, conf.s.brk_shut_power?1:0);
	encode_u8(buff++, conf.s.tcs_enable?1:0);
	return buff - ori;
}


int mc_conf_decode_gear(gear_t *g , u8 *buff) {
	u8 *ori = buff;
	for (int i = 0; i < CONFIG_MAX_GEARS; i++) {
		g[i].max_speed = decode_s16(buff); buff += 2;
		g[i].max_trq = decode_s16(buff); buff += 2;
		g[i].max_idc = decode_s16(buff); buff += 2;
		g[i].zero_accl = decode_u16(buff); buff += 2;
		g[i].accl_time = decode_u16(buff); buff += 2;
		for (int j = 0; j < CONFIG_GEAR_SPEED_TRQ_NUM; j++) {
			g[i].torque[j] = decode_u8(buff++);
		}
	}
	return buff - ori;
}

int mc_conf_encode_gear(gear_t *g, u8 *buff) {
	u8 *ori = buff;
	for (int i = 0; i < CONFIG_MAX_GEARS; i++) {
		encode_s16(buff, g[i].max_speed); buff += 2;
		encode_s16(buff, g[i].max_trq); buff += 2;
		encode_s16(buff, g[i].max_idc); buff += 2;
		encode_u16(buff, g[i].zero_accl); buff += 2;
		encode_u16(buff, g[i].accl_time); buff += 2;
		for (int j = 0; j < CONFIG_GEAR_SPEED_TRQ_NUM; j++) {
			encode_u8(buff++, g[i].torque[j]);
		}
	}
	return buff - ori;
}


int mc_conf_decode_limiter(limiter_t *l, int size, u8 *buff) {
	u8 *ori = buff;
	for (int i = 0; i < size; i++) {
		l->enter_pointer = decode_s16(buff);buff += 2;
		l->exit_pointer = decode_s16(buff);buff += 2;
		l->limit_value = decode_s16(buff);buff += 2;
	}
	return buff - ori;
}

int mc_conf_encode_limiter(limiter_t *l, int size, u8 *buff) {
	u8 *ori = buff;
	for (int i = 0; i < size; i++) {
		encode_s16(buff, l->enter_pointer);buff += 2;
		encode_s16(buff, l->exit_pointer);buff += 2;
		encode_s16(buff, l->limit_value);buff += 2;
	}
	return buff - ori;
}


int mc_conf_decode_engreco(u8 *buff) {
	u8 *ori = buff;
	for (int i = 0; i < CONFIG_EBRK_LVL_NUM; i++) {
		conf.e_r[i].torque = decode_s16(buff); buff += 2;
		conf.e_r[i].time = decode_s16(buff); buff += 2;
	}
	return buff - ori;
}

int mc_conf_encode_engreco(u8 *buff) {
	u8 *ori = buff;
	for (int i = 0; i < CONFIG_EBRK_LVL_NUM; i++) {
		encode_s16(buff, conf.e_r[i].torque); buff += 2;
		encode_s16(buff, conf.e_r[i].time); buff += 2;
	}
	return buff - ori;
}

void mc_conf_decode_configs(void) {
	u8 *buff = conf_buff;
	conf.version = decode_u8(buff++);
	buff += mc_conf_decode_motor(buff);
	buff += mc_conf_decode_controler(buff);
	buff += mc_conf_decode_setting(buff);
	buff += mc_conf_decode_gear(conf.g_n, buff);
	buff += mc_conf_decode_gear(conf.g_l, buff);
	buff += mc_conf_decode_limiter(conf.p_mot, CONFIG_TEMP_PROT_NUM, buff);
	buff += mc_conf_decode_limiter(conf.p_mos, CONFIG_TEMP_PROT_NUM, buff);
	buff += mc_conf_decode_limiter(&conf.p_vol, 1, buff);
	buff += mc_conf_decode_engreco(buff);
}

int mc_conf_encode_configs(u8 *buff) {
	u8 *ori = buff;
	encode_u8(buff++, conf.version);
	buff += mc_conf_encode_motor(buff);
	buff += mc_conf_encode_controler(buff);
	buff += mc_conf_encode_setting(buff);
	buff += mc_conf_encode_gear(conf.g_n, buff);
	buff += mc_conf_encode_gear(conf.g_l, buff);
	buff += mc_conf_encode_limiter(conf.p_mot, CONFIG_TEMP_PROT_NUM, buff);
	buff += mc_conf_encode_limiter(conf.p_mos, CONFIG_TEMP_PROT_NUM, buff);
	buff += mc_conf_encode_limiter(&conf.p_vol, 1, buff);
	buff += mc_conf_encode_engreco(buff);
	return buff - ori;
}

void mc_conf_set_pid(u8 id, pid_t *pid) {
	conf.c.pid[id] = *pid;
}

void mc_conf_get_pid(u8 id, pid_t *pid) {
	*pid = conf.c.pid[id];
}

bool mc_conf_set_gear(u8 mode, u8 *data, int len) {
	gear_t *g = conf.g_n;
	if (mode == 0) {
		g = conf.g_l;
	}
	if (mc_conf_decode_gear(g, data) > len) {
		return false;
	}
	return true;
}

int mc_conf_get_gear(u8 mode, u8 *data) {
	gear_t *g = conf.g_n;
	if (mode == 0) {
		g = conf.g_l;
	}
	return mc_conf_decode_gear(g, data);
}

bool mc_conf_set_limter(u8 *data, int len) {
	int l = mc_conf_decode_limiter(conf.p_mot, CONFIG_TEMP_PROT_NUM, data);
	l += mc_conf_decode_limiter(conf.p_mos, CONFIG_TEMP_PROT_NUM, data + l);
	l += mc_conf_decode_limiter(&conf.p_vol, 1, data + l);
	return len>=l;
}

int mc_conf_get_limter(u8 *data) {
	u8 *ori = data;
	data += mc_conf_decode_limiter(conf.p_mot, CONFIG_TEMP_PROT_NUM, data);
	data += mc_conf_decode_limiter(conf.p_mos, CONFIG_TEMP_PROT_NUM, data);
	data += mc_conf_decode_limiter(&conf.p_vol, 1, data);
	return data-ori;
}