Added kill switch support

kubark42 · Jun 13, 2021 · 75b84f1 · 75b84f1
1 parent 5742d1b
commit 75b84f1
Show file tree

Hide file tree

Showing 16 changed files with 142 additions and 41 deletions.
diff --git a/CHANGELOG b/CHANGELOG
@@ -27,6 +27,7 @@
 * Dynamic QML-script write support.
 * Use fast speed tracker for current controller.
 * Disable motor for 5 seconds after flash operations.
+* Added kill switch support.
 
 === FW 5.02 ===
 * IMU calibration improvement.

diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h
@@ -45,6 +45,9 @@
 #ifndef APPCONF_SERVO_OUT_ENABLE
 #define APPCONF_SERVO_OUT_ENABLE			false
 #endif
+#ifndef APPCONF_KILL_SW_MODE
+#define APPCONF_KILL_SW_MODE				KILL_SW_MODE_DISABLED
+#endif
 #ifndef APPCONF_SEND_CAN_STATUS_RATE_HZ
 #define APPCONF_SEND_CAN_STATUS_RATE_HZ		50
 #endif

diff --git a/commands.c b/commands.c
@@ -426,6 +426,12 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		if (mask & ((uint32_t)1 << 20)) {
 			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_vq(), 1e3, &ind);
 		}
+		if (mask & ((uint32_t)1 << 21)) {
+			uint8_t status = 0;
+			status |= timeout_has_timeout();
+			status |= timeout_kill_sw_active() << 1;
+			send_buffer[ind++] = status;
+		}
 
 		reply_func(send_buffer, ind);
 		chMtxUnlock(&send_buffer_mutex);
@@ -565,7 +571,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 
 			conf_general_store_app_configuration(appconf);
 			app_set_configuration(appconf);
-			timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current);
+			timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current, appconf->kill_sw_mode);
 			chThdSleepMilliseconds(200);
 
 			int32_t ind = 0;
@@ -1122,6 +1128,16 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 			buffer_append_float32_auto(send_buffer, q[3], &ind);
 		}
 
+		if (mask & ((uint32_t)1 << 16)) {
+			uint8_t current_controller_id = app_get_configuration()->controller_id;
+#ifdef HW_HAS_DUAL_MOTORS
+			if (mc_interface_get_motor_thread() == 2) {
+				current_controller_id = utils_second_motor_id();
+			}
+#endif
+			send_buffer[ind++] = current_controller_id;
+		}
+
 		reply_func(send_buffer, ind);
 	} break;
 

