Add motor characterisation

src/BLDCMotor.h

@@ -81,6 +81,16 @@ class BLDCMotor: public FOCMotor
     */
     void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
 
+    /**
+     * Measure resistance and inductance of a BLDCMotor and print results to debug.
+     * If a sensor is available, an estimate of zero electric angle will be reported too.
+     * @param voltage The voltage applied to the motor
+     * @returns 0 for success, >0 for failure
+     */
+    int characteriseMotor(float voltage){
+      return FOCMotor::characteriseMotor(voltage, 1.5f);
+    }
+
   private:
     // FOC methods 
 

src/StepperMotor.h

@@ -83,6 +83,17 @@ class StepperMotor: public FOCMotor
     */
     void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
 
+    /**
+     * Measure resistance and inductance of a StepperMotor and print results to debug.
+     * If a sensor is available, an estimate of zero electric angle will be reported too.
+     * TODO: determine the correction factor
+     * @param voltage The voltage applied to the motor
+     * @returns 0 for success, >0 for failure
+     */
+    int characteriseMotor(float voltage){
+      return FOCMotor::characteriseMotor(voltage, 1.0f);
+    }
+
   private:
 
     /** Sensor alignment to electrical 0 angle of the motor */

src/common/base_classes/FOCMotor.cpp

@@ -92,6 +92,227 @@ void FOCMotor::useMonitoring(Print &print){
   #endif
 }
 
