diff --git a/src/encoders/linearhall/LinearHall.cpp b/src/encoders/linearhall/LinearHall.cpp index ad2efae..919e93f 100644 --- a/src/encoders/linearhall/LinearHall.cpp +++ b/src/encoders/linearhall/LinearHall.cpp @@ -7,20 +7,29 @@ __attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) *b = analogRead(hallB); } -LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ +LinearHall::LinearHall(int _hallA, int _hallB, int _pp, SensorSpacing _sensor_spacing){ centerA = 512; centerB = 512; pinA = _hallA; pinB = _hallB; pp = _pp; electrical_rev = 0; + amplitude_ratio = 1.0f; + sensor_spacing = _sensor_spacing; prev_reading = 0; } -float LinearHall::getSensorAngle() { +float LinearHall::readSensors() { ReadLinearHalls(pinA, pinB, &lastA, &lastB); - //offset readings using center values, then compute angle - float reading = _atan2(lastA - centerA, lastB - centerB); + float a = lastA - centerA, b = (lastB - centerB) * amplitude_ratio; + if (sensor_spacing != SensorSpacing::_90) + b = (sensor_spacing==SensorSpacing::_60?-a:a) * _1_SQRT3 + b * _2_SQRT3; // Clarke transform, as in CurrentSense::getABCurrents + + return _atan2(a, b); +} + +float LinearHall::getSensorAngle() { + float reading = readSensors(); //handle rollover logic between each electrical revolution of the motor if (reading > prev_reading) { @@ -50,7 +59,7 @@ float LinearHall::getSensorAngle() { return result; } -void LinearHall::init(int _centerA, int _centerB) { +void LinearHall::init(int _centerA, int _centerB, float _amplitude_ratio) { // 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. @@ -60,11 +69,11 @@ void LinearHall::init(int _centerA, int _centerB) { centerA = _centerA; centerB = _centerB; + amplitude_ratio = _amplitude_ratio; //establish initial reading for rollover handling electrical_rev = 0; - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - prev_reading = _atan2(lastA - centerA, lastB - centerB); + prev_reading = readSensors(); } void LinearHall::init(FOCMotor *motor) { @@ -77,11 +86,22 @@ void LinearHall::init(FOCMotor *motor) { //pinMode(pinA, INPUT); //pinMode(pinB, INPUT); - int minA, maxA, minB, maxB; + // Get the initial reading, or time out if either of the sensors fails to give a nonzero value after 100ms + int minA = 0, maxA = 0, minB = 0, maxB = 0; + int32_t start = millis(); + while(minA == 0 || minB == 0) { + if ((int32_t)(millis() - start) >= 100) { + if(minA) SIMPLEFOC_DEBUG("LinearHall::init failed. Sensor B not responding."); + else if(minB) SIMPLEFOC_DEBUG("LinearHall::init failed. Sensor A not responding."); + else SIMPLEFOC_DEBUG("LinearHall::init failed. Sensors not responding."); + return; + } - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - minA = maxA = centerA = lastA; - minB = maxB = centerB = lastB; + _delay(2); + 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++) @@ -105,8 +125,15 @@ void LinearHall::init(FOCMotor *motor) { _delay(2); } + motor->setPhaseVoltage(0, 0, angle); + + amplitude_ratio = (float)(maxA - minA) / (float)(maxB - minB); + + SIMPLEFOC_DEBUG("LinearHall centerA: ", centerA); + SIMPLEFOC_DEBUG("LinearHall centerB: ", centerB); + SIMPLEFOC_DEBUG("LinearHall amplitude_ratio: ", amplitude_ratio); //establish initial reading for rollover handling electrical_rev = 0; - prev_reading = _atan2(lastA - centerA, lastB - centerB); + prev_reading = readSensors(); } diff --git a/src/encoders/linearhall/LinearHall.h b/src/encoders/linearhall/LinearHall.h index d17035e..7fc8598 100644 --- a/src/encoders/linearhall/LinearHall.h +++ b/src/encoders/linearhall/LinearHall.h @@ -6,23 +6,29 @@ // 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); + enum SensorSpacing { + _60 = -1, // 60 degree spacing, equivalent to 120 degrees with one sensor flipped to opposite polarity + _90 = 0, // When one sensor is centered on a magnet, the other is half way between magnets + _120 = 1, // Sensors spaced 120 electrical degrees, same as digital halls use + }; - void init(int centerA, int centerB); // Initialize without moving motor + LinearHall(int hallA, int hallB, int pp, SensorSpacing sensor_spacing = SensorSpacing::_90); + + void init(int centerA, int centerB, float _amplitude_ratio = 1.0f); // Initialize without moving motor void init(class FOCMotor *motor); // Move motor to find center values + int centerA; + int centerB; + int lastA, lastB; + int electrical_rev; + float amplitude_ratio; // Correction factor if one sensor is slightly farther from the magnets + SensorSpacing sensor_spacing; + + protected: + float readSensors(); + // 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. @@ -30,15 +36,9 @@ class LinearHall: public Sensor{ // 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; }; diff --git a/src/encoders/linearhall/README.md b/src/encoders/linearhall/README.md new file mode 100644 index 0000000..2845614 --- /dev/null +++ b/src/encoders/linearhall/README.md @@ -0,0 +1,47 @@ +# LinearHall + +by [@nanoparticle](https://github.com/nanoparticle), [@XieMaster](https://github.com/XieMaster) and [@dekutree64](https://github.com/dekutree64) + +This sensor class is for two linear hall effect sensors such as 49E to sense the rotor magnets of an outrunner or a diametric magnet mounted on the shaft (set pp to 1). The sensors can be spaced 60, 90 or 120 electrical degrees apart. 90 degrees will give a slightly more accurate reading (if one is centered on a rotor magnet, the other is half way between magnets), but sometimes it's convenient to position them 120 degrees like digital hall sensors, or 60 degrees which is equivalent to 120 with one sensor flipped to opposite polarity. + +Optimal distance of sensors to magnets is typically between 1mm and 3mm. Too close and the reading will saturate and become trapezoidal, too far and resolution diminishes. It can still work fairly well with as little as ±50 ADC units of resolution. + +The Arduino function analogRead has poor performance on some platforms such as STM32 and RP2350. The ReadLinearHalls function is declared "weaK" so you can override it with custom ADC code without having to modify the library code. + +Please also see our [forum thread](https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959) on this topic, and this PDF for more detail on sensor placement https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 + + +## Example usage +```c++ +#include +#include +#include + +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +LinearHall sensor = LinearHall(A0, A1, 11, LinearHall::SensorSpacing::_90); + +void setup() { + Serial.begin(115200); + motor.useMonitoring(Serial); // LinearHall uses this to print out the calibration results + + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + motor.linkSensor(&sensor); + motor.initFOC(); +} +``` + +## Future work + +- High-performance ADC configurations on platforms with poor analogRead performance. +- Support for redundancy using 3 sensors spaced 120 degrees. Probably best to make a subclass. \ No newline at end of file