Fixed bug introduced in PR and calculate motor current from magnitude of both axes

Author: vedderb

CHANGELOG

@@ -12,6 +12,7 @@
 * Added bm_reset terminal command.
 * Added bm support for STM32F30x and STM32L47x.
 * App Balance updates. See https://github.com/vedderb/bldc/pull/193
+* Motor current now based on magnitude of both axes.
 
 === FW 5.01 ===
 * Fixed PPM bug in previous release.

mcpwm_foc.c

@@ -1084,22 +1084,20 @@ bool mcpwm_foc_is_using_encoder(void) {
 float mcpwm_foc_get_tot_current_motor(bool is_second_motor) {
 #ifdef HW_HAS_DUAL_MOTORS
 	volatile motor_all_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1;
-	return SIGN(motor->m_motor_state.vq) * motor->m_motor_state.iq;
+	return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq) * motor->m_motor_state.i_abs;
 #else
 	(void)is_second_motor;
-	return SIGN(m_motor_1.m_motor_state.vq) * m_motor_1.m_motor_state.iq;
+	return SIGN(m_motor_1.m_motor_state.vq * m_motor_1.m_motor_state.iq) * m_motor_1.m_motor_state.i_abs;
 #endif
 }
 
 float mcpwm_foc_get_tot_current_filtered_motor(bool is_second_motor) {
 #ifdef HW_HAS_DUAL_MOTORS
 	volatile motor_all_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1;
-	return SIGN(motor->m_motor_state.vq) *
-			sqrtf( SQ(motor->m_motor_state.iq_filter) + SQ(motor->m_motor_state.id_filter) );
+	return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq_filter) * motor->m_motor_state.i_abs_filter;
 #else
 	(void)is_second_motor;
-	return SIGN(m_motor_1.m_motor_state.vq) *
-			sqrtf( SQ(m_motor_1.m_motor_state.iq_filter) + SQ(m_motor_1.m_motor_state.id_filter) );
+	return SIGN(m_motor_1.m_motor_state.vq * m_motor_1.m_motor_state.iq_filter) * m_motor_1.m_motor_state.i_abs_filter;
 #endif
 }
 
@@ -1185,7 +1183,8 @@ float mcpwm_foc_get_rpm_faster(void) {
  * The motor current.
  */
 float mcpwm_foc_get_tot_current(void) {
-	return SIGN(motor_now()->m_motor_state.vq) * motor_now()->m_motor_state.iq;
+	volatile motor_all_state_t *motor = motor_now();
+	return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq) * motor->m_motor_state.i_abs;
 }
 
 /**
@@ -1197,7 +1196,8 @@ float mcpwm_foc_get_tot_current(void) {
  * The filtered motor current.
  */
 float mcpwm_foc_get_tot_current_filtered(void) {
-	return SIGN(motor_now()->m_motor_state.vq) * motor_now()->m_motor_state.iq_filter;
+	volatile motor_all_state_t *motor = motor_now();
+	return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq_filter) * motor->m_motor_state.i_abs_filter;
 }
 
 /**