Added support for statistics

Author: vedderb

CHANGELOG

@@ -44,6 +44,7 @@
 * Compensate inductance for motor saliency in observer.
 * Added MTPA mode based on measured current.
 * Faster overvoltage protection.
+* Added statistics counters.
 
 === FW 5.02 ===
 * IMU calibration improvement.

commands.c

@@ -1491,6 +1491,45 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		comm_can_io_board_set_output_digital(id, channel, on);
 	} break;
 
+	case COMM_GET_STATS: {
+		int32_t ind = 0;
+		uint32_t mask = buffer_get_uint16(data, &ind);
+
+		ind = 0;
+		uint8_t send_buffer[60];
+		send_buffer[ind++] = packet_id;
+		buffer_append_uint32(send_buffer, mask, &ind);
+
+		if (mask & ((uint32_t)1 << 0)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_speed_avg(), &ind); }
+		if (mask & ((uint32_t)1 << 1)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_speed_max(), &ind); }
+		if (mask & ((uint32_t)1 << 2)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_power_avg(), &ind); }
+		if (mask & ((uint32_t)1 << 3)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_power_max(), &ind); }
+		if (mask & ((uint32_t)1 << 4)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_current_avg(), &ind); }
+		if (mask & ((uint32_t)1 << 5)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_current_max(), &ind); }
+		if (mask & ((uint32_t)1 << 6)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_temp_mosfet_avg(), &ind); }
+		if (mask & ((uint32_t)1 << 7)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_temp_mosfet_max(), &ind); }
+		if (mask & ((uint32_t)1 << 8)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_temp_motor_avg(), &ind); }
+		if (mask & ((uint32_t)1 << 9)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_temp_motor_max(), &ind); }
+		if (mask & ((uint32_t)1 << 10)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_count_time(), &ind); }
+
+		reply_func(send_buffer, ind);
+	} break;
+
+	case COMM_RESET_STATS: {
+		bool ack = false;
+
+		if (len > 0) {
+			ack = data[0];
+		}
+
+		if (ack) {
+			int32_t ind = 0;
+			uint8_t send_buffer[50];
+			send_buffer[ind++] = packet_id;
+			reply_func(send_buffer, ind);
+		}
+	} 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.

conf_general.h

@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			5
 #define FW_VERSION_MINOR			03
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		63
+#define FW_TEST_VERSION_NUMBER		64
 
 #include "datatypes.h"
 

datatypes.h

@@ -1057,6 +1057,8 @@ typedef enum {
 	COMM_BM_MEM_WRITE,
 	COMM_BMS_BLNC_SELFTEST,
 	COMM_GET_EXT_HUM_TMP,
+	COMM_GET_STATS,
+	COMM_RESET_STATS,
 } COMM_PACKET_ID;
 
 // CAN commands
@@ -1302,6 +1304,21 @@ typedef struct {
 	uint8_t num_vescs;
 } setup_values;
 
