New mechanism for disabling the motor regardless of app/package used

comm/commands.c

@@ -475,6 +475,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 			uint8_t status = 0;
 			status |= timeout_has_timeout();
 			status |= timeout_kill_sw_active() << 1;
+			status |= mc_interface_is_disabled() << 2;
 			send_buffer[ind++] = status;
 		}
 
@@ -1653,6 +1654,17 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		mc_interface_release_motor_override_both();
 	} break;
 
+	case COMM_DISABLE: {
+		int32_t ind = 0;
+		uint8_t disable = data[ind++];
+
+		if (disable) {
+			mc_interface_disable();
+		} else {
+			mc_interface_enable();
+		}
+	} 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.c

@@ -140,6 +140,8 @@ __attribute__((section(".text2"))) void conf_general_init(void) {
 			backup_tmp.can_baud = APPCONF_CAN_BAUD_RATE;
 			backup_tmp.can_id = HW_DEFAULT_ID;
 		}
+
+		backup_tmp.motor_disabled = g_backup.motor_disabled;
 	}
 
 	backup_tmp.odometer_init_flag = BACKUP_VAR_INIT_CODE;

datatypes.h

@@ -1140,6 +1140,8 @@ typedef enum {
 	COMM_CAN_UPDATE_BAUD_ALL				= 158,
 
 	COMM_MOTOR_ESTOP						= 159,
+
+	COMM_DISABLE							= 160,
 } COMM_PACKET_ID;
 
 // CAN commands
@@ -1464,7 +1466,10 @@ typedef struct __attribute__((packed)) {
 	uint8_t can_baud;
 	uint8_t can_id;
 
-	uint8_t dummy;
+	// Motor disable state
+	uint8_t motor_disabled;
+
+	uint8_t dummy[2];
 } backup_data;
 
 #endif /* DATATYPES_H_ */

lispBM/README.md

@@ -1262,6 +1262,48 @@ Set kill switch state. When state is set to 1 the motor is disabled and optional
 
 ---
 
+#### disable-motor
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 7.01+ |
+
+```clj
+(disable-motor)
+```
+
+Disable the motor and persist this state across reboots. Only works while the motor is stationary. Returns t on success and nil if the motor is currently spinning.
+
+---
+
+#### enable-motor
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 7.01+ |
+
+```clj
+(enable-motor)
+```
+
+Re-enable the motor after disable-motor.
+
+---
+
+#### is-motor-disabled
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 7.01+ |
+
+```clj
+(is-motor-disabled)
+```
+
+Check whether the motor is currently disabled.
+
+---
+
 #### foc-beep
 
 | Platforms | Firmware |

lispBM/c_libs/vesc_c_if.h

