MC100/Applications/prot/can_foc_msg.c

#include "libs/utils.h"
#include "prot/can_message.h"
#include "prot/can_foc_msg.h"
#include "foc/commands.h"
#include "foc/samples.h"
#include "foc/motor/motor.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "foc/core/foc_observer.h"

static float can_get_speed(void) {
	float speed = PMSM_FOC_GetSpeed();
	if (!PMSM_FOC_Is_Start() || foc_observer_is_encoder()) {
		speed = motor_encoder_get_speed();
	}else {
		if (foc_observer_sensorless_stable()) {
			speed = foc_observer_sensorless_speed();
		}else {
			speed = 0;
		}
	}
	return speed;
}

void can_report_speed(u8 can, s16 rpm) {
	u8 data[6];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Speed));
	encode_s16(data + 2, rpm);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_power(u8 can) {
	u8 data[8];
	s16 rpm = (s16)can_get_speed();
	float vDC = get_vbus_float();
	float iDC = PMSM_FOC_GetVbusCurrent();	
	s16 v = S16Q5(vDC);
	s16 i = S16Q5(iDC);
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Power));
	encode_s16(data + 2, ABS(rpm));
	encode_s16(data + 4, v);
	encode_s16(data + 6, i);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}


void can_report_phase_current(u8 can) {
	u8 data[8];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Current));
	encode_s16(data + 2, S16Q5(PMSM_FOC_Get()->in.s_iABC[0]));
	encode_s16(data + 4, S16Q5(PMSM_FOC_Get()->in.s_iABC[1]));
	encode_s16(data + 6, S16Q5(PMSM_FOC_Get()->in.s_iABC[2]));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_phase_voltage(u8 can) {
	u8 data[8];
	float s_vABC[3];
	get_phase_vols(s_vABC);
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Vol));
	encode_s16(data + 2, S16Q5(s_vABC[0]));
	encode_s16(data + 4, S16Q5(s_vABC[1]));
	encode_s16(data + 6, S16Q5(s_vABC[2]));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_dq_current(u8 can) {
	u8 data[6];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Current));
	float id = PMSM_FOC_Get()->out.s_FilterIdq.d;
	float iq = PMSM_FOC_Get()->out.s_FilterIdq.q;
	encode_s16(data + 2, S16Q5(id));
	encode_s16(data + 4, S16Q5(iq));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_response_hall_offset(u8 can, int offset) {
	u8 data[7];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Cali_Hall_Offset));
	encode_u8(data + 2, 1);
	encode_u32(data + 3, offset);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_pid_value(u8 can, u8 id) {
	float kp, ki, kd;
	PMSM_FOC_GetPid(id, &kp, &ki, &kd);
	u8 data[15];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Pid));
	data[2] = id;
	encode_float(data + 3, kp);
	encode_float(data + 7, ki);
	encode_float(data + 11, kd);
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_foc_status(u8 can) {
	u8 data[16];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Status));
	mc_get_running_status(data+2);
	u32 errMask = mc_get_critical_error();
	encode_u32(data + 3, errMask);
	encode_s16(data + 7, get_motor_temp());
	encode_s16(data + 9, get_mos_temp());
	encode_u24(data + 11, shark_get_seconds());
	encode_u8(data + 14, mc_get_gear());
	encode_u8(data + 15, mc_params()->mos_lim | (mc_params()->motor_lim<<4));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

/* 采用组播方式上报,目的7E */
void can_mcast_foc_status2(void) {
	u8 data[8];
	encode_u16(data, mc_get_running_status2());
	encode_u16(data + 2, ABS((s16)(can_get_speed())));
	float vDC = get_vbus_float();
	encode_s16(data + 4, (s16)(vDC*10));
	float iDC = PMSM_FOC_GetVbusCurrent();
	if (!PMSM_FOC_Is_Start()) {
		iDC = 0;
	}
	encode_s16(data + 6, (s16)(iDC*10));
	shark_can0_send_message(get_indicator_can_id(0x7F), data, sizeof(data));
}

void can_report_mpta_values(u8 can) {
	u8 data[8];
	if (!PMSM_FOC_Get()->in.b_MTPA_calibrate) {
		return;
	}
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_MTPA_DQ_Angle));
	encode_s16(data + 2, S16Q5(PMSM_FOC_Get()->out.s_RealCurrentFiltered));
	encode_s16(data + 4, S16Q5(PMSM_FOC_Get()->out.s_FilterIdq.d));
	encode_s16(data + 6, S16Q5(PMSM_FOC_Get()->out.s_FilterIdq.q));
	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
}

void can_report_ext_status(u8 can) {
	u8 data[8] = {0x0, 0x0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
	data[0] = (get_vbus_int()>60?1:0) << 5;
	data[0] |= (mc_is_epm()?1:0) << 6;
	data[0] |= (mc_is_epm()?1:0) << 7;
	data[1] = mc_is_start()?0:1;
	data[1] |= (mc_is_cruise_enabled()?1:0) << 3;
	data[1] |= mc_get_gear() << 6;
	encode_s16(data + 2, ABS((s16)(can_get_speed()/4.0f)));
	float vDC = get_vbus_float();
	encode_s16(data + 4, (s16)(vDC*10));
	float iDC = PMSM_FOC_GetVbusCurrent();
	if (!PMSM_FOC_Is_Start()) {
		iDC = 0;
	}
	encode_s16(data + 6, (s16)(iDC*10));
	shark_can0_send_ext_message(0x1A014D43, data, sizeof(data));
}


void can_response_vols(u8 can, u8 key) {
	u8 data[16];
	int len;
	encoder_can_key(data, CMD_2_CAN_KEY(key));
	len = 2;
	float thro = get_throttle_float() * 10.0f;
	encode_s16(data + len, (s16)thro);
	len += 2;
	float acc = get_acc_vol() * 10.0f;
	encode_s16(data + len, (s16)acc);
	len += 2;
	float vbus = get_vbus_float() * 10.0f;
	encode_s16(data + len, (s16)vbus);
	len += 2;
	float thro_5v = get_thro_5v_float() * 10.0f;
	encode_s16(data + len, (s16)thro_5v);
	len += 2;
	thro = get_throttle2_float() * 10.0f;
	encode_s16(data + len, (s16)thro);
	len += 2;
	thro_5v = get_thro2_5v_float() * 10.0f;
	encode_s16(data + len, (s16)thro_5v);
	len += 2;
	can_send_response(can, data, len);
}

void can_report_plot_values(u8 can) {
    s16 trq_ref = eCtrl_get_RefTorque() * 10;
	s16 real_ref = PMSM_FOC_Get_Real_dqVector() * 10;
	can_plot2(trq_ref, real_ref);
}

void can_plot1(s16 v1) {
	u8 data[4];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
	encode_s16(data + 2, v1);
	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
}

void can_plot2(s16 v1, s16 v2) {
	u8 data[6];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
	encode_s16(data + 2, v1);
	encode_s16(data + 4, v2);
	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
}

void can_plot3(s16 v1, s16 v2, s16 v3) {
	u8 data[8];
	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
	encode_s16(data + 2, v1);
	encode_s16(data + 4, v2);
	encode_s16(data + 6, v3);
	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
}