initial commit

src/BLDCMotor.cpp

@@ -315,6 +315,12 @@ int BLDCMotor::absoluteZeroSearch() {
 // Iterative function looping FOC algorithm, setting Uq on the Motor
 // The faster it can be run the better
 void BLDCMotor::loopFOC() {
+  float now = _micros();
+  if (timestamp_prev == 0) timestamp_prev = now; // First time  
+  Ts = ( now - timestamp_prev)*1e-6f;
+  if (Ts < 0.0f ) Ts = 1e-3f;
+  timestamp_prev = now;
+
   // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
   //                 of full rotations otherwise.
   if (sensor) sensor->update();
@@ -350,11 +356,11 @@ void BLDCMotor::loopFOC() {
       // read dq currents
       current = current_sense->getFOCCurrents(electrical_angle);
       // filter values
-      current.q = LPF_current_q(current.q);
-      current.d = LPF_current_d(current.d);
+      current.q = LPF_current_q(current.q,Ts);
+      current.d = LPF_current_d(current.d,Ts);
       // calculate the phase voltages
-      voltage.q = PID_current_q(current_sp - current.q);
-      voltage.d = PID_current_d(-current.d);
+      voltage.q = PID_current_q(current_sp - current.q,Ts);
+      voltage.d = PID_current_d(-current.d,Ts);
       // d voltage - lag compensation - TODO verify
       // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
       break;

src/common/base_classes/FOCMotor.h

@@ -161,6 +161,8 @@ class FOCMotor
     DQVoltage_s voltage;//!< current d and q voltage set to the motor
     DQCurrent_s current;//!< current d and q current measured
     float voltage_bemf; //!< estimated backemf voltage (if provided KV constant)
+    float Ts=0;
+    float timestamp_prev=0;
 
     // motor configuration parameters
     float voltage_sensor_align;//!< sensor and motor align voltage parameter

src/common/lowpass_filter.cpp

@@ -8,19 +8,23 @@ LowPassFilter::LowPassFilter(float time_constant)
 }
 
 
-float LowPassFilter::operator() (float x)
+float LowPassFilter::operator() (float x,float Ts)
 {
-    unsigned long timestamp = _micros();
-    float dt = (timestamp - timestamp_prev)*1e-6f;
+    unsigned long timestamp = 0;
+
+    if (Ts==0){
+        timestamp = _micros();
+        Ts = (timestamp - timestamp_prev)*1e-6f;
+        if (Ts < 0.0f ) Ts = 1e-3f;
+    }
 
-    if (dt < 0.0f ) dt = 1e-3f;
-    else if(dt > 0.3f) {
+    else if(Ts > 0.3f) {
         y_prev = x;
         timestamp_prev = timestamp;
         return x;
     }
 
-    float alpha = Tf/(Tf + dt);
+    float alpha = Tf/(Tf + Ts);
     float y = alpha*y_prev + (1.0f - alpha)*x;
     y_prev = y;
     timestamp_prev = timestamp;

src/common/lowpass_filter.h

@@ -17,7 +17,7 @@ class LowPassFilter
     LowPassFilter(float Tf);
     ~LowPassFilter() = default;
 
-    float operator() (float x);
+    float operator() (float x,float Ts=0);
     float Tf; //!< Low pass filter time constant
 
 protected:

src/common/pid.cpp

@@ -14,12 +14,16 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit)
 }
 
 // PID controller function
-float PIDController::operator() (float error){
-    // calculate the time from the last call
-    unsigned long timestamp_now = _micros();
-    float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
-    // quick fix for strange cases (micros overflow)
-    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+float PIDController::operator() (float error,float Ts){
+    unsigned long timestamp_now = 0;
+
+    if (Ts == 0){
+        // calculate the time from the last call
+        timestamp_now = _micros();
+        Ts = (timestamp_now - timestamp_prev) * 1e-6f;
+        // quick fix for strange cases (micros overflow)
+        if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+    }
 
     // u(s) = (P + I/s + Ds)e(s)
     // Discrete implementations

src/common/pid.h

@@ -22,7 +22,7 @@ class PIDController
     PIDController(float P, float I, float D, float ramp, float limit);
     ~PIDController() = default;
 
-    float operator() (float error);
+    float operator() (float error,float Ts=0);
     void reset();
 
     float P; //!< Proportional gain