diff --git a/conf_general.c b/conf_general.c
@@ -531,8 +531,9 @@ bool conf_general_detect_motor_param(float current, float min_rpm, float low_dut
 	// Disable timeout
 	systime_t tout = timeout_get_timeout_msec();
 	float tout_c = timeout_get_brake_current();
+	KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
 	timeout_reset();
-	timeout_configure(60000, 0.0);
+	timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
 
 	mc_interface_lock();
 
@@ -597,7 +598,7 @@ bool conf_general_detect_motor_param(float current, float min_rpm, float low_dut
 
 	if (!started) {
 		mc_interface_set_current(0.0);
-		timeout_configure(tout, tout_c);
+		timeout_configure(tout, tout_c, tout_ksw);
 		mc_interface_set_configuration(mcconf_old);
 		mc_interface_unlock();
 		mempools_free_mcconf(mcconf);
@@ -689,7 +690,7 @@ bool conf_general_detect_motor_param(float current, float min_rpm, float low_dut
 
 	// Restore settings
 	mc_interface_set_configuration(mcconf_old);
-	timeout_configure(tout, tout_c);
+	timeout_configure(tout, tout_c, tout_ksw);
 
 	mc_interface_unlock();
 
@@ -762,8 +763,9 @@ bool conf_general_measure_flux_linkage(float current, float duty,
 	// Disable timeout
 	systime_t tout = timeout_get_timeout_msec();
 	float tout_c = timeout_get_brake_current();
+	KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
 	timeout_reset();
-	timeout_configure(60000, 0.0);
+	timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
 
 	mc_interface_lock();
 
@@ -838,7 +840,7 @@ bool conf_general_measure_flux_linkage(float current, float duty,
 
 	if (!started) {
 		mc_interface_set_current(0.0);
-		timeout_configure(tout, tout_c);
+		timeout_configure(tout, tout_c, tout_ksw);
 		mc_interface_set_configuration(mcconf);
 		mc_interface_unlock();
 		mempools_free_mcconf(mcconf);
@@ -861,7 +863,7 @@ bool conf_general_measure_flux_linkage(float current, float duty,
 		chThdSleepMilliseconds(1.0);
 	}
 
-	timeout_configure(tout, tout_c);
+	timeout_configure(tout, tout_c, tout_ksw);
 	mc_interface_set_configuration(mcconf_old);
 	mc_interface_unlock();
 	mc_interface_set_current(0.0);
@@ -978,8 +980,9 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
 	// Disable timeout
 	systime_t tout = timeout_get_timeout_msec();
 	float tout_c = timeout_get_brake_current();
+	KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
 	timeout_reset();
-	timeout_configure(60000, 0.0);
+	timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
 
 	mc_interface_lock();
 
@@ -1100,7 +1103,7 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
 		result = true;
 	}
 
-	timeout_configure(tout, tout_c);
+	timeout_configure(tout, tout_c, tout_ksw);
 	mc_interface_unlock();
 	mc_interface_release_motor();
 	mc_interface_wait_for_motor_release(1.0);
@@ -1166,8 +1169,9 @@ int conf_general_autodetect_apply_sensors_foc(float current,
 	// Disable timeout
 	systime_t tout = timeout_get_timeout_msec();
 	float tout_c = timeout_get_brake_current();
+	KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
 	timeout_reset();
-	timeout_configure(60000, 0.0);
+	timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
 
 	mc_interface_lock();
 
@@ -1242,7 +1246,7 @@ int conf_general_autodetect_apply_sensors_foc(float current,
 		res = true;
 	}
 
-	timeout_configure(tout, tout_c);
+	timeout_configure(tout, tout_c, tout_ksw);
 	mc_interface_unlock();
 	mc_interface_release_motor();
 	mc_interface_wait_for_motor_release(1.0);
@@ -1531,8 +1535,9 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
 	// Disable timeout
 	systime_t tout = timeout_get_timeout_msec();
 	float tout_c = timeout_get_brake_current();
+	KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
 	timeout_reset();
-	timeout_configure(60000, 0.0);
+	timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
 
 	mc_interface_lock();
 
@@ -1565,7 +1570,7 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
 #endif
 
 	if (!res_r_l_imax_m1 || !res_r_l_imax_m2) {
-		timeout_configure(tout, tout_c);
+		timeout_configure(tout, tout_c, tout_ksw);
 		mc_interface_unlock();
 		mc_interface_release_motor();
 		mc_interface_wait_for_motor_release(1.0);
@@ -1690,7 +1695,7 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
 		result = -10;
 	}
 
-	timeout_configure(tout, tout_c);
+	timeout_configure(tout, tout_c, tout_ksw);
 	mc_interface_lock_override_once();
 	mc_interface_release_motor();
 	mc_interface_wait_for_motor_release(1.0);

diff --git a/conf_general.h b/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		38
+#define FW_TEST_VERSION_NUMBER		39
 
 #include "datatypes.h"
 

diff --git a/confgenerator.c b/confgenerator.c
@@ -200,6 +200,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
 	buffer[ind++] = (uint8_t)conf->uavcan_esc_index;
 	buffer[ind++] = conf->uavcan_raw_mode;
 	buffer[ind++] = conf->servo_out_enable;
+	buffer[ind++] = conf->kill_sw_mode;
 	buffer[ind++] = conf->app_to_use;
 	buffer[ind++] = conf->app_ppm_conf.ctrl_type;
 	buffer_append_float32_auto(buffer, conf->app_ppm_conf.pid_max_erpm, &ind);
@@ -560,6 +561,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
 	conf->uavcan_esc_index = buffer[ind++];
 	conf->uavcan_raw_mode = buffer[ind++];
 	conf->servo_out_enable = buffer[ind++];
+	conf->kill_sw_mode = buffer[ind++];
 	conf->app_to_use = buffer[ind++];
 	conf->app_ppm_conf.ctrl_type = buffer[ind++];
 	conf->app_ppm_conf.pid_max_erpm = buffer_get_float32_auto(buffer, &ind);
@@ -904,6 +906,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
 	conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX;
 	conf->uavcan_raw_mode = APPCONF_UAVCAN_RAW_MODE;
 	conf->servo_out_enable = APPCONF_SERVO_OUT_ENABLE;
+	conf->kill_sw_mode = APPCONF_KILL_SW_MODE;
 	conf->app_to_use = APPCONF_APP_TO_USE;
 	conf->app_ppm_conf.ctrl_type = APPCONF_PPM_CTRL_TYPE;
 	conf->app_ppm_conf.pid_max_erpm = APPCONF_PPM_PID_MAX_ERPM;

diff --git a/confgenerator.h b/confgenerator.h
@@ -9,7 +9,7 @@
 
 // Constants
 #define MCCONF_SIGNATURE		4213619100
-#define APPCONF_SIGNATURE		470228522
+#define APPCONF_SIGNATURE		2268062315
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

diff --git a/datatypes.h b/datatypes.h
@@ -797,6 +797,14 @@ typedef enum {
 	UAVCAN_RAW_MODE_DUTY
 } UAVCAN_RAW_MODE;
 
+typedef enum {
+	KILL_SW_MODE_DISABLED = 0,
+	KILL_SW_MODE_PPM_LOW,
+	KILL_SW_MODE_PPM_HIGH,
+	KILL_SW_MODE_ADC2_LOW,
+	KILL_SW_MODE_ADC2_HIGH
+} KILL_SW_MODE;
+
 typedef struct {
 	// Settings
 	uint8_t controller_id;
@@ -809,6 +817,7 @@ typedef struct {
 	bool permanent_uart_enabled;
 	SHUTDOWN_MODE shutdown_mode;
 	bool servo_out_enable;
+	KILL_SW_MODE kill_sw_mode;
 
 	// CAN modes
 	CAN_MODE can_mode;

diff --git a/flash_helper.c b/flash_helper.c
@@ -120,8 +120,7 @@ uint16_t flash_helper_erase_new_app(uint32_t new_app_size) {
 
 	new_app_size += flash_addr[NEW_APP_BASE];
 
-	mc_interface_unlock();
-	mc_interface_release_motor();
+	mc_interface_release_motor_override();
 	mc_interface_ignore_input_both(5000);
 
 	if (!mc_interface_wait_for_motor_release(3.0)) {
@@ -214,8 +213,7 @@ uint16_t flash_helper_qmlui_flags(void) {
 void flash_helper_jump_to_bootloader(void) {
 	typedef void (*pFunction)(void);
 
-	mc_interface_unlock();
-	mc_interface_release_motor();
+	mc_interface_release_motor_override();
 	usbDisconnectBus(&USBD1);
 	usbStop(&USBD1);
 
@@ -370,8 +368,7 @@ static uint16_t erase_sector(uint32_t sector) {
 	FLASH_ClearFlag(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR |
 			FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
 
-	mc_interface_unlock();
-	mc_interface_release_motor();
+	mc_interface_release_motor_override();
 	mc_interface_ignore_input_both(5000);
 
 	if (!mc_interface_wait_for_motor_release(3.0)) {
@@ -404,8 +401,7 @@ static uint16_t write_data(uint32_t base, uint8_t *data, uint32_t len) {
 	FLASH_ClearFlag(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR |
 			FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
 
-	mc_interface_unlock();
-	mc_interface_release_motor();
+	mc_interface_release_motor_override();
 	mc_interface_ignore_input_both(5000);
 
 	if (!mc_interface_wait_for_motor_release(3.0)) {

diff --git a/main.c b/main.c
@@ -264,7 +264,7 @@ int main(void) {
 	chThdCreateStatic(flash_integrity_check_thread_wa, sizeof(flash_integrity_check_thread_wa), LOWPRIO, flash_integrity_check_thread, NULL);
 
 	timeout_init();
-	timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current);
+	timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current, appconf->kill_sw_mode);
 
 	mempools_free_appconf(appconf);
 

diff --git a/mc_interface.c b/mc_interface.c
@@ -805,6 +805,22 @@ void mc_interface_release_motor(void) {
 	mc_interface_set_current(0.0);
 }
 
+void mc_interface_release_motor_override(void) {
+	switch (motor_now()->m_conf.motor_type) {
+	case MOTOR_TYPE_BLDC:
+	case MOTOR_TYPE_DC:
+		mcpwm_set_current(0.0);
+		break;
+
+	case MOTOR_TYPE_FOC:
+		mcpwm_foc_set_current(0.0);
+		break;
+
+	default:
+		break;
+	}
+}
+
 bool mc_interface_wait_for_motor_release(float timeout) {
 	systime_t time_start = chVTGetSystemTimeX();
 	bool res = false;

diff --git a/mc_interface.h b/mc_interface.h
@@ -52,6 +52,7 @@ void mc_interface_set_handbrake_rel(float val);
 int mc_interface_set_tachometer_value(int steps);
 void mc_interface_brake_now(void);
 void mc_interface_release_motor(void);
+void mc_interface_release_motor_override(void);
 bool mc_interface_wait_for_motor_release(float timeout);
 float mc_interface_get_duty_cycle_set(void);
 float mc_interface_get_duty_cycle_now(void);