@@ -672,6 +672,16 @@ typedef struct {
 
 	// Functions below were added in firmware 7.00
 	void (*foc_set_fw_override)(float current);
+
+	// Functions below were added in firmware 7.01
+
+	// Disable the motor and persist this state across reboots. Only works
+	// while the motor is stationary. Returns false if rejected.
+	bool (*mc_disable)(void);
+	// Re-enable the motor after mc_disable.
+	void (*mc_enable)(void);
+	// Check whether the motor is currently disabled.
+	bool (*mc_is_disabled)(void);
 } vesc_c_if;
 
 typedef struct {

lispBM/lispif_c_lib.c

@@ -1078,6 +1078,11 @@ lbm_value ext_load_native_lib(lbm_value *args, lbm_uint argn) {
 		// 7.00
 		cif.cif.foc_set_fw_override = mcpwm_foc_set_fw_override;
 
+		// 7.01
+		cif.cif.mc_disable = mc_interface_disable;
+		cif.cif.mc_enable = mc_interface_enable;
+		cif.cif.mc_is_disabled = mc_interface_is_disabled;
+
 		lib_init_done = true;
 	}
 

lispBM/lispif_vesc_extensions.c

@@ -1996,6 +1996,22 @@ static lbm_value ext_set_kill_sw(lbm_value *args, lbm_uint argn) {
 	return ENC_SYM_TRUE;
 }
 
+static lbm_value ext_disable_motor(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return mc_interface_disable() ? ENC_SYM_TRUE : ENC_SYM_NIL;
+}
+
+static lbm_value ext_enable_motor(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	mc_interface_enable();
+	return ENC_SYM_TRUE;
+}
+
+static lbm_value ext_is_motor_disabled(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return mc_interface_is_disabled() ? ENC_SYM_TRUE : ENC_SYM_NIL;
+}
+
 static lbm_value ext_foc_beep(lbm_value *args, lbm_uint argn) {
 	LBM_CHECK_ARGN_NUMBER(3);
 	timeout_reset();
@@ -6515,6 +6531,9 @@ void lispif_load_vesc_extensions(bool main_found) {
 		lbm_add_extension("foc-openloop-phase", ext_foc_openloop_phase);
 //		lbm_add_extension("foc-set-fw-override", ext_foc_set_fw_override);
 		lbm_add_extension("set-kill-sw", ext_set_kill_sw);
+		lbm_add_extension("disable-motor", ext_disable_motor);
+		lbm_add_extension("enable-motor", ext_enable_motor);
+		lbm_add_extension("is-motor-disabled", ext_is_motor_disabled);
 
 		lbm_add_extension("foc-beep", ext_foc_beep);
 		lbm_add_extension("foc-play-tone", ext_foc_play_tone);

motor/mc_interface.c

@@ -262,6 +262,13 @@ void mc_interface_init(bool reset_conf) {
 	}
 
 	bms_init((bms_config*)&m_motor_1.m_conf.bms);
+
+	if (mc_interface_is_disabled()) {
+		m_motor_1.m_lock_enabled = true;
+#ifdef HW_HAS_DUAL_MOTORS
+		m_motor_2.m_lock_enabled = true;
+#endif
+	}
 }
 
 int mc_interface_motor_now(void) {
@@ -1733,6 +1740,56 @@ void mc_interface_ignore_input(int time_ms) {
 	motor->m_ignore_iterations = time_ms;
 }
 
+/**
+ * Disable the motor and persist the disabled state across reboots. The
+ * motor can only be disabled while it is stationary.
+ *
+ * @return
+ * true if the motor was disabled, false if the request was refused because
+ * the motor is currently spinning.
+ */
+bool mc_interface_disable(void) {
+	if (fabsf(mc_interface_get_rpm()) > 100) {
+		return false;
+	}
+
+	mc_interface_ignore_input_both(5000);
+	mc_interface_release_motor_override_both();
+
+	m_motor_1.m_lock_enabled = true;
+#ifdef HW_HAS_DUAL_MOTORS
+	m_motor_2.m_lock_enabled = true;
+#endif
+
+	g_backup.motor_disabled = 1;
+	conf_general_store_backup_data();
+
+	return true;
+}
+
+/**
+ * Re-enable the motor after mc_interface_disable() and clear the persisted
+ * disabled state.
+ */
+void mc_interface_enable(void) {
+	m_motor_1.m_lock_enabled = false;
+#ifdef HW_HAS_DUAL_MOTORS
+	m_motor_2.m_lock_enabled = false;
+#endif
+
+	g_backup.motor_disabled = 0;
+	conf_general_store_backup_data();
+}
+
+/**
+ * @return
+ * true if the motor has been disabled with mc_interface_disable() (persists
+ * across reboots).
+ */
+bool mc_interface_is_disabled(void) {
+	return g_backup.motor_disabled != 0;
+}
+
 /**
  * Ignore motor control commands for this amount of time on both motors.
  */

motor/mc_interface.h

@@ -37,6 +37,9 @@ void mc_interface_set_pwm_callback(void (*p_func)(void));
 void mc_interface_lock(void);
 void mc_interface_unlock(void);
 void mc_interface_lock_override_once(void);
+bool mc_interface_disable(void);
+void mc_interface_enable(void);
+bool mc_interface_is_disabled(void);
 mc_fault_code mc_interface_get_fault(void);
 const char* mc_interface_fault_to_string(mc_fault_code fault);
 mc_state mc_interface_get_state(void);

terminal.c

@@ -1158,6 +1158,17 @@ __attribute__((section(".text2"))) void terminal_process_string(char *str) {
 		commands_printf("User Git Hash  : %s", USER_GIT_COMMIT_HASH);
 #endif
 		commands_printf(" ");
+	} else if (strcmp(argv[0], "motor_disable") == 0) {
+		if (mc_interface_disable()) {
+			commands_printf("Motor disabled.\n");
+		} else {
+			commands_printf("Could not disable motor: it is currently spinning.\n");
+		}
+	} else if (strcmp(argv[0], "motor_enable") == 0) {
+		mc_interface_enable();
+		commands_printf("Motor enabled.\n");
+	} else if (strcmp(argv[0], "motor_is_disabled") == 0) {
+		commands_printf("Motor disabled: %s\n", mc_interface_is_disabled() ? "Yes" : "No");
 	} else if (strcmp(argv[0], "rebootwdt") == 0) {
 		chSysLock();
 		for (;;) {__NOP();}
@@ -1279,6 +1290,16 @@ __attribute__((section(".text2"))) void terminal_process_string(char *str) {
 		commands_printf("fw_info");
 		commands_printf("  Print detailed firmware info.");
 
+		commands_printf("motor_disable");
+		commands_printf("  Disable the motor and persist this state across reboots. Only");
+		commands_printf("  works while the motor is stationary.");
+
+		commands_printf("motor_enable");
+		commands_printf("  Re-enable the motor after motor_disable.");
+
+		commands_printf("motor_is_disabled");
+		commands_printf("  Print whether the motor is currently disabled.");
+
 		commands_printf("rebootwdt");
 		commands_printf("  Reboot using the watchdog timer.");