File content as of revision 36:11766b5da6ed:

#include "includes.h"
#include "sensors.h"
#include "lut.h"

AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a, 
                             float cal1_b, float cal2_b, float offset) {
    _in_a = new AnalogIn(pin_a);
    _in_b = new AnalogIn(pin_b);
    _cal1_a = cal1_a;
    _cal2_a = cal2_a;
    _cal1_b = cal1_b;
    _cal2_b = cal2_b;
    _offset = offset;
}

float AnalogHallPositionSensor::GetPosition() {
    float angle = 0.0f;
    float ascaled = 2 * (((float) *_in_a - _cal1_a) / (_cal2_a - _cal1_a) - 0.5f);
    float bscaled = 2 * (((float) *_in_b - _cal1_b)/ (_cal2_b - _cal1_b) - 0.5f);
    
    float x = bscaled / ascaled;
    
    unsigned int index = (abs(x) / ATAN_UPPER_BOUND) * ATAN_TABLE_SIZE;
    
    if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
    
    if(ascaled < 0){
        if(bscaled < 0) angle = 90 - arctan[index];
        if(bscaled > 0) angle = 90 + arctan[index];
    } 
       
    if(ascaled>0){
        if(bscaled > 0) angle = 270 - arctan[index];
        if(bscaled < 0) angle = 270 + arctan[index];
    }
    
    if(angle > 360.0f) angle = 360.0f;
    if(angle < 0) angle = 0;
    
    angle -= _offset;
    if (angle < 0.0f) angle += 360.0f;
    if (angle >= 360.0f) angle -= 360.0f;
    
    return angle;
}