DJI-Gimbal-FOC/src/linearHallSensor.cpp

#include "linearHallSensor.h"

#include <math.h>

float norm(float x, float in_min, float in_max, float out_min = -1.0, float out_max = 1.0)
{
    return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}

LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2)
{
    _analogPin1 = ch1;
    _analogPin2 = ch2;
}

inline uint32_t min(uint32_t arr[], int n)
{
    uint32_t m = 0;
    for (int i = 0; i < n; i++)
        m = arr[i] < m ? arr[i] : m;
    return m;
}

inline uint32_t max(uint32_t *arr, int n)
{
    uint32_t m = 0;
    for (int i = 0; i < n; i++)
        m = arr[i] > m ? arr[i] : m;
    return m;
}

inline uint32_t mean(uint32_t *arr, int n)
{
    uint32_t m = 0;
    for (int i = 0; i < n; i++)
        m += arr[i];
    return m / n;
}

inline uint32_t std_var(uint32_t *arr, int n)
{
    uint32_t s = 0;
    uint32_t m = mean(arr, n);
    for (int i = 0; i < n; i++)
        s += (arr[i] - m) * (arr[i] - m);
    s /= (uint32_t)n;
    s = sqrt(s);
    return s;
}

void init_arr(uint32_t *arr, int n, uint32_t val)
{
    for (int i = 0; i < n; i++)
        arr[i] = val;
}

void print_arr(uint32_t *arr, int n)
{
    Serial.printf("[");
    for (int i = 0; i < n; i++)
        Serial.printf("%lu,", arr[i]);
    Serial.printf("]");
}

void LinearHallSensor::init(BLDCMotor motor)
{
    MotionControlType prevController = motor.controller;
    motor.controller = MotionControlType::angle_openloop;
    float prevVoltageLimit = motor.voltage_limit;
    motor.voltage_limit = 2.0;

    // Swipe motor to search hard end and find max analog values of sensors
    bool endFound = false;
    const float step = 0.005;
    const uint8_t epsilon = 3;
    float currentPosition = 0.0;

    const uint8_t N = 20;
    uint32_t senseA[N];
    uint32_t senseB[N];

    int ptr = 0;

    init_arr(senseA, N, 0);
    init_arr(senseB, N, 0);

    senseA[ptr] = analogRead(_analogPin1);
    senseB[ptr] = analogRead(_analogPin2);

    _minCh1 = senseA[ptr];
    _minCh2 = senseB[ptr];
    _maxCh1 = _minCh1;
    _maxCh2 = _minCh2;

    Serial.println("\t- Finding end stops");
    while (!endFound)
    {
        senseA[ptr] = analogRead(_analogPin1);
        senseB[ptr] = analogRead(_analogPin2);

        // Check if new extremes sensor values
        if (senseA[ptr] > _maxCh1)
            _maxCh1 = senseA[ptr];
        else if (senseA[ptr] < _minCh1)
            _minCh1 = senseA[ptr];

        if (senseB[ptr] > _maxCh2)
            _maxCh2 = senseB[ptr];
        else if (senseB[ptr] < _minCh2)
            _minCh2 = senseB[ptr];

        if (std_var(senseA, N) < epsilon && std_var(senseB, N) < epsilon)
            endFound = true;

        ptr = (ptr + 1) % N;

        // Move to new position
        currentPosition += step;
        motor.move(currentPosition);
        delay(3);
    }
    _maxPositionEndValue = currentPosition - (step * (ptr > N ? N : ptr));
    currentPosition = _maxPositionEndValue - M_PI / 16;
    delay(100);
    motor.move(currentPosition);
    Serial.println("\t- Found first end stop : Max position = " + String(_maxPositionEndValue));
    delay(100);

    // Swipe motor to search other hard end, and find eventually new max analog values of sensors
    endFound = false;

    init_arr(senseA, N, 0);
    init_arr(senseB, N, 0);
    ptr = 0;

    while (!endFound)
    {
        senseA[ptr] = analogRead(_analogPin1);
        senseB[ptr] = analogRead(_analogPin2);

        // Check if new extremes sensor values
        if (senseA[ptr] > _maxCh1)
            _maxCh1 = senseA[ptr];
        else if (senseA[ptr] < _minCh1)
            _minCh1 = senseA[ptr];

        if (senseB[ptr] > _maxCh2)
            _maxCh2 = senseB[ptr];
        else if (senseB[ptr] < _minCh2)
            _minCh2 = senseB[ptr];

        if (std_var(senseA, N) < epsilon && std_var(senseB, N) < epsilon)
            endFound = true;

        ptr = (ptr + 1) % N;

        // Move to new position
        currentPosition -= step;
        motor.move(currentPosition);
        delay(3);
    }
    _minPositionEndValue = currentPosition + (step * (ptr > N ? N : ptr));
    Serial.println("\t- Found second end stop : Min position = " + String(_minPositionEndValue));
    delay(100);

    // Set min offset with 50 values average
    float sum = 0.0;
    _offset = 0.0;
    for (uint8_t i = 0; i <= 49; i++)
        sum += readSensorCallback();
    _offset = sum / 50.0;

    Serial.println("\t- maxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2));
    Serial.println("\t- Offset: " + String(_offset));

    // Read max angle
    for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.0025)
    {
        motor.move(i);
        delay(1);
        _maxSensor = readSensorCallback();
    }
    delay(250);

    // Set max take average value of 50 values
    sum = 0.0;
    for (uint8_t i = 0; i <= 49; i++)
        sum += readSensorCallback();
    _maxSensor = sum / 50.0;

    Serial.println("\t- Max angle: " + String(_maxSensor));

    // Recovering previous controller type and go to 0
    for (float i = _maxPositionEndValue; i >= (0); i = i - 0.005)
    {
        motor.move(i);
        delay(1);
        readSensorCallback();
    }
    motor.controller = prevController;
    motor.voltage_limit = prevVoltageLimit;
}

float dist_angle(float newAngle, float prevAngle)
{
    float diff = newAngle - prevAngle;
    while (diff < (-M_PI))
        diff += 2 * M_PI;
    while (diff > M_PI)
        diff -= 2 * M_PI;
    return diff;
}

float LinearHallSensor::readSensorCallback()
{
    // Update variables
    uint32_t currentCh1 = analogRead(_analogPin1);
    uint32_t currentCh2 = analogRead(_analogPin2);

    //  Normalise variable between -1 and 1
    float normCh1 = norm((float)currentCh1, _minCh1, _maxCh1);
    float normCh2 = norm((float)currentCh2, _minCh2, _maxCh2);

    _currentPosition = atan2f(normCh2, normCh1);
    _estimatePosition += dist_angle(_currentPosition, _prevPosition);

    _prevPosition = _currentPosition;
    return (_estimatePosition / 4.0 - _offset);
}

float LinearHallSensor::getMaxAngle()
{
    return _maxSensor;
}