Fix NaN-problem after lost tracking on some motors

Author: vedderb

hwconf/hw_100_500.h

@@ -216,8 +216,7 @@
 #define READ_HALL2()			palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
 #define READ_HALL3()			palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
 
-// Override dead time. See the stm32f4 reference manual for calculating this value.
-#define HW_DEAD_TIME_NSEC		660.0
+#define HW_DEAD_TIME_NSEC		1000.0
 
 // Default setting overrides
 #ifndef MCCONF_L_MIN_VOLTAGE

mcpwm_foc.c

@@ -2765,6 +2765,9 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 				observer_update(motor_now->m_motor_state.v_alpha, motor_now->m_motor_state.v_beta,
 						motor_now->m_motor_state.i_alpha, motor_now->m_motor_state.i_beta, dt,
 						&motor_now->m_observer_x1, &motor_now->m_observer_x2, &motor_now->m_phase_now_observer, motor_now);
+
+				// Compensate from the phase lag caused by the switching frequency. This is important for motors
+				// that run on high ERPM compared to the switching frequency.
 				motor_now->m_phase_now_observer += motor_now->m_pll_speed * dt * 0.5;
 				utils_norm_angle_rad((float*)&motor_now->m_phase_now_observer);
 			}
@@ -3781,8 +3784,8 @@ static void control_current(volatile motor_all_state_t *motor, float dt) {
 	float dec_bemf = 0.0;
 
 	if (motor->m_control_mode < CONTROL_MODE_HANDBRAKE && conf_now->foc_cc_decoupling != FOC_CC_DECOUPLING_DISABLED) {
-		float lq = conf_now->foc_motor_l + conf_now->foc_motor_ld_lq_diff / 2.0;
-		float ld = conf_now->foc_motor_l - conf_now->foc_motor_ld_lq_diff / 2.0;
+		float lq = conf_now->foc_motor_l + conf_now->foc_motor_ld_lq_diff * 0.5;
+		float ld = conf_now->foc_motor_l - conf_now->foc_motor_ld_lq_diff * 0.5;
 
 		switch (conf_now->foc_cc_decoupling) {
 		case FOC_CC_DECOUPLING_CROSS:

utils.c

@@ -323,6 +323,8 @@ float utils_fast_atan2(float y, float x) {
 		angle = ((0.1963 * rsq) - 0.9817) * r + (3.0 * M_PI / 4.0);
 	}
 
+	UTILS_NAN_ZERO(angle);
+
 	if (y < 0) {
 		return(-angle);
 	} else {