Merge branch 'dev' into feat_additional_position_sensor

Author: askuric

src/common/base_classes/CurrentSense.cpp

@@ -202,12 +202,12 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool m
     bool phases_inverted = 0;
 
     float zero = 0;
-    if(modulation_centered) zero = driver->voltage_limit/2.0;
+    if(modulation_centered) zero = driver->voltage_limit/2.0f;
 
     // set phase A active and phases B and C down
     // 300 ms of ramping
     for(int i=0; i < 100; i++){
-        bldc_driver->setPwm(voltage/100.0*((float)i)+zero , zero, zero);
+        bldc_driver->setPwm(voltage/100.0f*((float)i)+zero , zero, zero);
         _delay(3);
     }
     _delay(500);
@@ -318,7 +318,7 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool m
     // set phase B active and phases A and C down
     // 300 ms of ramping
     for(int i=0; i < 100; i++){
-        bldc_driver->setPwm(zero, voltage/100.0*((float)i)+zero, zero);
+        bldc_driver->setPwm(zero, voltage/100.0f*((float)i)+zero, zero);
         _delay(3);
     }
     _delay(500);
@@ -430,7 +430,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive
     // set phase A active and phases B down
     // ramp 300ms
     for(int i=0; i < 100; i++){
-        stepper_driver->setPwm(voltage/100.0*((float)i), 0);
+        stepper_driver->setPwm(voltage/100.0f*((float)i), 0);
         _delay(3);
     }
     _delay(500);
@@ -465,7 +465,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive
     // set phase B active and phases A down
     // ramp 300ms
     for(int i=0; i < 100; i++){
-        stepper_driver->setPwm(0, voltage/100.0*((float)i));
+        stepper_driver->setPwm(0, voltage/100.0f*((float)i));
         _delay(3);
     }
     _delay(500);
@@ -511,7 +511,7 @@ int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool
     // set phase A active and phases B active, and C down
     // ramp 300ms
     for(int i=0; i < 100; i++){
-        bldc_driver->setPwm(voltage/100.0*((float)i), voltage/100.0*((float)i), 0);
+        bldc_driver->setPwm(voltage/100.0f*((float)i), voltage/100.0f*((float)i), 0);
         _delay(3);
     }
     _delay(500);
@@ -601,7 +601,7 @@ int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool
     // set phase A active and phases B down
     // ramp 300ms
     for(int i=0; i < 100; i++){
-        bldc_driver->setPwm(voltage/100.0*((float)i), 0, 0);
+        bldc_driver->setPwm(voltage/100.0f*((float)i), 0, 0);
         _delay(3);
     }
     _delay(500);
@@ -682,7 +682,7 @@ int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool
     // set phase B active and phases A down
     // ramp 300ms
     for(int i=0; i < 100; i++){
-        bldc_driver->setPwm(0, voltage/100.0*((float)i), 0);
+        bldc_driver->setPwm(0, voltage/100.0f*((float)i), 0);
         _delay(3);
     }
     _delay(500);

src/common/base_classes/FOCMotor.cpp

@@ -140,7 +140,7 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
     // 300 ms of ramping
     current_electric_angle = electricalAngle();
     for(int i=0; i < 100; i++){
-        setPhaseVoltage(0, voltage/100.0*((float)i), current_electric_angle);
+        setPhaseVoltage(0, voltage/100.0f*((float)i), current_electric_angle);
         _delay(3);
     }
     _delay(10);
@@ -225,7 +225,7 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
             continue;
           }
 
-          inductanced += fabsf(- (resistance * dt) / log((voltage - resistance * (l_currents.d - zerocurrent.d)) / voltage))/correction_factor;
+          inductanced += fabsf(- (resistance * dt) / logf((voltage - resistance * (l_currents.d - zerocurrent.d)) / voltage))/correction_factor;
 
           qcurrent+= l_currents.q - zerocurrent.q; // average the measured currents
           dcurrent+= l_currents.d - zerocurrent.d;
@@ -237,7 +237,7 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
 
 
         inductanced /= cycles;