+// Measure resistance and inductance of a motor
+int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
+    if (!this->current_sense || !this->current_sense->initialized)
+    {
+      SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: CS unconfigured or not initialized");
+      return 1;
+    }
+
+    if (voltage <= 0.0f){
+      SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: Voltage is negative or less than zero");
+      return 2;
+    }
+    voltage = _constrain(voltage, 0.0f, voltage_limit);
+
+    SIMPLEFOC_DEBUG("MOT: Measuring phase to phase resistance, keep motor still...");
+
+    float current_electric_angle = electricalAngle();
+
+    // Apply zero volts and measure a zero reference
+    setPhaseVoltage(0, 0, current_electric_angle);
+    _delay(500);
+
+    PhaseCurrent_s zerocurrent_raw = current_sense->readAverageCurrents();
+    DQCurrent_s zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle);
+
+
+    // Ramp and hold the voltage to measure resistance
+    // 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);
+        _delay(3);
+    }
+    _delay(10);
+    PhaseCurrent_s r_currents_raw = current_sense->readAverageCurrents();
+    DQCurrent_s r_currents = current_sense->getDQCurrents(current_sense->getABCurrents(r_currents_raw), current_electric_angle);
+
+    // Zero again
+    setPhaseVoltage(0, 0, current_electric_angle);
+
+
+    if (fabsf(r_currents.d - zerocurrent.d) < 0.2f)
+    {
+      SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: measured current too low");
+      return 3;
+    }
+
+    float resistance = voltage / (correction_factor * (r_currents.d - zerocurrent.d));
+    if (resistance <= 0.0f)
+    {
+      SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: Calculated resistance <= 0");
+      return 4;
+    }
+
+    SIMPLEFOC_DEBUG("MOT: Estimated phase to phase resistance: ", 2.0f * resistance);
+    _delay(100);
+
+
+    // Start inductance measurement
+    SIMPLEFOC_DEBUG("MOT: Measuring inductance, keep motor still...");
+
+    unsigned long t0 = 0;
+    unsigned long t1 = 0;
+    float Ltemp = 0;
+    float Ld = 0;
+    float Lq = 0;
+    float d_electrical_angle = 0;
+
+    uint iterations = 40;    // how often the algorithm gets repeated.
+    uint cycles = 3;         // averaged measurements for each iteration
+    uint risetime_us = 200;  // initially short for worst case scenario with low inductance
+    uint settle_us = 100000; // initially long for worst case scenario with high inductance
+
+    // Pre-rotate the angle to the q-axis (only useful with sensor, else no harm in doing it)
+    current_electric_angle += 0.5f * _PI;
+    current_electric_angle = _normalizeAngle(current_electric_angle);
+
+    for (size_t axis = 0; axis < 2; axis++)
+    {
+      for (size_t i = 0; i < iterations; i++)
+      {
+        // current_electric_angle = i * _2PI / iterations; // <-- Do a sweep of the inductance. Use eg. for graphing
+        float inductanced = 0.0f;
+
+        float qcurrent = 0.0f;
+        float dcurrent = 0.0f;
+        for (size_t j = 0; j < cycles; j++)
+        {
+          // read zero current
+          zerocurrent_raw = current_sense->readAverageCurrents(20);
+          zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle);
+
+          // step the voltage
+          setPhaseVoltage(0, voltage, current_electric_angle);
+          t0 = micros();
+          delayMicroseconds(risetime_us); // wait a little bit
+
+          PhaseCurrent_s l_currents_raw = current_sense->getPhaseCurrents();
+          t1 = micros();
+          setPhaseVoltage(0, 0, current_electric_angle);
+
+          DQCurrent_s l_currents = current_sense->getDQCurrents(current_sense->getABCurrents(l_currents_raw), current_electric_angle);
+          delayMicroseconds(settle_us); // wait a bit for the currents to go to 0 again
+
+          if (t0 > t1) continue; // safety first
+
+          // calculate the inductance
+          float dt = (t1 - t0)/1000000.0f;
+          if (l_currents.d - zerocurrent.d <= 0 || (voltage - resistance * (l_currents.d - zerocurrent.d)) <= 0)
+          {
+            continue;
+          }
+
+          inductanced += fabsf(- (resistance * dt) / log((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;
+        }
+        qcurrent /= cycles;
+        dcurrent /= cycles;
+
+        float delta = qcurrent / (fabsf(dcurrent) + fabsf(qcurrent)); 
+
+
+        inductanced /= cycles;
+        Ltemp = i < 2 ? inductanced : Ltemp * 0.6 + inductanced * 0.4;
+
+        float timeconstant = fabsf(Ltemp / resistance); // Timeconstant of an RL circuit (L/R) 
+        // SIMPLEFOC_DEBUG("MOT: Estimated time constant in us: ", 1000000.0f * timeconstant);
+
+        // Wait as long as possible (due to limited timing accuracy & sample rate), but as short as needed (while the current still changes)
+        risetime_us = _constrain(risetime_us * 0.6f + 0.4f * 1000000 * 0.6f * timeconstant, 100, 10000);
+        settle_us = 10 * risetime_us;
+
+
+        // Serial.printf(">inductance:%f:%f|xy\n", current_electric_angle, Ltemp * 1000.0f); // <-- Plot an angle sweep
+
+
+        /**
+         * How this code works:
+         * If we apply a current spike in the d´-axis, there will be cross coupling to the q´-axis current, if we didn´t use the actual d-axis (ie. d´ != d).
+         * This has to do with saliency (Ld != Lq). 
+         * The amount of cross coupled current is somewhat proportional to the angle error, which means that if we iteratively change the angle to min/maximise this current, we get the correct d-axis (and q-axis).
+        */
+        if (axis)
+        {
+          qcurrent *= -1.0f; // to d or q axis
+        }
+
+        if (qcurrent < 0)
+        {
+          current_electric_angle+=fabsf(delta);
+        } else
+        {
+          current_electric_angle-=fabsf(delta);
+        }
+        current_electric_angle = _normalizeAngle(current_electric_angle);
+
+
+        // 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;
+        }
+
+      }
+
+      // We know the minimum is 0.5*PI radians away, so less iterations are needed.
+      current_electric_angle += 0.5f * _PI;
+      current_electric_angle = _normalizeAngle(current_electric_angle);
+      iterations /= 2;
+
+      if (axis == 0)
+      {
+        Lq = Ltemp;
+      }else
+      {
+        Ld = Ltemp;
+      }
+
+    }
+
+    if (sensor)
+    {
+      /**
+       * The d_electrical_angle should now be aligned to the d axis or the -d axis. We can therefore calculate two possible electrical zero angles. 
+       * We then report the one closest to the actual value. This could be useful if the zero search method is not reliable enough (eg. high pole count).
+      */
+
+      float estimated_zero_electric_angle_A = _normalizeAngle(  (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle);
+      float estimated_zero_electric_angle_B = _normalizeAngle(  (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle + _PI);
+      float estimated_zero_electric_angle = 0.0f;
+      if (fabsf(estimated_zero_electric_angle_A - zero_electric_angle) < fabsf(estimated_zero_electric_angle_B - zero_electric_angle))
+      {
+        estimated_zero_electric_angle = estimated_zero_electric_angle_A;
+      } else
+      {
+        estimated_zero_electric_angle = estimated_zero_electric_angle_B;
+      }
+
+      SIMPLEFOC_DEBUG("MOT: Newly estimated electrical zero: ", estimated_zero_electric_angle);
+      SIMPLEFOC_DEBUG("MOT: Current electrical zero: ", zero_electric_angle);
+    }
+
+
+    SIMPLEFOC_DEBUG("MOT: Inductance measurement complete!");
+    SIMPLEFOC_DEBUG("MOT: Measured D-inductance in mH: ", Ld * 1000.0f);
+    SIMPLEFOC_DEBUG("MOT: Measured Q-inductance in mH: ", Lq * 1000.0f);
+    if (Ld > Lq)
+    {
+      SIMPLEFOC_DEBUG("WARN: MOT: Measured inductance is larger in D than in Q axis. This is normally a sign of a measurement error.");
+    }
+    if (Ld * 2.0f < Lq)
+    {
+      SIMPLEFOC_DEBUG("WARN: MOT: Measured Q inductance is more than twice the D inductance. This is probably wrong. From experience, the lower value is probably close to reality.");
+    }    
+
+    return 0;
+
+}
+
 // utility function intended to be used with serial plotter to monitor motor variables
 // significantly slowing the execution down!!!!
 void FOCMotor::monitor() {

src/common/base_classes/FOCMotor.h

@@ -149,6 +149,15 @@ class FOCMotor
      */
     float electricalAngle();
 
+    /**
+     * Measure resistance and inductance of a motor and print results to debug.
+     * If a sensor is available, an estimate of zero electric angle will be reported too.
+     * @param voltage The voltage applied to the motor
+     * @param correction_factor  Is 1.5 for 3 phase motors, because we measure over a series-parallel connection. TODO: what about 2 phase motors?
+     * @returns 0 for success, >0 for failure
+     */
+    int characteriseMotor(float voltage, float correction_factor);
+
     // state variables
     float target; //!< current target value - depends of the controller
     float feed_forward_velocity = 0.0f; //!< current feed forward velocity