diff --git a/src/encoders/hall_any/HallSensorAny.cpp b/src/encoders/hall_any/HallSensorAny.cpp new file mode 100644 index 0000000..b4bf9e9 --- /dev/null +++ b/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(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((0b111 - previous_states[0] ^ previous_states[2])%8); + previous_states[0] ^= static_cast(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; +} diff --git a/src/encoders/hall_any/HallSensorAny.h b/src/encoders/hall_any/HallSensorAny.h new file mode 100644 index 0000000..1d94554 --- /dev/null +++ b/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; +};