Hall auto sector

src/encoders/hall_any/HallSensorAny.cpp

@@ -0,0 +1,259 @@
+#include "HallSensorAny.h"
+#include "common/time_utils.h"
+#include "communication/SimpleFOCDebug.h"
+
+
+// seq 1 > 5 > 4 > 6 > 2 > 3 > 1        000 001 010 011 100 101 110 111
+const int8_t ELECTRIC_SECTORS[8] = { -1,  0,  4,  5,  2,  1,  3 , -1 };
+
+/*
+  HallSensorAny(int hallA, int hallB , int cpr, int index)
+  - hallA, hallB, hallC    - HallSensorAny A, B and C pins
+  - pp           - pole pairs
+*/
+HallSensorAny::HallSensorAny(int _hallA, int _hallB, int _hallC, int _pp, HallType _hall_type){
+
+  // hardware pins
+  pinA = _hallA;
+  pinB = _hallB;
+  pinC = _hallC;
+  hall_type = _hall_type;
+  last_print_type = hall_type;
+  for (size_t i = 0; i < sizeof(previous_states); i++)
+  {
+    previous_states[i] = -1;
+  }
+
+
+  // hall has 6 segments per electrical revolution
+  cpr = _pp * 6;
+
+  // extern pullup as default
+  pullup = Pullup::USE_EXTERN;
+}
+
+//  HallSensorAny interrupt callback functions
+// A channel
+void HallSensorAny::handleA() {
+  A_active= digitalRead(pinA);
+  updateState();
+}
+// B channel
+void HallSensorAny::handleB() {
+  B_active = digitalRead(pinB);
+  updateState();
+}
+
+// C channel
+void HallSensorAny::handleC() {
+  C_active = digitalRead(pinC);
+  updateState();
+}
+
+/**
+ * Updates the state and sector following an interrupt
+ */
+void HallSensorAny::updateState() {
+  int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2);
+  // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed
+  if (new_hall_state == hall_state_raw) return;
+  hall_state_raw = new_hall_state;
+
+  static const int num_previous_states = sizeof(previous_states);
+
+  //flip a line maybe
+  if (hall_type != HallType::UNKNOWN)
+  {
+    new_hall_state ^= static_cast<int8_t>(hall_type);
+  }
+
+  long new_pulse_timestamp = _micros();
+  if (hall_type == HallType::UNKNOWN) //Store previous steps for hall config detection
+  {
+    for (int i = num_previous_states - 2; i >= 0; i--)
+    {
+      previous_states[i+1] = previous_states[i];
+    }
+    previous_states[0] = new_hall_state;
+    //7 and 0 are illegal in 120deg mode, so we're gonna try to see which line hel up during that time and flip it so it doesn't happen
+    if ((previous_states[1] == 0b111 ||  previous_states[1] == 0b000) && previous_states[2] != -1)
+    {
+      if (previous_states[2] == previous_states[0])
+      {
+        //went back, can't do anything
+      }
+      else
+      {
+        hall_type = static_cast<HallType>((0b111 - previous_states[0] ^ previous_states[2])%8);
+        previous_states[0] ^= static_cast<int8_t>(hall_type);
+      }
+    }
+    if (abs(electric_rotations) > 2)
+    {
+      hall_type = HallType::HALL_120;
+    }
+  }
+
+
+
+
+  int8_t new_electric_sector;
+  new_electric_sector = ELECTRIC_SECTORS[new_hall_state];
+  int8_t electric_sector_dif = new_electric_sector - electric_sector;
+  if (electric_sector_dif > 3) {
+    //underflow
+    direction = Direction::CCW;
+    electric_rotations += direction;
+  } else if (electric_sector_dif < (-3)) {
+    //overflow
+    direction = Direction::CW;
+    electric_rotations += direction;
+  } else {
+    direction = (new_electric_sector > electric_sector)? Direction::CW : Direction::CCW;
+  }
+  electric_sector = new_electric_sector;
+
+  // glitch avoidance #2 changes in direction can cause velocity spikes.  Possible improvements needed in this area
+  if (direction == old_direction) {
+    // not oscilating or just changed direction
+    pulse_diff = new_pulse_timestamp - pulse_timestamp;
+  } else {
+    pulse_diff = 0;
+  }
+
+  pulse_timestamp = new_pulse_timestamp;
+  total_interrupts++;
+  old_direction = direction;
+  if (onSectorChange != nullptr) onSectorChange(electric_sector);
+}
+
+/**
+ * Optionally set a function callback to be fired when sector changes
+ * void onSectorChange(int sector) {
+ *  ... // for debug or call driver directly?
+ * }
+ * sensor.attachSectorCallback(onSectorChange);
+ */
+void HallSensorAny::attachSectorCallback(void (*_onSectorChange)(int sector)) {
+  onSectorChange = _onSectorChange;
+}
+
+
+
+// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables.
+void HallSensorAny::update() {
+  // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed
+  if (use_interrupt){
+    noInterrupts();
+  }else{
+    A_active = digitalRead(pinA);
+    B_active = digitalRead(pinB);
+    C_active = digitalRead(pinC);
+    updateState();
+  }
+
+  angle_prev_ts = pulse_timestamp;
+  long last_electric_rotations = electric_rotations;
+  int8_t last_electric_sector = electric_sector;
+  if (use_interrupt) interrupts();
+  angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ;
+  full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr);
+  if (last_print_type != hall_type)
+  {
+    last_print_type = hall_type;
+    switch (hall_type)
+    {
+    case HallType::HALL_120 :
+      SIMPLEFOC_DEBUG("HALL: Found type: HALL_120");
+      break;
+    case HallType::HALL_60A :
+    SIMPLEFOC_DEBUG("HALL: Found type: HALL_60A");
+      break;
+    case HallType::HALL_60B :
+      SIMPLEFOC_DEBUG("HALL: Found type: HALL_60B");
+        break;
+    case HallType::HALL_60C :
+      SIMPLEFOC_DEBUG("HALL: Found type: HALL_60C");
+      break;
+
+    default:
+      SIMPLEFOC_DEBUG("HALL: Type unknown! Wtf!");
+      break;
+    }
+
+  }
+
+}
+
+
+
+/*
+	Shaft angle calculation
+  TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost
+*/
+float HallSensorAny::getSensorAngle() {
+  return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ;
+}
+
+/*
+  Shaft velocity calculation
+  function using mixed time and frequency measurement technique
+*/
+float HallSensorAny::getVelocity(){
+  noInterrupts();
+  long last_pulse_timestamp = pulse_timestamp;
+  long last_pulse_diff = pulse_diff;
+  interrupts();
+  if (last_pulse_diff == 0 || ((long)(_micros() - last_pulse_timestamp) > last_pulse_diff*2) ) { // last velocity isn't accurate if too old
+    return 0;
+  } else {
+    return direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f);
+  }
+
+}
+
+int HallSensorAny::needsSearch()
+{
+  return hall_type == HallType::UNKNOWN;
+}
+
+// HallSensorAny initialisation of the hardware pins 
+// and calculation variables
+void HallSensorAny::init(){
+  // initialise the electrical rotations to 0
+  electric_rotations = 0;
+
+  // HallSensorAny - check if pullup needed for your HallSensorAny
+  if(pullup == Pullup::USE_INTERN){
+    pinMode(pinA, INPUT_PULLUP);
+    pinMode(pinB, INPUT_PULLUP);
+    pinMode(pinC, INPUT_PULLUP);
+  }else{
+    pinMode(pinA, INPUT);
+    pinMode(pinB, INPUT);
+    pinMode(pinC, INPUT);
+  }
+
+    // init hall_state
+  A_active = digitalRead(pinA);
+  B_active = digitalRead(pinB);
+  C_active = digitalRead(pinC);
+  updateState();
+
+  pulse_timestamp = _micros();
+
+  // we don't call Sensor::init() here because init is handled in HallSensorAny class.
+}
+
+// function enabling hardware interrupts for the callback provided
+// if callback is not provided then the interrupt is not enabled
+void HallSensorAny::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){
+  // attach interrupt if functions provided
+
+  // A, B and C callback
+  if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE);
+  if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
+  if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE);
+
+  use_interrupt = true;
+}

