Added power switch can commands

Author: vedderb

comm_can.c

@@ -79,6 +79,7 @@ static can_status_msg_5 stat_msgs_5[CAN_STATUS_MSGS_TO_STORE];
 static io_board_adc_values io_board_adc_1_4[CAN_STATUS_MSGS_TO_STORE];
 static io_board_adc_values io_board_adc_5_8[CAN_STATUS_MSGS_TO_STORE];
 static io_board_digial_inputs io_board_digital_in[CAN_STATUS_MSGS_TO_STORE];
+static psw_status psw_stat[CAN_STATUS_MSGS_TO_STORE];
 static unsigned int detect_all_foc_res_index = 0;
 static int8_t detect_all_foc_res[50];
 
@@ -120,6 +121,8 @@ void comm_can_init(void) {
 		io_board_adc_1_4[i].id = -1;
 		io_board_adc_5_8[i].id = -1;
 		io_board_digital_in[i].id = -1;
+
+		psw_stat[i].id = -1;
 	}
 
 #if CAN_ENABLE
@@ -911,6 +914,35 @@ void comm_can_io_board_set_output_pwm(int id, int channel, float duty) {
 			buffer, send_index, true);
 }
 
+psw_status *comm_can_get_psw_status_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &psw_stat[index];
+	} else {
+		return 0;
+	}
+}
+
+psw_status *comm_can_get_psw_status_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (psw_stat[i].id == id) {
+			return &psw_stat[i];
+		}
+	}
+
+	return 0;
+}
+
+void comm_can_psw_switch(int id, bool is_on, bool plot) {
+	int32_t send_index = 0;
+	uint8_t buffer[8];
+
+	buffer[send_index++] = is_on ? 1 : 0;
+	buffer[send_index++] = plot ? 1 : 0;
+
+	comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_PSW_SWITCH << 8),
+			buffer, send_index, true);
+}
+
 CANRxFrame *comm_can_get_rx_frame(void) {
 #if CAN_ENABLE
 	chMtxLock(&can_rx_mtx);
@@ -1599,7 +1631,26 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
 			}
 		}
 		break;
-		break;
+
+	case CAN_PACKET_PSW_STAT: {
+		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+			psw_status *msg = &psw_stat[i];
+			if (msg->id == id || msg->id == -1) {
+				ind = 0;
+				msg->id = id;
+				msg->rx_time = chVTGetSystemTime();
+
+				msg->v_in = buffer_get_float16(data8, 10.0, &ind);
+				msg->v_out = buffer_get_float16(data8, 10.0, &ind);
+				msg->temp = buffer_get_float16(data8, 10.0, &ind);
+				msg->is_out_on = (data8[ind] >> 0) & 1;
+				msg->is_pch_on = (data8[ind] >> 1) & 1;
+				msg->is_dsc_on = (data8[ind] >> 2) & 1;
+				ind++;
+				break;
+			}
+		}
+	} break;
 
 	default:
 		break;

comm_can.h

@@ -76,6 +76,10 @@ io_board_digial_inputs *comm_can_get_io_board_digital_in_id(int id);
 void comm_can_io_board_set_output_digital(int id, int channel, bool on);
 void comm_can_io_board_set_output_pwm(int id, int channel, float duty);
 
+psw_status *comm_can_get_psw_status_index(int index);
+psw_status *comm_can_get_psw_status_id(int id);
+void comm_can_psw_switch(int id, bool is_on, bool plot);
+
 CANRxFrame *comm_can_get_rx_frame(void);
 
 #endif /* COMM_CAN_H_ */

commands.c

@@ -212,6 +212,12 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 
 		send_buffer[ind++] = 0; // No custom config
 
+#ifdef HW_HAS_PHASE_FILTERS
+		send_buffer[ind++] = 1;
+#else
+		send_buffer[ind++] = 0;
+#endif
+
 		fw_version_sent_cnt++;
 
 		reply_func(send_buffer, ind);
