diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 64c5e48c..8dd15a9d 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -1,17 +1,27 @@ #include "HallSensor.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 }; /* HallSensor(int hallA, int hallB , int cpr, int index) - hallA, hallB, hallC - HallSensor A, B and C pins - pp - pole pairs */ -HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ +HallSensor::HallSensor(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; @@ -43,14 +53,50 @@ void HallSensor::handleC() { */ void HallSensor::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) return; + 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(); - hall_state = new_hall_state; + 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 = ELECTRIC_SECTORS[hall_state]; + 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 @@ -110,6 +156,31 @@ void HallSensor::update() { 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; + } + + } + } @@ -139,6 +210,11 @@ float HallSensor::getVelocity(){ } +int HallSensor::needsSearch() +{ + return hall_type == HallType::UNKNOWN; +} + // HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 94053e0f..a90be18b 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -6,20 +6,29 @@ #include "../common/foc_utils.h" #include "../common/time_utils.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 }; class HallSensor: public Sensor{ public: + + + enum class HallType : uint8_t + { + HALL_120 = 0, + HALL_60C = 0b001, + HALL_60B = 0b010, + HALL_60A = 0b100, + UNKNOWN = 0b111 + }; + /** HallSensor 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 index index pin number (optional input) + @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 */ - HallSensor(int encA, int encB, int encC, int pp); + HallSensor(int encA, int encB, int encC, int pp, HallType hall_type = HallType::UNKNOWN); /** HallSensor initialise pins */ void init(); @@ -47,7 +56,8 @@ class HallSensor: public Sensor{ int pinA; //!< HallSensor hardware pin A int pinB; //!< HallSensor hardware pin B int pinC; //!< HallSensor hardware pin C - int use_interrupt; //!< True if interrupts have been attached + 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 @@ -61,14 +71,24 @@ class HallSensor: public Sensor{ /** 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); - // the current 3bit state of the hall sensors - volatile int8_t hall_state; + //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 @@ -93,7 +113,6 @@ class HallSensor: public Sensor{ void (*onSectorChange)(int sector) = nullptr; volatile long pulse_diff; - };