src/encoders/hall_any/HallSensorAny.h

@@ -0,0 +1,112 @@
+#pragma once
+
+#include "Arduino.h"
+#include "common/base_classes/Sensor.h"
+
+class HallSensorAny: public Sensor{
+ public:
+
+
+    enum class HallType : uint8_t
+    {
+      HALL_120 = 0,
+      HALL_60C = 0b001,
+      HALL_60B = 0b010,
+      HALL_60A = 0b100,
+      UNKNOWN = 0b111
+    };
+
+    /**
+    HallSensorAny class constructor
+    @param encA  HallSensor A pin
+    @param encB  HallSensor B pin
+    @param encC  HallSensor C pin
+    @param pp  pole pairs  (e.g hoverboard motor has 15pp and small gimbals often have 7pp)
+    @param hall_60deg Indicate if the hall sensors are 60 degrees apart electrically (means that they can all be one or off at the same time). In 60deg mode, B needs to lead, so you may need to swap the connections until you find one that works
+    */
+    HallSensorAny(int encA, int encB, int encC, int pp, HallType hall_type = HallType::UNKNOWN);
+
+    /** HallSensor initialise pins */
+    void init();
+    /**
+     *  function enabling hardware interrupts for the HallSensor channels with provided callback functions
+     *  if callback is not provided then the interrupt is not enabled
+     * 
+     * @param doA pointer to the A channel interrupt handler function
+     * @param doB pointer to the B channel interrupt handler function
+     * @param doIndex pointer to the Index channel interrupt handler function
+     * 
+     */
+    void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doC)() = nullptr);
+
+    //  HallSensor interrupt callback functions
+    /** A channel callback function */
+    void handleA();
+    /** B channel callback function */
+    void handleB();
+    /** C channel callback function */
+    void handleC();
+
+
+    // pins A and B
+    int pinA; //!< HallSensor hardware pin A
+    int pinB; //!< HallSensor hardware pin B
+    int pinC; //!< HallSensor hardware pin C
+    bool use_interrupt; //!< True if interrupts have been attached
+    HallType hall_type, last_print_type; //!< Connectivity of hall sensor. The type indicates the pin to be swapped. Hall120 has no swapped pin
+
+    // HallSensor configuration
+    Pullup pullup; //!< Configuration parameter internal or external pullups
+    int cpr;//!< HallSensor cpr number
+
+    // Abstract functions of the Sensor class implementation
+    /** Interrupt-safe update */
+    void update() override;
+    /** get current angle (rad) */
+    float getSensorAngle() override;
+    /**  get current angular velocity (rad/s) */
+    float getVelocity() override;
+
+    /** 
+     * returns 0 if it does need search for absolute zero
+     * 0 - magnetic sensor (& encoder with index which is found)
+     * 1 - ecoder with index (with index not found yet)
+     */
+    int needsSearch() override;
+
+
+    // whether last step was CW (+1) or CCW (-1).  
+    Direction direction;
+    Direction old_direction;
+
+    void attachSectorCallback(void (*onSectorChange)(int a) = nullptr);
+
+    //last unique previous states, 0 = recent, used to detect the hall type
+    volatile uint8_t previous_states[3]; 
+    // the current 3bit state of the hall sensors, without any line flipped
+    volatile int8_t hall_state_raw;
+    // the current sector of the sensor. Each sector is 60deg electrical
+    volatile int8_t electric_sector;
+    // the number of electric rotations
+    volatile long electric_rotations;
+    // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts)
+    volatile long total_interrupts; 
+
+    // variable used to filter outliers - rad/s
+    float velocity_max = 1000.0f;
+
+  private:
+
+    Direction decodeDirection(int oldState, int newState);
+    void updateState();
+
+    volatile unsigned long pulse_timestamp;//!< last impulse timestamp in us
+    volatile int A_active; //!< current active states of A channel
+    volatile int B_active; //!< current active states of B channel
+    volatile int C_active; //!< current active states of C channel
+
+    // function pointer for on sector change call back
+    void (*onSectorChange)(int sector) = nullptr;
+
+    volatile long pulse_diff;
+};