File content as of revision 155:7c6005933d4c:

#include "mbed.h"
#include "PositionSensor.h"
#include "config_motor.h"
#include <math.h>

/*
 * CPR: counts per revolution (4x lines per revolution)
 * offset: mechanical position offset in radians
 */

PositionSensorEncoder::PositionSensorEncoder(int cpr, float offset) {
    _cpr = cpr;
    _offset = offset;
    _lobes = (int) (RESOLVER_LOBES);
    
    _valid = false;
    _rotations = 0;
    
    __GPIOA_CLK_ENABLE();
    __GPIOB_CLK_ENABLE();
 
    GPIOB->MODER   |= GPIO_MODER_MODER3_1;           
    GPIOB->OTYPER  |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ;                 
    GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ;     
    GPIOB->AFR[0]  |= 0x00001000 ;                                          
    
    GPIOA->MODER   |= GPIO_MODER_MODER15_1;           
    GPIOA->OTYPER  |= GPIO_OTYPER_OT_15;                 
    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15;     
    GPIOA->AFR[1]  |= 0x10000000 ;
   
    __TIM2_CLK_ENABLE();
 
    TIM2->CR1   = 0x0001;
    TIM2->SMCR  = TIM_ENCODERMODE_TI12;
    TIM2->CCMR1 = 0x0101;
    TIM2->CCMR2 = 0x0000;
    TIM2->CCER  = 0x0011;
    TIM2->PSC   = 0x0000;
    TIM2->ARR   = 0xffffffff;
  
    TIM2->CNT = 0;
    
    ZPulse = new InterruptIn(PB_12);
    ZSense = new DigitalIn(PB_12);
    ZPulse->enable_irq();
    ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
    ZPulse->mode(PullDown);
}

/*
 * Returns mechanical position in radians
 */
 
float PositionSensorEncoder::GetMechPosition() {
    return GetUnlimitedElecPosition() / 3.0f;
}

/*
 * Returns electrical position in radians
 */
 
float PositionSensorEncoder::GetElecPosition() {
    int raw = TIM2->CNT;
    if (raw < 0) raw += _cpr;
    if (raw >= _cpr) raw -= _cpr;
    float ep = fmod((POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset)), 2 * PI);
    if (ep < 0) {
        return ep + 2 * PI;
    } else {
        return ep;
    }
}

/*
 * Return the electrical position in radians (no limit, INT_MAX * 2 * PI max value)
 */
 
float PositionSensorEncoder::GetUnlimitedElecPosition() {
    int raw = TIM2->CNT;
    float ep = POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset);
    return ep + _rotations * 2 * PI;
}

void PositionSensorEncoder::ZeroEncoderCount(void){
    if (ZSense->read() == 1){
        if (ZSense->read() == 1){
            TIM2->CNT = 0;
            int dir = TIM2->CR1 & (1 << 4);
            if (!_valid) {
                _valid = true;
                return;
            }
            if (dir == 0) {//upcounting
                _rotations++;
            } else {//downcounting
                _rotations--;
            }
        }
    }
}

bool PositionSensorEncoder::IsValid() {
    return _valid;
}