MPU9x50 fix, experiment plots, DQ voltage RT data, smart reverse for nunchuk app

CHANGELOG

@@ -1,3 +1,10 @@
+=== FW 3.60 ===
+* Fixed IMU9x50 bug.
+* Unrigester ICM20948 terminal callbacks when unused.
+* Added experiment plot functions.
+* Added D and Q axis voltage to RT data.
+* Added smart reverse function to nunchuk app.
+
 === FW 3.59 ===
 * Added more data to MOTE_PACKET_ALIVE.
 * Added app template.

appconf/appconf_default.h

@@ -221,6 +221,15 @@
 #ifndef APPCONF_CHUK_TC_MAX_DIFF
 #define APPCONF_CHUK_TC_MAX_DIFF			3000.0
 #endif
+#ifndef APPCONF_CHUK_USE_SMART_REV
+#define APPCONF_CHUK_USE_SMART_REV			true
+#endif
+#ifndef APPCONF_CHUK_SMART_REV_MAX_DUTY
+#define APPCONF_CHUK_SMART_REV_MAX_DUTY		0.07
+#endif
+#ifndef APPCONF_CHUK_SMART_REV_RAMP_TIME
+#define APPCONF_CHUK_SMART_REV_RAMP_TIME	3.0
+#endif
 
 // NRF app
 #ifndef APPCONF_NRF_SPEED

applications/app_custom_template.c

