MC100/Applications/app/nv_storage.c

#include "app/nv_storage.h"
#include "bsp/fmc_flash.h"
#include "libs/crc16.h"
#include "libs/logger.h"
#include "foc/motor/motor_param.h"
#include "foc/foc_config.h"

static motor_params_t m_params;
static foc_params_t   foc_params;
static mc_gear_config_t gear_config;

motor_params_t *nv_get_motor_params(void) {
	return &m_params;
}
foc_params_t *nv_get_foc_params(void) {
	return &foc_params;
}

mc_gear_config_t *nv_get_gear_configs(void) {
	return &gear_config;
}

void nv_save_hall_table(s32 *hall_table) {
	memcpy((char *)m_params.hall_table, (char *)hall_table, sizeof(m_params.hall_table));
	nv_save_motor_params();
}

void nv_save_angle_offset(float offset) {
	m_params.offset = offset;
	nv_save_motor_params();
}


static void nv_default_motor_params(void) {
	m_params.mot_nr = MOTOR_NR;
	m_params.poles = MOTOR_POLES;
	m_params.r = MOTOR_R;
	m_params.ld = MOTOR_Ld;
	m_params.lq = MOTOR_Lq;
	m_params.offset = MOTOR_ENC_OFFSET;//180;//(69.0f);
	m_params.est_pll_band = 100;
	m_params.pos_lock_pll_band = 500;
	m_params.flux_linkage = 0.0f;
}

static void nv_default_foc_params(void) {
	foc_params.s_maxDCVol = CONFIG_MAX_DC_VOL;
	foc_params.s_minDCVol = CONFIG_MIN_DC_VOL;
	foc_params.s_PhaseCurrLim = CONFIG_DEFAULT_PHASE_CURR_LIM;
	foc_params.s_maxRPM = CONFIG_DEFAULT_RPM_LIM;
	foc_params.s_maxEpmRPM = CONFIG_DEFAULT_EPM_RPM;
	foc_params.s_maxEpmPhaseCurrLim = CONFIG_DEFAULT_EPM_PHASE_CURR;
	foc_params.s_maxTorque = foc_params.s_PhaseCurrLim;
	foc_params.s_PhaseCurreBrkLim = CONFIG_DEFAULT_EBRK_PHASE_CURR;
	foc_params.n_currentBand = CONFIG_CURRENT_BANDWITH;
	foc_params.n_brkShutPower = CONFIG_BRK_SHUT_POWER_ENABLE;
	foc_params.s_LimitiDC = CONFIG_DEFAULT_IDC_LIM;
	foc_params.s_iDCeBrkLim = CONFIG_DEFAULT_EBRK_IDC_LIM;
	foc_params.n_minThroVol = CONFIG_THROTTLE_LOW_VALUE;
	foc_params.n_maxThroVol = CONFIG_THROTTLE_MAX_VALUE;
	foc_params.n_autoHold = CONFIG_AUTOHOLD_ENABLE;
	foc_params.n_acc_time = CONFIG_ACC_TIME;
	foc_params.n_dec_time = CONFIG_DEC_TIME;
	foc_params.n_ebrk_time = CONFIG_EBRK_RAMP_TIME;
	foc_params.n_FwEnable  = CONFIG_DEFAULT_FW_ENABLE;
	foc_params.pid_conf[PID_D_id].kp = (foc_params.n_currentBand * MOTOR_Ld);
	foc_params.pid_conf[PID_D_id].ki = (MOTOR_R/MOTOR_Ld);
	foc_params.pid_conf[PID_D_id].kb = 0;
	
	foc_params.pid_conf[PID_Q_id].kp = (foc_params.n_currentBand * MOTOR_Lq);
	foc_params.pid_conf[PID_Q_id].ki = (MOTOR_R/MOTOR_Lq);
	foc_params.pid_conf[PID_Q_id].kb = 0;

	foc_params.pid_conf[PID_TRQ_id].kp = TRQ_PI_KP;
	foc_params.pid_conf[PID_TRQ_id].ki = TRQ_PI_KI;
	foc_params.pid_conf[PID_TRQ_id].kb = 1.0f;

	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;
	foc_params.pid_conf[PID_Spd_id].ki = 0.08f;
	foc_params.pid_conf[PID_Spd_id].kb = 1.0f;

	foc_params.pid_conf[PID_Pow_id].kp = 0.6f;
	foc_params.pid_conf[PID_Pow_id].ki = 4.0f;
	foc_params.pid_conf[PID_Pow_id].kb = 0;

	foc_params.pid_conf[PID_Lock_id].kp = (0.01f);
	foc_params.pid_conf[PID_Lock_id].ki = (0.20f);
	foc_params.pid_conf[PID_Lock_id].kb = 0;

	foc_params.pid_conf[PID_FW_id].kp = 0;
	foc_params.pid_conf[PID_FW_id].ki = 0.01f;
	foc_params.pid_conf[PID_FW_id].kb = 0;
}

