More hall sensor tweaks

CHANGELOG.md

@@ -22,6 +22,7 @@
 	* Added flat value support.
 	* Added recv-to with a timeout argument.
 	* Added remote message extensions.
+	* Added phase-hall extension.
 * Hall sensors improvements:
 	* Smooth transition to sensorless.
 	* Bug fix in interpolation.

conf_general.h

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

datatypes.h

@@ -181,7 +181,8 @@ typedef enum {
 	DISP_POS_MODE_ENCODER,
 	DISP_POS_MODE_PID_POS,
 	DISP_POS_MODE_PID_POS_ERROR,
-	DISP_POS_MODE_ENCODER_OBSERVER_ERROR
+	DISP_POS_MODE_ENCODER_OBSERVER_ERROR,
+	DISP_POS_MODE_HALL_OBSERVER_ERROR
 } disp_pos_mode;
 
 typedef enum {

lispBM/README.md

@@ -1353,6 +1353,20 @@ Returns the encoder position mapped to the electrical position of the motor. Uni
 
 ---
 
+#### phase-hall
+
+| Platforms | Firmware |
+|---|---|
+| ESC | 6.05+ |
+
+```clj
+(phase-hall)
+```
+
+Returns the hall sensor position of the motor. Unit: Degrees.
+
+---
+
 #### phase-observer
 
 | Platforms | Firmware |

lispBM/lispif_vesc_extensions.c

@@ -1657,6 +1657,11 @@ static lbm_value ext_phase_encoder(lbm_value *args, lbm_uint argn) {
 	return lbm_enc_float(mcpwm_foc_get_phase_encoder());
 }
 
+static lbm_value ext_phase_hall(lbm_value *args, lbm_uint argn) {
+	(void)args; (void)argn;
+	return lbm_enc_float(mcpwm_foc_get_phase_hall());
+}
+
 static lbm_value ext_phase_observer(lbm_value *args, lbm_uint argn) {
 	(void)args; (void)argn;
 	return lbm_enc_float(mcpwm_foc_get_phase_observer());
@@ -4268,6 +4273,7 @@ void lispif_load_vesc_extensions(void) {
 	lbm_add_extension("pos-pid-error", ext_pos_pid_error);
 	lbm_add_extension("phase-motor", ext_phase_motor);
 	lbm_add_extension("phase-encoder", ext_phase_encoder);
+	lbm_add_extension("phase-hall", ext_phase_hall);
 	lbm_add_extension("phase-observer", ext_phase_observer);
 	lbm_add_extension("observer-error", ext_observer_error);
 

main.c

@@ -187,6 +187,10 @@ static THD_FUNCTION(periodic_thread, arg) {
 				commands_send_rotor_pos(utils_angle_difference(mcpwm_foc_get_phase_observer(), mcpwm_foc_get_phase_encoder()));
 				break;
 
+			case DISP_POS_MODE_HALL_OBSERVER_ERROR:
+				commands_send_rotor_pos(utils_angle_difference(mcpwm_foc_get_phase_observer(), mcpwm_foc_get_phase_hall()));
+				break;
+
 			default:
 				break;
 			}

motor/foc_math.c

@@ -544,7 +544,7 @@ float foc_correct_hall(float angle, float dt, motor_all_state_t *motor, int hall
 	mc_configuration *conf_now = motor->m_conf;
 	motor->m_hall_dt_diff_now += dt;
 
-	float rad_per_sec = motor->m_speed_est_fast;
+	float rad_per_sec = motor->m_speed_est_fast_corrected;
 	float rpm_abs = fabsf(RADPS2RPM_f(motor->m_pll_speed));
 	float rpm_abs_hall = fabsf(RADPS2RPM_f(rad_per_sec));
 

motor/foc_math.h

@@ -131,6 +131,7 @@ typedef struct {
 	float m_pll_phase;
 	float m_pll_speed;
 	float m_speed_est_fast;
+	float m_speed_est_fast_corrected; // Same as m_speed_est_fast, but always based on the corrected position
 	float m_speed_est_faster;
 	mc_sample_t m_samples;
 	int m_tachometer;
@@ -164,6 +165,7 @@ typedef struct {
 	float m_x1_prev;
 	float m_x2_prev;
 	float m_phase_before_speed_est;
+	float m_phase_before_speed_est_corrected;
 	int m_tacho_step_last;
 	float m_pid_div_angle_last;
 	float m_pid_div_angle_accumulator;

motor/mcconf_default.h

@@ -488,7 +488,7 @@
 #define MCCONF_FOC_FW_Q_CURRENT_FACTOR	0.02 // Factor of the FW-current to feed to the Q-axis to slow motor down when setting 0 current
 #endif
 #ifndef MCCONF_FOC_SPEED_SOURCE
-#define MCCONF_FOC_SPEED_SOURCE			FOC_SPEED_SRC_OBSERVER // Position source for speed trackers
+#define MCCONF_FOC_SPEED_SOURCE			FOC_SPEED_SRC_CORRECTED // Position source for speed trackers
 #endif
 
 // GPD

motor/mcpwm_foc.c

@@ -1341,42 +1341,30 @@ int mcpwm_foc_get_tachometer_abs_value(bool reset) {
 	return val;
 }
 
-/**
- * Read the motor phase.
- *
- * @return
- * The phase angle in degrees.
- */
 float mcpwm_foc_get_phase(void) {
 	float angle = RAD2DEG_f(get_motor_now()->m_motor_state.phase);
 	utils_norm_angle(&angle);
 	return angle;
 }
 
-/**
- * Read the phase that the observer has calculated.
- *
- * @return
- * The phase angle in degrees.
- */
 float mcpwm_foc_get_phase_observer(void) {
 	float angle = RAD2DEG_f(get_motor_now()->m_phase_now_observer);
 	utils_norm_angle(&angle);
 	return angle;
 }
 
-/**
- * Read the phase from based on the encoder.
- *
- * @return
- * The phase angle in degrees.
- */
 float mcpwm_foc_get_phase_encoder(void) {
 	float angle = RAD2DEG_f(get_motor_now()->m_phase_now_encoder);
 	utils_norm_angle(&angle);
 	return angle;
 }
 
+float mcpwm_foc_get_phase_hall(void) {
+	float angle = RAD2DEG_f(get_motor_now()->m_ang_hall_rate_limited);
+	utils_norm_angle(&angle);
+	return angle;
+}
+
 float mcpwm_foc_get_vd(void) {
 	return get_motor_now()->m_motor_state.vd;
 }
@@ -2966,9 +2954,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 				}
 				break;
 			case FOC_SENSOR_MODE_HALL:
-				motor_now->m_phase_now_observer = foc_correct_hall(motor_now->m_phase_now_observer, dt, motor_now,
+				motor_now->m_motor_state.phase = foc_correct_hall(motor_now->m_phase_now_observer, dt, motor_now,
 						utils_read_hall(motor_now != &m_motor_1, conf_now->m_hall_extra_samples));
-				motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
 
 				if (!motor_now->m_phase_override && motor_now->m_control_mode != CONTROL_MODE_OPENLOOP_PHASE) {
 					id_set_tmp = 0.0;
@@ -3153,9 +3140,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 						motor_now);
 				break;
 			case FOC_SENSOR_MODE_HALL:
-				motor_now->m_phase_now_observer = foc_correct_hall(motor_now->m_phase_now_observer, dt, motor_now,
+				motor_now->m_motor_state.phase = foc_correct_hall(motor_now->m_phase_now_observer, dt, motor_now,
 						utils_read_hall(motor_now != &m_motor_1, conf_now->m_hall_extra_samples));
-				motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
 				break;
 			case FOC_SENSOR_MODE_SENSORLESS:
 				motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
@@ -3261,10 +3247,17 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 		UTILS_LP_FAST(motor_now->m_speed_est_faster, diff / dt, 0.2);
 		UTILS_NAN_ZERO(motor_now->m_speed_est_faster);
 
+		float diff_corr = utils_angle_difference_rad(motor_now->m_motor_state.phase, motor_now->m_phase_before_speed_est_corrected);
+		utils_truncate_number(&diff_corr, -M_PI / 3.0, M_PI / 3.0);
+
+		UTILS_LP_FAST(motor_now->m_speed_est_fast_corrected, diff_corr / dt, 0.01);
+		UTILS_NAN_ZERO(motor_now->m_speed_est_fast_corrected);
+
 		// pll wind-up protection
 		utils_truncate_number_abs((float*)&motor_now->m_pll_speed, fabsf(motor_now->m_speed_est_fast) * 3.0);
 
 		motor_now->m_phase_before_speed_est = phase_for_speed_est;
+		motor_now->m_phase_before_speed_est_corrected = motor_now->m_motor_state.phase;
 	}
 
 	// Update tachometer (resolution = 60 deg as for BLDC)

motor/mcpwm_foc.h

@@ -75,6 +75,7 @@ int mcpwm_foc_get_tachometer_abs_value(bool reset);
 float mcpwm_foc_get_phase(void);
 float mcpwm_foc_get_phase_observer(void);
 float mcpwm_foc_get_phase_encoder(void);
+float mcpwm_foc_get_phase_hall(void);
 float mcpwm_foc_get_vd(void);
 float mcpwm_foc_get_vq(void);
 float mcpwm_foc_get_mod_alpha_raw(void);