From 4499982f993bdc777134b8cf07c5c3fdd040864b Mon Sep 17 00:00:00 2001
From: Benjamin Vedder <benjamin@vedder.se>
Date: Wed, 15 May 2024 13:24:04 +0200
Subject: [PATCH] Added hall sensor rate limiter again, now based on the hall
 toggle rate as in 6.02

---
 motor/foc_math.c  | 17 ++++++++++++++++-
 motor/foc_math.h  |  1 +
 motor/mcpwm_foc.c |  2 +-
 3 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/motor/foc_math.c b/motor/foc_math.c
index f57d0279..ce218f95 100644
--- a/motor/foc_math.c
+++ b/motor/foc_math.c
@@ -550,6 +550,7 @@ float foc_correct_hall(float angle, float dt, motor_all_state_t *motor, int hall
 
 	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((M_PI / 3.0) / motor->m_hall_dt_diff_last));
 
 	motor->m_using_hall = rpm_abs < conf_now->foc_sl_erpm;
 	float angle_old = angle;
@@ -603,8 +604,22 @@ float foc_correct_hall(float angle, float dt, motor_all_state_t *motor, int hall
 			}
 		}
 
+		utils_norm_angle_rad((float*)&motor->m_ang_hall);
+
+		// Limit hall sensor rate of change. This will reduce current spikes in the current controllers when the angle estimation
+		// changes fast.
+		float angle_step = (fmaxf(rpm_abs_hall, conf_now->foc_hall_interp_erpm) / 60.0) * 2.0 * M_PI * dt * 1.5;
+		float angle_diff = utils_angle_difference_rad(motor->m_ang_hall, motor->m_ang_hall_rate_limited);
+		if (fabsf(angle_diff) < angle_step) {
+			motor->m_ang_hall_rate_limited = motor->m_ang_hall;
+		} else {
+			motor->m_ang_hall_rate_limited += angle_step * SIGN(angle_diff);
+		}
+
+		utils_norm_angle_rad((float*)&motor->m_ang_hall_rate_limited);
+
 		if (motor->m_using_hall) {
-			angle = motor->m_ang_hall;
+			angle = motor->m_ang_hall_rate_limited;
 		}
 	} else {
 		// Invalid hall reading. Don't update angle.
diff --git a/motor/foc_math.h b/motor/foc_math.h
index a0ba4e2f..ac8574d9 100644
--- a/motor/foc_math.h
+++ b/motor/foc_math.h
@@ -215,6 +215,7 @@ typedef struct {
 	int m_ang_hall_int_prev;
 	bool m_using_hall;
 	float m_ang_hall;
+	float m_ang_hall_rate_limited;
 	float m_hall_dt_diff_last;
 	float m_hall_dt_diff_now;
 	bool m_motor_released;
diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c
index 25652b97..d465cb39 100644
--- a/motor/mcpwm_foc.c
+++ b/motor/mcpwm_foc.c
@@ -1390,7 +1390,7 @@ float mcpwm_foc_get_phase_encoder(void) {
 }
 
 float mcpwm_foc_get_phase_hall(void) {
-	float angle = RAD2DEG_f(get_motor_now()->m_ang_hall);
+	float angle = RAD2DEG_f(get_motor_now()->m_ang_hall_rate_limited);
 	utils_norm_angle(&angle);
 	return angle;
 }