static void nv_default_gear_config(void) {
	/* 48v 模式 */
	gear_config.gears_48[0].u_maxRPM = 1430;
	gear_config.gears_48[0].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM * 0.5f;
	gear_config.gears_48[0].u_maxIdc = 25;
	gear_config.gears_48[0].u_accTime = 50;

	gear_config.gears_48[1].u_maxRPM = 2100;
	gear_config.gears_48[1].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM * 0.8f;
	gear_config.gears_48[1].u_maxIdc = 30;
	gear_config.gears_48[1].u_accTime = 80;

	gear_config.gears_48[2].u_maxRPM = 2850;
	gear_config.gears_48[2].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_48[2].u_maxIdc = 40;
	gear_config.gears_48[2].u_accTime = 80;

	gear_config.gears_48[3].u_maxRPM = 2850;
	gear_config.gears_48[3].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_48[3].u_maxIdc = 40;
	gear_config.gears_48[3].u_accTime = 80;

	gear_config.gears_48[4].u_maxRPM = 2850;
	gear_config.gears_48[4].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_48[4].u_maxIdc = 40;
	gear_config.gears_48[4].u_accTime = 80;

	/* 96v 模式 */
	gear_config.gears_96[0].u_maxRPM = 2850;
	gear_config.gears_96[0].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM * 0.5f;
	gear_config.gears_96[0].u_maxIdc = 25;
	gear_config.gears_96[0].u_accTime = 50;

	gear_config.gears_96[1].u_maxRPM = 4300;
	gear_config.gears_96[1].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM * 0.8f;
	gear_config.gears_96[1].u_maxIdc = 30;
	gear_config.gears_96[1].u_accTime = 80;

	gear_config.gears_96[2].u_maxRPM = 5500;
	gear_config.gears_96[2].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_96[2].u_maxIdc = 35;
	gear_config.gears_96[2].u_accTime = 100;

	gear_config.gears_96[3].u_maxRPM = 5500;
	gear_config.gears_96[3].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_96[3].u_maxIdc = 45;
	gear_config.gears_96[3].u_accTime = 100;

	gear_config.gears_96[4].u_maxRPM = 5500;
	gear_config.gears_96[4].u_maxTorque = CONFIG_DEFAULT_PHASE_CURR_LIM;
	gear_config.gears_96[4].u_maxIdc = 45;
	gear_config.gears_96[4].u_accTime = 100;
}



void nv_save_motor_params(void) {
	u16 crc = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
	m_params.crc16 = crc;
	fmc_write_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	fmc_write_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
}
void nv_read_motor_params(void) {
	fmc_read_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	u16 crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
	if (crc0 != m_params.crc16) {
		sys_debug("mp 0 error\n");
		fmc_read_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
		if (crc0 != m_params.crc16) {
			nv_default_motor_params();
			nv_save_motor_params();
			return;
		}
		fmc_write_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	}else {
		fmc_read_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
		if (crc0 != m_params.crc16) {
			sys_debug("mp 1 error\n");
			fmc_read_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
			fmc_write_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		}
	}
}
void nv_save_foc_params(void) {
	u16 crc = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
	foc_params.crc16 = crc;
	fmc_write_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	fmc_write_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
}

void nv_read_foc_params(void) {
	fmc_read_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	u16 crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
	if (crc0 != foc_params.crc16) {
		sys_debug("fp 0 error\n");
		fmc_read_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
		if (crc0 != foc_params.crc16) {
			nv_default_foc_params();
			nv_save_foc_params();
			return;
		}
		fmc_write_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	}else {
		fmc_read_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
		if (crc0 != foc_params.crc16) {
			sys_debug("fp 1 error\n");
			fmc_read_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
			fmc_write_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		}
	}
	foc_params.s_maxTorque = foc_params.s_PhaseCurrLim;//overwrite
	sys_debug("maxTorque=%f\n", foc_params.s_maxTorque);
}

void nv_save_gear_configs(void) {
	u16 crc = crc16_get((u8 *)&gear_config, sizeof(gear_config) - 2);
	gear_config.crc16 = crc;
	fmc_write_data(gear_config_idx_0, (u8 *)&gear_config, sizeof(gear_config));
	fmc_write_data(gear_config_idx_1, (u8 *)&gear_config, sizeof(gear_config));
}

