Improvements to LinearHall

Changes compared to the original pull request simplefoc#12

1. Added a version of init which turns the motor one revolution to find the center values of the sensors.
2. Moved the calls to analogRead into a weakly bound function ReadLinearHalls so it can be overridden with custom ADC code on platforms with poor analogRead performance.
3. Commented out the pinMode calls in init, which makes it possible to pass in ADC channel numbers for custom ReadLinearHalls to use without having to remap them every update.
4. Changed to use the much faster _atan2 function that was added to foc_utils recently.

Author: dekutree64

src/encoders/linearhall/LinearHall.cpp

@@ -0,0 +1,110 @@
+#include "LinearHall.h"
+
+// This function can be overridden with custom ADC code on platforms with poor analogRead performance.
+__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b)
+{
+  *a = analogRead(hallA);
+  *b = analogRead(hallB);
+}
+
+LinearHall::LinearHall(int _hallA, int _hallB, int _pp){
+  centerA = 512;
+  centerB = 512;
+  pinA = _hallA;
+  pinB = _hallB;
+  pp = _pp;
+}
+
+float LinearHall::getSensorAngle() {
+  ReadLinearHalls(pinA, pinB, &lastA, &lastB);
+  //offset readings using center values, then compute angle
+  float reading = _atan2(lastA - centerA, lastB - centerB);
+
+  //handle rollover logic between each electrical revolution of the motor
+  if (reading > prev_reading) {
+    if (reading - prev_reading >= PI) {
+      if (electrical_rev - 1 < 0) {
+        electrical_rev = pp - 1;
+      } else {
+        electrical_rev = electrical_rev - 1;
+      }
+    }
+  } else if (reading < prev_reading) {
+    if (prev_reading - reading >= PI) {
+      if (electrical_rev + 1 >= pp) {
+        electrical_rev = 0;
+      } else {
+        electrical_rev = electrical_rev + 1;
+      }
+    }
+  }
+
+  //convert result from electrical angle and electrical revolution count to shaft angle in radians
+  float result = (reading + PI) / _2PI;
+  result = _2PI * (result + electrical_rev) / pp;
+
+  //update previous reading for rollover handling
+  prev_reading = reading;
+  return result;
+}
+
+void LinearHall::init(int _centerA, int _centerB) {
+  // Skip configuring the pins here because they normally default to input anyway, and
+  // this makes it possible to use ADC channel numbers instead of pin numbers when using
+  // custom implementation of ReadLinearHalls, to avoid having to remap them every update.
+  // If pins do need to be configured, it can be done by user code before calling init.
+  //pinMode(pinA, INPUT);
+  //pinMode(pinB, INPUT);
+
+  centerA = _centerA;
+  centerB = _centerB;
+
+  //establish initial reading for rollover handling
+  electrical_rev = 0;
+  ReadLinearHalls(pinA, pinB, &lastA, &lastB);
+  prev_reading = _atan2(lastA - centerA, lastB - centerB);
+}
+
+void LinearHall::init(FOCMotor *motor) {
+  if (!motor->enabled) {
+    SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC.");
+    return;
+  }
+
+  // See comment in other version of init for why these are commented out
+  //pinMode(pinA, INPUT);
+  //pinMode(pinB, INPUT);
+
+  int minA, maxA, minB, maxB;
+
+  ReadLinearHalls(pinA, pinB, &lastA, &lastB);
+  minA = maxA = centerA = lastA;
+  minB = maxB = centerB = lastB;
+
+  // move one mechanical revolution forward
+  for (int i = 0; i <= 2000; i++)
+  {
+    float angle = _3PI_2 + _2PI * i * pp / 2000.0f;
+    motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle);
+
+    ReadLinearHalls(pinA, pinB, &lastA, &lastB);
+
+    if (lastA < minA)
+      minA = lastA;
+    if (lastA > maxA)
+      maxA = lastA;
+    centerA = (minA + maxA) / 2;
+
+    if (lastB < minB)
+      minB = lastB;
+    if (lastB > maxB)
+      maxB = lastB;
+    centerB = (minB + maxB) / 2;
+
+    _delay(2);
+  }
+
+  //establish initial reading for rollover handling
+  electrical_rev = 0;
+  prev_reading = _atan2(lastA - centerA, lastB - centerB);
+}

src/encoders/linearhall/LinearHall.h

@@ -0,0 +1,45 @@
+#ifndef LINEAR_HALL_SENSOR_LIB_H
+#define LINEAR_HALL_SENSOR_LIB_H
+
+#include <SimpleFOC.h>
+
+// This function can be overridden with custom ADC code on platforms with poor analogRead performance.
+void ReadLinearHalls(int hallA, int hallB, int *a, int *b);
+
+/**
+ * This sensor class is for two linear hall effect sensors such as 49E, which are
+ * positioned 90 electrical degrees apart (if one is centered on a rotor magnet,
+ * the other is half way between rotor magnets).
+ * It can also be used for a single magnet mounted to the motor shaft (set pp to 1).
+ *
+ * For more information, see this forum thread and PDF
+ * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959
+ * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1
+ */
+class LinearHall: public Sensor{
+  public:
+    LinearHall(int hallA, int hallB, int pp);
+
+    void init(int centerA, int centerB); // Initialize without moving motor
+    void init(class FOCMotor *motor); // Move motor to find center values
+
+    // Get current shaft angle from the sensor hardware, and
+    // return it as a float in radians, in the range 0 to 2PI.
+    //  - This method is pure virtual and must be implemented in subclasses.
+    //    Calling this method directly does not update the base-class internal fields.
+    //    Use update() when calling from outside code.
+    float getSensorAngle() override;
+
+    int centerA;
+    int centerB;
+    int lastA, lastB;
+
+  private:
+    int pinA;
+    int pinB;
+    int pp;
+    int electrical_rev;
+    float prev_reading;
+};
+
+#endif