@@ -1179,6 +1185,56 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		break;
 	}
 
+	// Power switch
+	case COMM_PSW_GET_STATUS: {
+		int32_t ind = 0;
+		bool by_id = data[ind++];
+		int id_ind = buffer_get_int16(data, &ind);
+
+		int psws_num = 0;
+		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+			psw_status *stat = comm_can_get_psw_status_index(i);
+			if (stat->id >= 0) {
+				psws_num++;
+			} else {
+				break;
+			}
+		}
+
+		psw_status *stat = 0;
+		if (by_id) {
+			stat = comm_can_get_psw_status_id(id_ind);
+		} else if (id_ind < psws_num) {
+			stat = comm_can_get_psw_status_index(id_ind);
+		}
+
+		if (stat) {
+			ind = 0;
+			uint8_t send_buffer[70];
+
+			send_buffer[ind++] = packet_id;
+			buffer_append_int16(send_buffer, stat->id, &ind);
+			buffer_append_int16(send_buffer, psws_num, &ind);
+			buffer_append_float32_auto(send_buffer, UTILS_AGE_S(stat->rx_time), &ind);
+			buffer_append_float32_auto(send_buffer, stat->v_in, &ind);
+			buffer_append_float32_auto(send_buffer, stat->v_out, &ind);
+			buffer_append_float32_auto(send_buffer, stat->temp, &ind);
+			send_buffer[ind++] = stat->is_out_on;
+			send_buffer[ind++] = stat->is_pch_on;
+			send_buffer[ind++] = stat->is_dsc_on;
+
+			reply_func(send_buffer, ind);
+		}
+	} break;
+
+	case COMM_PSW_SWITCH: {
+		int32_t ind = 0;
+		int id = buffer_get_int16(data, &ind);
+		bool is_on = data[ind++];
+		bool plot = data[ind++];
+		comm_can_psw_switch(id, is_on, plot);
+	} break;
+
 	// Blocking commands. Only one of them runs at any given time, in their
 	// own thread. If other blocking commands come before the previous one has
 	// finished, they are discarded.

datatypes.h

@@ -376,6 +376,12 @@ typedef struct {
 	uint16_t foc_hfi_start_samples;
 	float foc_hfi_obs_ovr_sec;
 	foc_hfi_samples foc_hfi_samples;
+	bool foc_offsets_measured;
+	float foc_offsets_current[3];
+	float foc_offsets_voltage[3];
+	float foc_offsets_voltage_undriven[3];
+	bool foc_phase_filter_enable;
+	float foc_phase_filter_max_erpm;
 
 	// GPDrive
 	int gpd_buffer_notify_left;
@@ -924,6 +930,10 @@ typedef enum {
 	COMM_ERASE_BOOTLOADER_ALL_CAN_HW,
 
 	COMM_SET_ODOMETER,
+
+	// Power switch commands
+	COMM_PSW_GET_STATUS,
+	COMM_PSW_SWITCH,
 } COMM_PACKET_ID;
 
 // CAN commands
@@ -973,7 +983,9 @@ typedef enum {
 	CAN_PACKET_BMS_BAL,
 	CAN_PACKET_BMS_TEMPS,
 	CAN_PACKET_BMS_HUM,
-	CAN_PACKET_BMS_SOC_SOH_TEMP_STAT
+	CAN_PACKET_BMS_SOC_SOH_TEMP_STAT,
+	CAN_PACKET_PSW_STAT,
+	CAN_PACKET_PSW_SWITCH
 } CAN_PACKET_ID;
 
 // Logged fault data
@@ -1070,6 +1082,17 @@ typedef struct {
 	uint64_t inputs;
 } io_board_digial_inputs;
 
+typedef struct {
+	int id;
+	systime_t rx_time;
+	float v_in;
+	float v_out;
+	float temp;
+	bool is_out_on;
+	bool is_pch_on;
+	bool is_dsc_on;
+} psw_status;
+
 typedef struct {
 	uint8_t js_x;
 	uint8_t js_y;