Added foc_beep command to lisp

conf_general.h

@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			6
 #define FW_VERSION_MINOR			00
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		51
+#define FW_TEST_VERSION_NUMBER		52
 
 #include "datatypes.h"
 

lispBM/README.md

@@ -388,6 +388,13 @@ Position control. Set motor position in degrees, range 0.0 to 360.0.
 
 Run FOC in open loop. Useful to test thermal properties of motors and power stages.
 
+#### foc-beep
+```clj
+(foc-beep freq time voltage)
+```
+
+Use the motor to play a beep sound at frequency freq for time seconds using voltage excitation voltage. The frequency can be set between 380 Hz and 3700 Hz.
+
 ### Motor Get Commands
 
 #### get-current

lispBM/lispif_vesc_extensions.c

@@ -1056,6 +1056,13 @@ static lbm_value ext_foc_openloop(lbm_value *args, lbm_uint argn) {
 	return lbm_enc_sym(SYM_TRUE);
 }
 
+static lbm_value ext_foc_beep(lbm_value *args, lbm_uint argn) {
+	CHECK_ARGN_NUMBER(3);
+	timeout_reset();
+	mcpwm_foc_beep(lbm_dec_as_float(args[0]), lbm_dec_as_float(args[1]), lbm_dec_as_float(args[2]));
+	return lbm_enc_sym(SYM_TRUE);
+}
+
 // Motor get commands
 
 static lbm_value ext_get_current(lbm_value *args, lbm_uint argn) {
@@ -3296,7 +3303,7 @@ void lispif_load_vesc_extensions(void) {
 	lbm_add_extension("eeprom-read-i", ext_eeprom_read_i);
 	lbm_add_extension("sysinfo", ext_sysinfo);
 
-	//APP commands
+	// APP commands
 	lbm_add_extension("app-adc-detach", ext_app_adc_detach);
 	lbm_add_extension("app-adc-override", ext_app_adc_override);
 
@@ -3311,6 +3318,7 @@ void lispif_load_vesc_extensions(void) {
 	lbm_add_extension("set-rpm", ext_set_rpm);
 	lbm_add_extension("set-pos", ext_set_pos);
 	lbm_add_extension("foc-openloop", ext_foc_openloop);
+	lbm_add_extension("foc-beep", ext_foc_beep);
 
 	// Motor get commands
 	lbm_add_extension("get-current", ext_get_current);

motor/mcpwm_foc.c

@@ -1916,6 +1916,67 @@ float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *
 	return ind;
 }
 
+bool mcpwm_foc_beep(float freq, float time, float voltage) {
+	volatile motor_all_state_t *motor = get_motor_now();
+
+	mc_foc_sensor_mode sensor_mode_old = motor->m_conf->foc_sensor_mode;
+	float f_zv_old = motor->m_conf->foc_f_zv;
+	float hfi_voltage_start_old = motor->m_conf->foc_hfi_voltage_start;
+	float hfi_voltage_run_old = motor->m_conf->foc_hfi_voltage_run;
+	float hfi_voltage_max_old = motor->m_conf->foc_hfi_voltage_max;
+	float sl_erpm_hfi_old = motor->m_conf->foc_sl_erpm_hfi;
+	bool sample_v0_v7_old = motor->m_conf->foc_sample_v0_v7;
+	foc_hfi_samples samples_old = motor->m_conf->foc_hfi_samples;
+	uint16_t start_samples_old = motor->m_conf->foc_hfi_start_samples;
+
+	mc_interface_lock();
+	motor->m_control_mode = CONTROL_MODE_NONE;
+	motor->m_state = MC_STATE_OFF;
+	stop_pwm_hw((motor_all_state_t*)motor);
+
+	motor->m_conf->foc_sensor_mode = FOC_SENSOR_MODE_HFI;
+	motor->m_conf->foc_hfi_voltage_start = voltage;
+	motor->m_conf->foc_hfi_voltage_run = voltage;
+	motor->m_conf->foc_hfi_voltage_max = voltage;
+	motor->m_conf->foc_sl_erpm_hfi = 20000.0;
+	motor->m_conf->foc_sample_v0_v7 = false;
+	motor->m_conf->foc_hfi_samples = HFI_SAMPLES_8;
+	motor->m_conf->foc_hfi_start_samples = 10;
+
+	motor->m_conf->foc_f_zv = freq * 8.0;
+	utils_truncate_number(&motor->m_conf->foc_f_zv, 3.0e3, 30.0e3);
+
+	mcpwm_foc_set_configuration(motor->m_conf);
+
+	chThdSleepMilliseconds(1);
+
+	timeout_reset();
+	mcpwm_foc_set_duty(0.0);
+
+	int ms_sleep = (time * 1000.0) - 1;
+	if (ms_sleep > 0) {
+		chThdSleepMilliseconds(ms_sleep);
+	}
+
+	mcpwm_foc_set_current(0.0);
+
+	motor->m_conf->foc_sensor_mode = sensor_mode_old;
+	motor->m_conf->foc_f_zv = f_zv_old;
+	motor->m_conf->foc_hfi_voltage_start = hfi_voltage_start_old;
+	motor->m_conf->foc_hfi_voltage_run = hfi_voltage_run_old;
+	motor->m_conf->foc_hfi_voltage_max = hfi_voltage_max_old;
+	motor->m_conf->foc_sl_erpm_hfi = sl_erpm_hfi_old;
+	motor->m_conf->foc_sample_v0_v7 = sample_v0_v7_old;
+	motor->m_conf->foc_hfi_samples = samples_old;
+	motor->m_conf->foc_hfi_start_samples = start_samples_old;
+
+	mcpwm_foc_set_configuration(motor->m_conf);
+
+	mc_interface_unlock();
+
+	return true;
+}
+
 /**
  * Automatically measure the resistance and inductance of the motor with small steps.
  *

motor/mcpwm_foc.h

@@ -83,6 +83,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
 float mcpwm_foc_measure_resistance(float current, int samples, bool stop_after);
 float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float *ld_lq_diff);
 float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr, float *ld_lq_diff);
+bool mcpwm_foc_beep(float freq, float time, float voltage);
 bool mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff);
 bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table);
 int mcpwm_foc_dc_cal(bool cal_undriven);