File content as of revision 54:e8d9bc885723:

#include "includes.h"
#include "sensors.h"
#include "filters.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;
    _time_upd_ticker = new Ticker();
    _time_upd_ticker->attach_us(this, &AnalogHallPositionSensor::upd_function, 50);
    _last_time = 0.0f;
    _last_position = 0.0f;
    _speed = 0.0f;
    GetPosition(); //first speed result is wrong, need to do this once
}

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;
    
    float delta_t = _time - _last_time;
    float delta_angle = -(angle - _last_position); //backwards, for now
    _last_time = _time;
    _last_position = angle;
    if (delta_angle < 0.0f) return angle;
    _speed = delta_angle / delta_t;
    return angle;
}