void nv_read_gear_configs(void) {
	fmc_read_data(gear_config_idx_0, (u8 *)&gear_config, sizeof(gear_config));
	u16 crc0 = crc16_get((u8 *)&gear_config, sizeof(gear_config) - 2);
	if (crc0 != gear_config.crc16) {
		sys_debug("gear 0 error\n");
		fmc_read_data(gear_config_idx_1, (u8 *)&gear_config, sizeof(gear_config));
		crc0 = crc16_get((u8 *)&gear_config, sizeof(gear_config) - 2);
		if (crc0 != gear_config.crc16) {
			nv_default_gear_config();
			nv_save_gear_configs();
			return;
		}
		fmc_write_data(gear_config_idx_0, (u8 *)&gear_config, sizeof(gear_config));
	}else {
		fmc_read_data(gear_config_idx_1, (u8 *)&gear_config, sizeof(gear_config));
		crc0 = crc16_get((u8 *)&gear_config, sizeof(gear_config) - 2);
		if (crc0 != gear_config.crc16) {
			sys_debug("gear 1 error\n");
			fmc_read_data(gear_config_idx_0, (u8 *)&gear_config, sizeof(gear_config));
			fmc_write_data(gear_config_idx_1, (u8 *)&gear_config, sizeof(gear_config));
		}
	}
}

bool nv_set_gear_config(u8 mode4896, u8 gear, u16 rpm, u16 torque, u16 idc, u16 acc) {
	mc_gear_t *gear_cfg;
	if (gear >= CONFIG_MAX_GEAR_NUM) {
		return false;
	}

	if (mode4896 == 0) { //48vmode
		gear_cfg = &gear_config.gears_48[gear];
	}else {
		gear_cfg = &gear_config.gears_96[gear];
	}
	gear_cfg->u_maxRPM = rpm;
	gear_cfg->u_maxTorque = torque;
	gear_cfg->u_maxIdc = idc;
	gear_cfg->u_accTime = acc;

	nv_save_gear_configs();
	return true;
}

bool nv_get_gear_config(u8 mode4896, u8 gear, u16 *rpm, u16 *torque, u16 *idc, u16 *acc) {
	mc_gear_t *gear_cfg;
	if (gear >= CONFIG_MAX_GEAR_NUM) {
		return false;
	}

	if (mode4896 == 0) { //48vmode
		gear_cfg = &gear_config.gears_48[gear];
	}else {
		gear_cfg = &gear_config.gears_96[gear];
	}
	*rpm = gear_cfg->u_maxRPM;
	*torque = gear_cfg->u_maxTorque;
	*idc = gear_cfg->u_maxIdc;
	*acc = gear_cfg->u_accTime;

	return true;
}


void nv_set_pid(u8 id, pid_conf_t *pid) {
	foc_params.pid_conf[id] = *pid;
	nv_save_foc_params();
}

void nv_get_pid(u8 id, pid_conf_t *pid) {
	*pid = foc_params.pid_conf[id];
}

void nv_set_hwbrake_mode(u8 mode) {
	foc_params.n_brkShutPower = mode;
}

void nv_set_throttle_vol(float min, float max) {
	foc_params.n_minThroVol = min;
	foc_params.n_maxThroVol = max;
}

void nv_set_ebrake_current(float phase_curr, float dc_curr) {
	foc_params.s_PhaseCurreBrkLim = phase_curr;
	foc_params.s_iDCeBrkLim = dc_curr;
}

torque_lut_t *nv_get_trq_tlb(void) {
	torque_lut_t *tbl = (torque_lut_t *)fmc_get_addr(trq_Tbl_idx);

	if (tbl->magic != 0x5AA5) {
		sys_error("trq tlb magic error, 0x%x\n", tbl->magic);
		return NULL;
	}
	u16 crc = crc16_get((u8 *)tbl, sizeof(torque_lut_t) - 2);
	if (crc != tbl->crc16) {
		sys_error("trq tlb crc16 error\n");
		return NULL;
	}
	return tbl;
}


void nv_storage_init(void) {
	nv_read_motor_params();
	nv_read_foc_params();
	nv_read_gear_configs();
	sys_debug("encoder_off = %f\n", m_params.offset);
	if ((m_params.mot_nr != MOTOR_NR) || (foc_params.pid_conf[PID_TRQ_id].kp != TRQ_PI_KP) || (foc_params.pid_conf[PID_TRQ_id].ki != TRQ_PI_KI)) {
		nv_default_motor_params();
		nv_default_foc_params();
		nv_save_foc_params();
		nv_save_motor_params();
	}
	sys_debug("current band %f\n", foc_params.n_currentBand);
}