try to fix submodule

firmware/lib/Arduino-FOC/src/sensors/Encoder.cpp

@@ -0,0 +1,229 @@
+#include "Encoder.h"
+
+
+/*
+  Encoder(int encA, int encB , int cpr, int index)
+  - encA, encB    - encoder A and B pins
+  - cpr           - counts per rotation number (cpm=ppm*4)
+  - index pin     - (optional input)
+*/
+
+Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){
+
+  // Encoder measurement structure init
+  // hardware pins
+  pinA = _encA;
+  pinB = _encB;
+  // counter setup
+  pulse_counter = 0;
+  pulse_timestamp = 0;
+
+  cpr = _ppr;
+  A_active = 0;
+  B_active = 0;
+  I_active = 0;
+  // index pin
+  index_pin = _index; // its 0 if not used
+
+  // velocity calculation variables
+  prev_Th = 0;
+  pulse_per_second = 0;
+  prev_pulse_counter = 0;
+  prev_timestamp_us = _micros();
+
+  // extern pullup as default
+  pullup = Pullup::USE_EXTERN;
+  // enable quadrature encoder by default
+  quadrature = Quadrature::ON;
+}
+
+//  Encoder interrupt callback functions
+// A channel
+void Encoder::handleA() {
+  bool A = digitalRead(pinA);
+  switch (quadrature){
+    case Quadrature::ON:
+      // CPR = 4xPPR
+      if ( A != A_active ) {
+        pulse_counter += (A_active == B_active) ? 1 : -1;
+        pulse_timestamp = _micros();
+        A_active = A;
+      }
+      break;
+    case Quadrature::OFF:
+      // CPR = PPR
+      if(A && !digitalRead(pinB)){
+        pulse_counter++;
+        pulse_timestamp = _micros();
+      }
+      break;
+  }
+}
+// B channel
+void Encoder::handleB() {
+  bool B = digitalRead(pinB);
+  switch (quadrature){
+    case Quadrature::ON:
+  //     // CPR = 4xPPR
+      if ( B != B_active ) {
+        pulse_counter += (A_active != B_active) ? 1 : -1;
+        pulse_timestamp = _micros();
+        B_active = B;
+      }
+      break;
+    case Quadrature::OFF:
+      // CPR = PPR
+      if(B && !digitalRead(pinA)){
+        pulse_counter--;
+        pulse_timestamp = _micros();
+      }
+      break;
+  }
+}
+
+// Index channel
+void Encoder::handleIndex() {
+  if(hasIndex()){
+    bool I = digitalRead(index_pin);
+    if(I && !I_active){
+      index_found = true;
+      // align encoder on each index
+      long tmp = pulse_counter;
+      // corrent the counter value
+      pulse_counter = round((double)pulse_counter/(double)cpr)*cpr;
+      // preserve relative speed
+      prev_pulse_counter += pulse_counter - tmp;
+    }
+    I_active = I;
+  }
+}
+
+
+// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables.
+void Encoder::update() {
+  // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed
+  noInterrupts();
+  angle_prev_ts = pulse_timestamp;
+  long copy_pulse_counter = pulse_counter;
+  interrupts();
+  // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost
+  full_rotations = copy_pulse_counter / (int)cpr;
+  angle_prev = _2PI * ((copy_pulse_counter) % ((int)cpr)) / ((float)cpr);
+}
+
+/*
+	Shaft angle calculation
+*/
+float Encoder::getSensorAngle(){
+  return _2PI * (pulse_counter) / ((float)cpr);
+}
+
+
+
+/*
+  Shaft velocity calculation
+  function using mixed time and frequency measurement technique
+*/
+float Encoder::getVelocity(){
+  // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed
+  noInterrupts();
+  long copy_pulse_counter = pulse_counter;
+  long copy_pulse_timestamp = pulse_timestamp;
+  interrupts();
+  // timestamp
+  long timestamp_us = _micros();
+  // sampling time calculation
+  float Ts = (timestamp_us - prev_timestamp_us) * 1e-6f;
+  // quick fix for strange cases (micros overflow)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // time from last impulse
+  float Th = (timestamp_us - copy_pulse_timestamp) * 1e-6f;
+  long dN = copy_pulse_counter - prev_pulse_counter;
+
+  // Pulse per second calculation (Eq.3.)
+  // dN - impulses received
+  // Ts - sampling time - time in between function calls
+  // Th - time from last impulse
+  // Th_1 - time form last impulse of the previous call
+  // only increment if some impulses received
+  float dt = Ts + prev_Th - Th;
+  pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second;
+
+  // if more than 0.05f passed in between impulses
+  if ( Th > 0.1f) pulse_per_second = 0;
+
+  // velocity calculation
+  float velocity = pulse_per_second / ((float)cpr) * (_2PI);
+
+  // save variables for next pass
+  prev_timestamp_us = timestamp_us;
+  // save velocity calculation variables
+  prev_Th = Th;
+  prev_pulse_counter = copy_pulse_counter;
+  return velocity;
+}
+
+// getter for index pin
+// return -1 if no index
+int Encoder::needsSearch(){
+  return hasIndex() && !index_found;
+}
+
+// private function used to determine if encoder has index
+int Encoder::hasIndex(){
+  return index_pin != 0;
+}
+
+
+// encoder initialisation of the hardware pins
+// and calculation variables
+void Encoder::init(){
+
+  // Encoder - check if pullup needed for your encoder
+  if(pullup == Pullup::USE_INTERN){
+    pinMode(pinA, INPUT_PULLUP);
+    pinMode(pinB, INPUT_PULLUP);
+    if(hasIndex()) pinMode(index_pin,INPUT_PULLUP);
+  }else{
+    pinMode(pinA, INPUT);
+    pinMode(pinB, INPUT);
+    if(hasIndex()) pinMode(index_pin,INPUT);
+  }
+
+  // counter setup
+  pulse_counter = 0;
+  pulse_timestamp = _micros();
+  // velocity calculation variables
+  prev_Th = 0;
+  pulse_per_second = 0;
+  prev_pulse_counter = 0;
+  prev_timestamp_us = _micros();
+
+  // initial cpr = PPR
+  // change it if the mode is quadrature
+  if(quadrature == Quadrature::ON) cpr = 4*cpr;
+
+  // we don't call Sensor::init() here because init is handled in Encoder class.
+}
+
+// function enabling hardware interrupts of the for the callback provided
+// if callback is not provided then the interrupt is not enabled
+void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){
+  // attach interrupt if functions provided
+  switch(quadrature){
+    case Quadrature::ON:
+      // A callback and B callback
+      if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE);
+      if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
+      break;
+    case Quadrature::OFF:
+      // A callback and B callback
+      if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING);
+      if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING);
+      break;
+  }
+
+  // if index used initialize the index interrupt
+  if(hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE);
+}