Added pid offset and resistance measurement extensions, made resistan…

…ce measurement ramp faster

lispBM/README.md

@@ -1212,6 +1212,26 @@ Example:
 ; Print the result when done
 ```
 
+#### conf-set-pid-offset
+
+```clj
+(conf-set-pid-offset offset optStore)
+```
+
+Set the PID controller offset such that the current angle becomes offset. This can be used in position control applications when e.g. homing against a limit switch. The optional argument optStore can be set to 1 to store the offset persistently (although that requires stopping the motor).
+
+#### conf-measure-res
+
+```clj
+(conf-measure-res current optSamples)
+```
+
+Measure motor resistance with current. The optional argument optSamples sets the number of samples to use (default 100).
+
+This command is useful to update the configuration before starting the motor as the resistance is the most important when it comes to sensorless low-speed performance. It is also useful to sanity check if the motor is shorted out or if a connector is loose. Such faults cause a relatively significant change in resistance. Changes with more than 50% compared to the detected value are most likely faults.
+
+**NOTE:** Phase filters are required to get accurate resistance measurements, so resistance-based fault detection is not as useful on hardware without phase filters.
+
 ### EEPROM (Nonvolatile Storage)
 
 Up to 128 variables (int32 or float) can be stored in a nonvolatile memory reserved for LispBM. These variables persist between power cycles and configuration changes, but not between firmware updates. Keep in mind that the motor will be stopped briefly when writing them and that they only can be written a limited number of times (about 100 000 writes) before wear on the flash memory starts to become an issue.

lispBM/lispif_vesc_extensions.c

@@ -3033,6 +3033,68 @@ static lbm_value ext_conf_detect_foc(lbm_value *args, lbm_uint argn) {
 	return ENC_SYM_TRUE;
 }
 
+static lbm_value ext_conf_set_pid_offset(lbm_value *args, lbm_uint argn) {
+	if (argn != 1 && argn != 2) {
+		lbm_set_error_reason(error_reason_argn);
+		return ENC_SYM_EERROR;
+	}
+
+	CHECK_NUMBER_ALL();
+
+	float angle = lbm_dec_as_float(args[0]);
+	if (angle < -360.0 || angle > 360.0) {
+		lbm_set_error_reason("Invalid angle. Range should be -360 to 360.");
+		return ENC_SYM_EERROR;
+	}
+
+	bool store = false;
+	if (argn == 2) {
+		store = lbm_dec_as_u32(args[1]);
+	}
+
+	mc_interface_update_pid_pos_offset(angle, store);
+
+	return ENC_SYM_TRUE;
+}
+
+typedef struct {
+	float current;
+	int samples;
+	lbm_cid id;
+} measure_res_args;
+
+static void measure_res_task(void *arg) {
+	measure_res_args *a = (measure_res_args*)arg;
+	float res = mcpwm_foc_measure_resistance(a->current, a->samples, true);
+	lbm_unblock_ctx(a->id, lbm_enc_float(res));
+}
+
+static lbm_value ext_conf_measure_res(lbm_value *args, lbm_uint argn) {
+	if (argn != 1 && argn != 2) {
+		lbm_set_error_reason(error_reason_argn);
+		return ENC_SYM_EERROR;
+	}
+
+	CHECK_NUMBER_ALL();
+
+	if (mc_interface_get_configuration()->motor_type != MOTOR_TYPE_FOC) {
+		lbm_set_error_reason("Motor type must be FOC");
+		return ENC_SYM_EERROR;
+	}
+
+	static measure_res_args a;
+	a.current = lbm_dec_as_float(args[0]);
+	a.samples = 100;
+	if (argn == 2) {
+		a.samples = lbm_dec_as_u32(args[1]);
+	}
+	a.id = lbm_get_current_cid();
+
+	worker_execute(measure_res_task, &a);
+	lbm_block_ctx_from_extension();
+	return ENC_SYM_TRUE;
+}
+
 // Extra array extensions
 
 static lbm_value ext_bufclear(lbm_value *args, lbm_uint argn) {
@@ -3598,6 +3660,8 @@ void lispif_load_vesc_extensions(void) {
 	lbm_add_extension("conf-get", ext_conf_get);
 	lbm_add_extension("conf-store", ext_conf_store);
 	lbm_add_extension("conf-detect-foc", ext_conf_detect_foc);
+	lbm_add_extension("conf-set-pid-offset", ext_conf_set_pid_offset);
+	lbm_add_extension("conf-measure-res", ext_conf_measure_res);
 
 	// Array extensions
 	lbm_array_extensions_init();

motor/mcpwm_foc.c

@@ -1679,7 +1679,7 @@ float mcpwm_foc_measure_resistance(float current, int samples, bool stop_after)
 
 	// Ramp up the current slowly
 	while (fabsf(motor->m_iq_set - current) > 0.001) {
-		utils_step_towards((float*)&motor->m_iq_set, current, fabsf(current) / 500.0);		
+		utils_step_towards((float*)&motor->m_iq_set, current, fabsf(current) / 100.0);
 		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
 			motor->m_id_set = 0.0;
 			motor->m_iq_set = 0.0;
@@ -1697,7 +1697,7 @@ float mcpwm_foc_measure_resistance(float current, int samples, bool stop_after)
 	}
 
 	// Wait for the current to rise and the motor to lock.
-	chThdSleepMilliseconds(100);
+	chThdSleepMilliseconds(50);
 
 	// Sample
 	motor->m_samples.avg_current_tot = 0.0;