@@ -86,6 +86,22 @@ static THD_FUNCTION(my_thread, arg) {
 
 	is_running = true;
 
+	// Example of using the experiment plot
+//	chThdSleepMilliseconds(8000);
+//	commands_init_plot("Sample", "Voltage");
+//	commands_plot_add_graph("Temp Fet");
+//	commands_plot_add_graph("Input Voltage");
+//	float samp = 0.0;
+//
+//	for(;;) {
+//		commands_plot_set_graph(0);
+//		commands_send_plot_points(samp, mc_interface_temp_fet_filtered());
+//		commands_plot_set_graph(1);
+//		commands_send_plot_points(samp, GET_INPUT_VOLTAGE());
+//		samp++;
+//		chThdSleepMilliseconds(10);
+//	}
+
 	for(;;) {
 		// Check if it is time to stop.
 		if (stop_now) {

applications/app_nunchuk.c

@@ -213,6 +213,8 @@ static THD_FUNCTION(output_thread, arg) {
 		static float rpm_filtered = 0.0;
 		UTILS_LP_FAST(rpm_filtered, mc_interface_get_rpm(), 0.5);
 
+		const float dt = (float)OUTPUT_ITERATION_TIME_MS / 1000.0;
+
 		if (timeout_has_timeout() || chuck_error != 0 || config.ctrl_type == CHUK_CTRL_TYPE_NONE) {
 			was_pid = false;
 			continue;
@@ -234,6 +236,7 @@ static THD_FUNCTION(output_thread, arg) {
 		static bool is_reverse = false;
 		static bool was_z = false;
 		const float current_now = mc_interface_get_tot_current_directional_filtered();
+		const float duty_now = mc_interface_get_duty_cycle_now();
 		static float prev_current = 0.0;
 		const float max_current_diff = mcconf->l_current_max * mcconf->l_current_max_scale * 0.2;
 
@@ -367,6 +370,7 @@ static THD_FUNCTION(output_thread, arg) {
 
 		float rpm_lowest = rpm_local;
 		float current_highest_abs = current_now;
+		float duty_highest_abs = fabsf(duty_now);
 
 		if (config.multi_esc) {
 			for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
@@ -391,10 +395,55 @@ static THD_FUNCTION(output_thread, arg) {
 					if (fabsf(msg_current) > fabsf(current_highest_abs)) {
 						current_highest_abs = msg_current;
 					}
+
+					if (fabsf(msg->duty) > duty_highest_abs) {
+						duty_highest_abs = fabsf(msg->duty);
+					}
 				}
 			}
 		}
 
+		if (config.use_smart_rev) {
+			bool duty_control = false;
+			static bool was_duty_control = false;
+			static float duty_rev = 0.0;
+
+			if (out_val < -0.95 && duty_highest_abs < (mcconf->l_min_duty * 1.5) &&
+					current_highest_abs < (mcconf->l_current_max * mcconf->l_current_max_scale * 0.7)) {
+				duty_control = true;
+			}
+
+			if (duty_control || (was_duty_control && out_val < -0.1)) {
+				was_duty_control = true;
+
+				float goal = config.smart_rev_max_duty * -out_val;
+				utils_step_towards(&duty_rev, is_reverse ? goal : -goal,
+						config.smart_rev_max_duty * dt / config.smart_rev_ramp_time);
+
+				mc_interface_set_duty(duty_rev);
+
+				// Send the same duty cycle to the other controllers
+				if (config.multi_esc) {
+					for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+						can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+						if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+							comm_can_set_duty(msg->id, duty_rev);
+						}
+					}
+				}
+
+				// Set the previous ramping current to not get a spike when releasing
+				// duty control.
+				prev_current = current_now;
+
+				continue;
+			}
+
+			duty_rev = duty_now;
+			was_duty_control = false;
+		}
+
 		// Apply ramping
 		const float current_range = mcconf->l_current_max * mcconf->l_current_max_scale +
 				fabsf(mcconf->l_current_min) * mcconf->l_current_min_scale;

commands.c

@@ -320,6 +320,12 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 			buffer_append_float16(send_buffer, NTC_TEMP_MOS2(), 1e1, &ind);
 			buffer_append_float16(send_buffer, NTC_TEMP_MOS3(), 1e1, &ind);
 		}
+		if (mask & ((uint32_t)1 << 19)) {
+			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_vd(), 1e3, &ind);
+		}
+		if (mask & ((uint32_t)1 << 20)) {
+			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_vq(), 1e3, &ind);
+		}
 
 		reply_func(send_buffer, ind);
 		chMtxUnlock(&send_buffer_mutex);
@@ -1083,6 +1089,48 @@ void commands_apply_mcconf_hw_limits(mc_configuration *mcconf) {
 #endif
 }
 
+void commands_init_plot(char *namex, char *namey) {
+	int ind = 0;
+	chMtxLock(&send_buffer_mutex);
+	send_buffer_global[ind++] = COMM_PLOT_INIT;
+	memcpy(send_buffer_global + ind, namex, strlen(namex));
+	ind += strlen(namex);
+	send_buffer_global[ind++] = '\0';
+	memcpy(send_buffer_global + ind, namey, strlen(namey));
+	ind += strlen(namey);
+	send_buffer_global[ind++] = '\0';
+	commands_send_packet(send_buffer_global, ind);
+	chMtxUnlock(&send_buffer_mutex);
+}
+
+void commands_plot_add_graph(char *name) {
+	int ind = 0;
+	chMtxLock(&send_buffer_mutex);
+	send_buffer_global[ind++] = COMM_PLOT_ADD_GRAPH;
+	memcpy(send_buffer_global + ind, name, strlen(name));
+	ind += strlen(name);
+	send_buffer_global[ind++] = '\0';
+	commands_send_packet(send_buffer_global, ind);
+	chMtxUnlock(&send_buffer_mutex);
+}
+
+void commands_plot_set_graph(int graph) {
+	int ind = 0;
+	uint8_t buffer[2];
+	buffer[ind++] = COMM_PLOT_SET_GRAPH;
+	buffer[ind++] = graph;
+	commands_send_packet(buffer, ind);
+}
+
+void commands_send_plot_points(float x, float y) {
+	int32_t ind = 0;
+	uint8_t buffer[10];
+	buffer[ind++] = COMM_PLOT_DATA;
+	buffer_append_float32_auto(buffer, x, &ind);
+	buffer_append_float32_auto(buffer, y, &ind);
+	commands_send_packet(buffer, ind);
+}
+
 static THD_FUNCTION(blocking_thread, arg) {
 	(void)arg;
 

commands.h

@@ -39,5 +39,9 @@ void commands_send_gpd_buffer_notify(void);
 void commands_send_mcconf(COMM_PACKET_ID packet_id, mc_configuration *mcconf);
 void commands_send_appconf(COMM_PACKET_ID packet_id, app_configuration *appconf);
 void commands_apply_mcconf_hw_limits(mc_configuration *mcconf);
+void commands_init_plot(char *namex, char *namey);
+void commands_plot_add_graph(char *name);
+void commands_plot_set_graph(int graph);
+void commands_send_plot_points(float x, float y);
 
 #endif /* COMMANDS_H_ */

conf_general.h

@@ -22,7 +22,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR		3
-#define FW_VERSION_MINOR		59
+#define FW_VERSION_MINOR		60
 
 #include "datatypes.h"
 

confgenerator.c

@@ -208,6 +208,9 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
 	buffer[ind++] = conf->app_chuk_conf.multi_esc;
 	buffer[ind++] = conf->app_chuk_conf.tc;
 	buffer_append_float32_auto(buffer, conf->app_chuk_conf.tc_max_diff, &ind);
+	buffer[ind++] = conf->app_chuk_conf.use_smart_rev;
+	buffer_append_float32_auto(buffer, conf->app_chuk_conf.smart_rev_max_duty, &ind);
+	buffer_append_float32_auto(buffer, conf->app_chuk_conf.smart_rev_ramp_time, &ind);
 	buffer[ind++] = conf->app_nrf_conf.speed;
 	buffer[ind++] = conf->app_nrf_conf.power;
 	buffer[ind++] = conf->app_nrf_conf.crc_type;
@@ -452,6 +455,9 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
 	conf->app_chuk_conf.multi_esc = buffer[ind++];
 	conf->app_chuk_conf.tc = buffer[ind++];
 	conf->app_chuk_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind);
+	conf->app_chuk_conf.use_smart_rev = buffer[ind++];
+	conf->app_chuk_conf.smart_rev_max_duty = buffer_get_float32_auto(buffer, &ind);
+	conf->app_chuk_conf.smart_rev_ramp_time = buffer_get_float32_auto(buffer, &ind);
 	conf->app_nrf_conf.speed = buffer[ind++];
 	conf->app_nrf_conf.power = buffer[ind++];
 	conf->app_nrf_conf.crc_type = buffer[ind++];
@@ -680,6 +686,9 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
 	conf->app_chuk_conf.multi_esc = APPCONF_CHUK_MULTI_ESC;
 	conf->app_chuk_conf.tc = APPCONF_CHUK_TC;
 	conf->app_chuk_conf.tc_max_diff = APPCONF_CHUK_TC_MAX_DIFF;
+	conf->app_chuk_conf.use_smart_rev = APPCONF_CHUK_USE_SMART_REV;
+	conf->app_chuk_conf.smart_rev_max_duty = APPCONF_CHUK_SMART_REV_MAX_DUTY;
+	conf->app_chuk_conf.smart_rev_ramp_time = APPCONF_CHUK_SMART_REV_RAMP_TIME;
 	conf->app_nrf_conf.speed = APPCONF_NRF_SPEED;
 	conf->app_nrf_conf.power = APPCONF_NRF_POWER;
 	conf->app_nrf_conf.crc_type = APPCONF_NRF_CRC;

confgenerator.h

@@ -9,7 +9,7 @@
 
 // Constants
 #define MCCONF_SIGNATURE		503309878
-#define APPCONF_SIGNATURE		2074596585
+#define APPCONF_SIGNATURE		543202478
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

datatypes.h

@@ -423,6 +423,9 @@ typedef struct {
 	bool multi_esc;
 	bool tc;
 	float tc_max_diff;
+	bool use_smart_rev;
+	float smart_rev_max_duty;
+	float smart_rev_ramp_time;
 } chuk_config;
 
 // NRF Datatypes
@@ -650,6 +653,10 @@ typedef enum {
 	COMM_BM_MAP_PINS_NRF5X,
 	COMM_ERASE_BOOTLOADER,
 	COMM_ERASE_BOOTLOADER_ALL_CAN,
+	COMM_PLOT_INIT,
+	COMM_PLOT_DATA,
+	COMM_PLOT_ADD_GRAPH,
+	COMM_PLOT_SET_GRAPH,
 } COMM_PACKET_ID;
 
 // CAN commands

imu/bmi160_wrapper.c

@@ -54,12 +54,12 @@ static bool reset_init_bmi(BMI_STATE *s) {
 
 	bmi160_init(&(s->sensor));
 
-	s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_100HZ;
+	s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_200HZ;
 	s->sensor.accel_cfg.range = BMI160_ACCEL_RANGE_16G;
 	s->sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
 	s->sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
 
-	s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_100HZ;
+	s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_200HZ;
 	s->sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
 	s->sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
 	s->sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;

imu/icm20948.c

@@ -69,6 +69,11 @@ void icm20948_stop(ICM20948_STATE *s) {
 	while(s->is_running) {
 		chThdSleep(1);
 	}
+
+	if (s == m_terminal_state) {
+		terminal_unregister_callback(terminal_read_reg);
+		m_terminal_state = 0;
+	}
 }
 
 static void terminal_read_reg(int argc, const char **argv) {

imu/mpu9150.c

@@ -296,6 +296,7 @@ static THD_FUNCTION(mpu_thread, arg) {
 	for(;;) {
 		if (should_stop) {
 			is_running = false;
+			mpu_tp = 0;
 			return;
 		}
 

mc_interface.c

@@ -61,6 +61,10 @@ static volatile float m_motor_id_sum;
 static volatile float m_motor_iq_sum;
 static volatile float m_motor_id_iterations;
 static volatile float m_motor_iq_iterations;
+static volatile float m_motor_vd_sum;
+static volatile float m_motor_vq_sum;
+static volatile float m_motor_vd_iterations;
+static volatile float m_motor_vq_iterations;
 static volatile float m_amp_seconds;
 static volatile float m_amp_seconds_charged;
 static volatile float m_watt_seconds;
@@ -121,6 +125,10 @@ void mc_interface_init(mc_configuration *configuration) {
 	m_motor_iq_sum = 0.0;
 	m_motor_id_iterations = 0.0;
 	m_motor_iq_iterations = 0.0;
+	m_motor_vd_sum = 0.0;
+	m_motor_vq_sum = 0.0;
+	m_motor_vd_iterations = 0.0;
+	m_motor_vq_iterations = 0.0;
 	m_amp_seconds = 0.0;
 	m_amp_seconds_charged = 0.0;
 	m_watt_seconds = 0.0;
@@ -1055,6 +1063,32 @@ float mc_interface_read_reset_avg_iq(void) {
 	return DIR_MULT * res;
 }
 
+/**
+ * Read and reset the average direct axis motor voltage. (FOC only)
+ *
+ * @return
+ * The average D axis voltage.
+ */
+float mc_interface_read_reset_avg_vd(void) {
+	float res = m_motor_vd_sum / m_motor_vd_iterations;
+	m_motor_vd_sum = 0.0;
+	m_motor_vd_iterations = 0.0;
+	return DIR_MULT * res; // TODO: DIR_MULT?
+}
+
+/**
+ * Read and reset the average quadrature axis motor voltage. (FOC only)
+ *
+ * @return
+ * The average Q axis voltage.
+ */
+float mc_interface_read_reset_avg_vq(void) {
+	float res = m_motor_vq_sum / m_motor_vq_iterations;
+	m_motor_vq_sum = 0.0;
+	m_motor_vq_iterations = 0.0;
+	return DIR_MULT * res;
+}
+
 float mc_interface_get_pid_pos_set(void) {
 	return m_position_set;
 }
@@ -1428,6 +1462,11 @@ void mc_interface_mc_timer_isr(void) {
 	m_motor_id_iterations++;
 	m_motor_iq_iterations++;
 
+	m_motor_vd_sum += mcpwm_foc_get_vd();
+	m_motor_vq_sum += mcpwm_foc_get_vq();
+	m_motor_vd_iterations++;
+	m_motor_vq_iterations++;
+
 	float abs_current = mc_interface_get_tot_current();
 	float abs_current_filtered = current;
 	if (m_conf.motor_type == MOTOR_TYPE_FOC) {

mc_interface.h

@@ -67,6 +67,8 @@ float mc_interface_read_reset_avg_motor_current(void);
 float mc_interface_read_reset_avg_input_current(void);
 float mc_interface_read_reset_avg_id(void);
 float mc_interface_read_reset_avg_iq(void);
+float mc_interface_read_reset_avg_vd(void);
+float mc_interface_read_reset_avg_vq(void);
 float mc_interface_get_pid_pos_set(void);
 float mc_interface_get_pid_pos_now(void);
 float mc_interface_get_last_sample_adc_isr_duration(void);

mcpwm_foc.c

@@ -1903,16 +1903,6 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 			control_duty = true;
 			duty_set = 0.0;
 			was_full_brake = true;
-
-			// Reverse test
-//			if (!was_full_brake) {
-//				duty_set = 0.0;
-//				was_full_brake = true;
-//			} else {
-//				if (fabsf(duty_set) < 0.1) {
-//					duty_set -= 0.1 * dt * (m_conf->m_invert_direction ? -1.0 : 1.0);
-//				}
-//			}
 		} else {
 			was_full_brake = false;
 		}