-        Ltemp = i < 2 ? inductanced : Ltemp * 0.6 + inductanced * 0.4;
+        Ltemp = i < 2 ? inductanced : Ltemp * 0.6f + inductanced * 0.4f;
 
         float timeconstant = fabsf(Ltemp / resistance); // Timeconstant of an RL circuit (L/R) 
         // SIMPLEFOC_MOTOR_DEBUG("Estimated time constant in us: ", 1000000.0f * timeconstant);
@@ -274,7 +274,7 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
         // Average the d-axis angle further for calculating the electrical zero later
         if (axis)
         {
-          d_electrical_angle = i < 2 ? current_electric_angle : d_electrical_angle * 0.9 + current_electric_angle * 0.1;
+          d_electrical_angle = i < 2 ? current_electric_angle : d_electrical_angle * 0.9f + current_electric_angle * 0.1f;
         }
 
       }
@@ -916,7 +916,7 @@ int FOCMotor::alignSensor() {
     // setPhaseVoltage(0, 0, 0);
     _delay(200);
     // determine the direction the sensor moved
-    float moved =  fabs(mid_angle - end_angle);
+    float moved =  fabsf(mid_angle - end_angle);
     if (moved<MIN_ANGLE_DETECT_MOVEMENT) { // minimum angle to detect movement
       SIMPLEFOC_MOTOR_ERROR("Failed to notice movement");
       return 0; // failed calibration
@@ -928,7 +928,7 @@ int FOCMotor::alignSensor() {
       sensor_direction = Direction::CW;
     }
     // check pole pair number
-    pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f);  // 0.5f is arbitrary number it can be lower or higher!
+    pp_check_result = !(fabsf(moved*pole_pairs - _2PI) > 0.5f);  // 0.5f is arbitrary number it can be lower or higher!
     if( pp_check_result==false ) {
       SIMPLEFOC_MOTOR_WARN("PP check: fail - est. pp: ", _2PI/moved);
     } else {

src/common/base_classes/FOCMotor.h

@@ -230,8 +230,8 @@ class FOCMotor
     float voltage_bemf; //!< estimated backemf voltage (if provided KV constant)
     float	Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform
 
-    DQCurrent_s feed_forward_current;//!< current d and q current measured
-    DQVoltage_s feed_forward_voltage;//!< current d and q voltage set to the motor
+    DQCurrent_s feed_forward_current = {0.0f, 0.0f};//!< current d and q current measured
+    DQVoltage_s feed_forward_voltage = {0.0f, 0.0f};//!< current d and q voltage set to the motor
 
     // motor configuration parameters
     float voltage_sensor_align;//!< sensor and motor align voltage parameter
@@ -271,7 +271,7 @@ class FOCMotor
     PIDController PID_velocity{DEF_PID_VEL_P,DEF_PID_VEL_I,DEF_PID_VEL_D,DEF_PID_VEL_RAMP,DEF_PID_VEL_LIMIT};//!< parameter determining the velocity PID configuration
     PIDController P_angle{DEF_P_ANGLE_P,0,0,0,DEF_VEL_LIM};	//!< parameter determining the position PID configuration 
     LowPassFilter LPF_velocity{DEF_VEL_FILTER_Tf};//!<  parameter determining the velocity Low pass filter configuration 
-    LowPassFilter LPF_angle{0.0};//!<  parameter determining the angle low pass filter configuration 
+    LowPassFilter LPF_angle{0.0f};//!<  parameter determining the angle low pass filter configuration 
     unsigned int motion_downsample = DEF_MOTION_DOWNSMAPLE; //!< parameter defining the ratio of downsampling for move commad
     unsigned int motion_cnt = 0; //!< counting variable for downsampling for move commad
 

src/common/base_classes/Sensor.cpp

@@ -29,27 +29,25 @@ float Sensor::getVelocity() {
     }
     if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
 
-    float current_angle = 0.0f;
-    float prev_angle = 0.0f;
-    // Avoid floating point precision loss for large full_rotations
-    // this is likely optional
-    if (full_rotations == vel_full_rotations) {
-        current_angle = angle_prev;
-        prev_angle = vel_angle_prev;
-    } else {
-        current_angle = (float) full_rotations * _2PI + angle_prev;
-        prev_angle = (float) vel_full_rotations * _2PI + vel_angle_prev;
+    // Calculate change in angle. Handles `full_rotations` integer wrap-arounds,
+    // and avoids float precision loss issues by keeping numbers small.
+    float delta_angle = angle_prev - vel_angle_prev;
+    const int32_t delta_full_rotations = full_rotations - vel_full_rotations;
+    if (delta_full_rotations) {
+        delta_angle += delta_full_rotations * _2PI;
     }
-    const float delta_angle = current_angle - prev_angle;
 
     // floating point equality checks are bad, so instead we check that the angle change is very small
     if (fabsf(delta_angle) > 1e-8f) {
         velocity = delta_angle / Ts;
-
-        vel_angle_prev = angle_prev;
-        vel_full_rotations = full_rotations;
-        vel_angle_prev_ts = angle_prev_ts;
+    } else {
+        velocity = 0.0f;
     }
+
+    // Always advance the velocity reference sample to avoid stale deltas/time windows.
+    vel_angle_prev = angle_prev;
+    vel_full_rotations = full_rotations;
+    vel_angle_prev_ts = angle_prev_ts;
 
     return velocity;
 }

src/common/base_classes/Sensor.h

@@ -106,7 +106,7 @@ class Sensor{
         /**
          * Minimum time between updates to velocity. If time elapsed is lower than this, the velocity is not updated.
          */
-        float min_elapsed_time = 0.000100; // default is 100 microseconds, or 10kHz
+        float min_elapsed_time = 0.000100f; // default is 100 microseconds, or 10kHz
 
     protected:
         /** 

src/common/foc_utils.cpp

@@ -75,7 +75,7 @@ __attribute__((weak)) float _atan2(float y, float x) {
 
 // normalizing radian angle to [0,2PI]
 __attribute__((weak)) float _normalizeAngle(float angle){
-  float a = fmod(angle, _2PI);
+  float a = fmodf(angle, _2PI);
   return a >= 0 ? a : (a + _2PI);
 }
 

src/communication/Commander.cpp

@@ -582,6 +582,7 @@ void Commander::target(FOCMotor* motor,  char* user_cmd, char* separator){
   float pos, vel, torque;
   char* next_value;
   switch(motor->controller){
+    case MotionControlType::custom:
     case MotionControlType::angle_nocascade:
     case MotionControlType::torque: // setting torque target
       torque = atof(strtok (user_cmd, separator));

src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp

@@ -187,7 +187,7 @@ void* IRAM_ATTR _driverSyncLowSide(void* driver_params, void* cs_params){
   if (cs->pretrig_comparator){
     // Calculate pwm duty cycle ticks for pre-trigger channel
     // TODO: verify the timing it seems to be correct between 15 and 20kHz (but needs better testing)
-    uint32_t pwm_duty_cycle  = p->mcpwm_period * (0.75 - ((float)p->pwm_frequency*SIMPLEFOC_CS_PRETRIGGER_US)/1e6/2.0); 
+    uint32_t pwm_duty_cycle  = p->mcpwm_period * (0.75f - ((float)p->pwm_frequency*SIMPLEFOC_CS_PRETRIGGER_US)/1e6f/2.0f); 
     // set up the comparator duty cycle
     CHECK_CS_ERR(mcpwm_comparator_set_compare_value((mcpwm_cmpr_handle_t)cs->pretrig_comparator, pwm_duty_cycle),
                 "Failed to set pretrigger compare value");

src/current_sense/hardware_specific/generic_mcu.cpp

@@ -26,7 +26,7 @@ __attribute__((weak))  void* _configureADCInline(const void* driver_params, cons
 // function reading an ADC value and returning the read voltage
 __attribute__((weak))  float _readADCVoltageLowSide(const int pinA, const void* cs_params){
   SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!");
-  return 0.0;
+  return 0.0f;
 }
 
 // Configure low side for generic mcu

src/current_sense/hardware_specific/samd/samd21_mcu.cpp

@@ -161,7 +161,7 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params) {
     i++;
   }
 
-  return 0.0; // pin not available
+  return 0.0f; // pin not available
 }
 
 void* _driverSyncLowSide(void* driver_params, void* cs_params) {