+typedef struct {
+	systime_t time_start;
+	double samples;
+	double speed_sum;
+	float max_speed;
+	double power_sum;
+	float max_power;
+	double temp_motor_sum;
+	float max_temp_motor;
+	double temp_mos_sum;
+	float max_temp_mos;
+	double current_sum;
+	float max_current;
+} setup_stats;
+
 #define BACKUP_VAR_INIT_CODE				92891934
 
 typedef struct __attribute__((packed)) {

mc_interface.c

@@ -54,6 +54,7 @@ volatile float ADC_curr_norm_value[6];
 typedef struct {
 	volatile mc_configuration m_conf;
 	mc_fault_code m_fault_now;
+	setup_stats m_stats;
 	int m_ignore_iterations;
 	int m_drv_fault_iterations;
 	unsigned int m_cycles_running;
@@ -124,6 +125,7 @@ static volatile bool m_fault_stop_is_second_motor;
 // Private functions
 static void update_override_limits(volatile motor_if_state_t *motor, volatile mc_configuration *conf);
 static void run_timer_tasks(volatile motor_if_state_t *motor);
+static void update_stats(volatile motor_if_state_t *motor);
 static volatile motor_if_state_t *motor_now(void);
 
 // Function pointers
@@ -138,6 +140,8 @@ static thread_t *sample_send_tp;
 static THD_WORKING_AREA(fault_stop_thread_wa, 512);
 static THD_FUNCTION(fault_stop_thread, arg);
 static thread_t *fault_stop_tp;
+static THD_WORKING_AREA(stat_thread_wa, 512);
+static THD_FUNCTION(stat_thread, arg);
 
 void mc_interface_init(void) {
 	memset((void*)&m_motor_1, 0, sizeof(motor_if_state_t));
@@ -165,10 +169,13 @@ void mc_interface_init(void) {
 	m_sample_mode_last = DEBUG_SAMPLING_OFF;
 	m_sample_is_second_motor = false;
 
+	mc_interface_stat_reset();
+
 	// Start threads
 	chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
 	chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
 	chThdCreateStatic(fault_stop_thread_wa, sizeof(fault_stop_thread_wa), HIGHPRIO - 3, fault_stop_thread, NULL);
+	chThdCreateStatic(stat_thread_wa, sizeof(stat_thread_wa), NORMALPRIO, stat_thread, NULL);
 
 	int motor_old = mc_interface_get_motor_thread();
 	mc_interface_select_motor_thread(1);
@@ -2538,6 +2545,121 @@ static THD_FUNCTION(timer_thread, arg) {
 	}
 }
 
+static void update_stats(volatile motor_if_state_t *motor) {
+	mc_interface_select_motor_thread(motor == (&m_motor_1) ? 1 : 2);
+
+	setup_values val = mc_interface_get_setup_values();
+
+	const double power = mc_interface_get_input_voltage_filtered() * fabsf(val.current_in_tot);
+	const double speed = mc_interface_get_speed();
+	const double temp_mos = mc_interface_temp_fet_filtered();
+	const double temp_mot = mc_interface_temp_motor_filtered();
+
+	motor->m_stats.power_sum += power;
+	motor->m_stats.speed_sum += fabs(speed);
+	motor->m_stats.temp_mos_sum += temp_mos;
+	motor->m_stats.temp_motor_sum += temp_mot;
+	motor->m_stats.current_sum += fabs((double)(val.current_tot));
+	motor->m_stats.samples += (double)1.0;
+
+	if (power > (double)motor->m_stats.max_power) {
+		motor->m_stats.max_power = power;
+	}
+
+	if (fabs(speed) > (double)motor->m_stats.max_speed) {
+		motor->m_stats.max_speed = fabsf(speed);
+	}
+
+	if (temp_mos > (double)motor->m_stats.max_temp_mos) {
+		motor->m_stats.max_temp_mos = temp_mos;
+	}
+
+	if (temp_mot > (double)motor->m_stats.max_temp_motor) {
+		motor->m_stats.max_temp_motor = temp_mot;
+	}
+
+	if (fabsf(val.current_tot) > motor->m_stats.max_current) {
+		motor->m_stats.max_current = fabsf(val.current_tot);
+	}
+}
+
+float mc_interface_stat_speed_avg(void) {
+	volatile setup_stats *s = &motor_now()->m_stats;
+	double res = s->speed_sum / s->samples;
+	return res;
+}
+
+float mc_interface_stat_speed_max(void) {
+	return motor_now()->m_stats.max_speed;
+}
+
+float mc_interface_stat_power_avg(void) {
+	volatile setup_stats *s = &motor_now()->m_stats;
+	double res = s->power_sum / s->samples;
+	return res;
+}
+
+float mc_interface_stat_power_max(void) {
+	return motor_now()->m_stats.max_power;
+}
+
+float mc_interface_stat_current_avg(void) {
+	volatile setup_stats *s = &motor_now()->m_stats;
+	double res = s->current_sum / s->samples;
+	return res;
+}
+
+float mc_interface_stat_current_max(void) {
+	return motor_now()->m_stats.max_current;
+}
+
+float mc_interface_stat_temp_mosfet_avg(void) {
+	volatile setup_stats *s = &motor_now()->m_stats;
+	double res = s->temp_mos_sum / s->samples;
+	return res;
+}
+
+float mc_interface_stat_temp_mosfet_max(void) {
+	return motor_now()->m_stats.max_temp_mos;
+}
+
+float mc_interface_stat_temp_motor_avg(void) {
+	volatile setup_stats *s = &motor_now()->m_stats;
+	double res = s->temp_motor_sum / s->samples;
+	return res;
+}
+
+float mc_interface_stat_temp_motor_max(void) {
+	return motor_now()->m_stats.max_temp_motor;
+}
+
+float mc_interface_stat_count_time(void) {
+	return UTILS_AGE_S(motor_now()->m_stats.time_start);
+}
+
+void mc_interface_stat_reset(void) {
+	volatile setup_stats *s = &motor_now()->m_stats;
+	memset((void*)s, 0, sizeof(setup_stats));
+	s->time_start = chVTGetSystemTimeX();
+	s->max_temp_mos = -300.0;
+	s->max_temp_motor = -300.0;
+}
+
+static THD_FUNCTION(stat_thread, arg) {
+	(void)arg;
+
+	chRegSetThreadName("StatCounter");
+
+	for(;;) {
+		update_stats(&m_motor_1);
+#ifdef HW_HAS_DUAL_MOTORS
+		update_stats(&m_motor_2);
+#endif
+
+		chThdSleepMilliseconds(10);
+	}
+}
+
 static THD_FUNCTION(sample_send_thread, arg) {
 	(void)arg;
 

mc_interface.h

@@ -101,6 +101,20 @@ void mc_interface_ignore_input_both(int time_ms);
 
 void mc_interface_set_current_off_delay(float delay_sec);
 
+// Statistics
+float mc_interface_stat_speed_avg(void);
+float mc_interface_stat_speed_max(void);
+float mc_interface_stat_power_avg(void);
+float mc_interface_stat_power_max(void);
+float mc_interface_stat_current_avg(void);
+float mc_interface_stat_current_max(void);
+float mc_interface_stat_temp_mosfet_avg(void);
+float mc_interface_stat_temp_mosfet_max(void);
+float mc_interface_stat_temp_motor_avg(void);
+float mc_interface_stat_temp_motor_max(void);
+float mc_interface_stat_count_time(void);
+void mc_interface_stat_reset(void);
+
 // MC implementation functions
 void mc_interface_fault_stop(mc_fault_code fault, bool is_second_motor, bool is_isr);
 int mc_interface_try_input(void);

terminal.c

@@ -1121,6 +1121,21 @@ void terminal_process_string(char *str) {
 				commands_printf("Invalid arguments\n");
 			}
 		}
+	} else if (strcmp(argv[0], "stats") == 0) {
+		commands_printf("Speed Avg  : %.1f km/h", (double)(mc_interface_stat_speed_avg() * 3.6));
+		commands_printf("Speed Max  : %.1f km/h", (double)(mc_interface_stat_speed_max() * 3.6));
+		commands_printf("Power Avg  : %.1f W", (double)mc_interface_stat_power_avg());
+		commands_printf("Power Max  : %.1f W", (double)mc_interface_stat_power_max());
+		commands_printf("Current Avg: %.1f A", (double)mc_interface_stat_current_avg());
+		commands_printf("Current Max: %.1f A", (double)mc_interface_stat_current_max());
+		commands_printf("T FET Avg  : %.1f degC", (double)mc_interface_stat_temp_mosfet_avg());
+		commands_printf("T FET Max  : %.1f degC", (double)mc_interface_stat_temp_mosfet_max());
+		commands_printf("T MOTOR Avg: %.1f degC", (double)mc_interface_stat_temp_motor_avg());
+		commands_printf("T MOTOR Max: %.1f degC", (double)mc_interface_stat_temp_motor_max());
+		commands_printf("Count Time : %.1f s\n", (double)mc_interface_stat_count_time());
+	} else if (strcmp(argv[0], "stats_reset") == 0) {
+		mc_interface_stat_reset();
+		commands_printf("OK\n");
 	}
 
 	// The help command
@@ -1269,6 +1284,12 @@ void terminal_process_string(char *str) {
 		commands_printf("update_pid_pos_offset [angle_now] [store]");
 		commands_printf("  Update position PID offset.");
 
+		commands_printf("stats");
+		commands_printf("  Print setup statistics.");
+
+		commands_printf("stats_reset");
+		commands_printf("  Reset setup statistics.");
+
 		for (int i = 0;i < callback_write;i++) {
 			if (callbacks[i].cbf == 0) {
 				continue;