File content as of revision 10:36a2131f636c:

/*
 * As5047.cpp
 * Copyright (c) 2017, ZHAW
 * All rights reserved.
 *
 *  Created on: 03.05.2017
 *      Author: Quentin Mettraux
 */

#include "As5047.h"

using namespace std;

const int As5047::kNumSensorBits        = 14;      // 14-bits sensor
const uint16_t As5047::kCountsPerRev    = 0x4000;  // 2**NUM_SENSOR_BITS
const uint16_t As5047::kMask            = 0x3FFF;  // 2**NUM_SENSOR_BITS - 1
const int As5047::kParity               = 1;       // even parity

const int As5047::kSpiFrequency         = 1000000; // AS5047 max 10 MHz
const int As5047::kSpiBitsPerTransfer   = 8;
const int As5047::kSpiMode              = 1;

const float As5047::kDegPerRev          = 360.0f;  // 360 degrees/rev
const float As5047::kRadPerRev          = 6.28318530718f; // 2*pi rad/rev

/**
 * Creates an object of AS5047 sensor;
 * @param mosi: pinname of the mosi pin of the spi communication
 * @param miso: pinname of the miso pin of the spi communication
 * @param sck: pinname of the clock pin of the spi communication
 * @param cs: pinname of the chip select pin of the spi communication
 */
As5047::As5047(PinName mosi, PinName miso, PinName sck, PinName cs) : chip_(cs), spi_(mosi, miso, sck), thread(osPriorityHigh, STACK_SIZE)
{
    DeselectChip();

    spi_.format(kSpiBitsPerTransfer, kSpiMode);
    spi_.frequency(kSpiFrequency);

    read_buffer_  = 0;
    angle_buffer_ = 0;
    angle_offset_ = 0;
    directions_= true;

    last_command_ = AS_CMD_NOP;

    thread.start(callback(this, &As5047::run));
    ticker.attach(callback(this, &As5047::sendSignal), PERIOD);
}


/**
 * Destructor, memory deallocation
 */
As5047::~As5047()
{

    ticker.detach();
}

/**
 * This method is called by the ticker timer interrupt service routine.
 * It sends a signal to the thread to make it run again.
 */
void As5047::sendSignal()
{
    thread.signal_set(signal);
}

/**
 * Parity check
 * @param n: integer to check
 * @return: true if ok
 */
bool As5047::CheckParity(int n)
{
    int parity = n;
    for(int i=1; i <= kNumSensorBits+1; ++i) {
        n >>= 1;
        parity ^= n;
    }
    return (parity & kParity) == 0;
}

/**
 * Update the buffer with angular measurements
 * NOTE 1:
 *  If the last command sent through Transfer was *not* AS_CMD_ANGLE
 *  then we need an additional Transfer; this takes more time!
 *  This should not occur, since Transfer is not *yet* used elsewhere.
 * NOTE 2:
 *  We run a parity check on the results from the transfer. We only
 *  update the angle_buffer_ with values that pass the parity check.
 */
void As5047::run()
{
    while (true) {
        
        // wait for the periodic signal

        thread.signal_wait(signal);

        // ensure that the new results indeed will be angles
        if (last_command_ != AS_CMD_ANGLE) {
            Transfer(AS_CMD_ANGLE);
        }

        // update the read buffer
        Transfer(AS_CMD_ANGLE);

        // update the angle buffer with parity checked values

        if (CheckParity(read_buffer_)) {
            // only update angles when parity is correct
            angle_buffer_ = read_buffer_;
        }

    }
}

/**
 * @return: read_buffer_
 */
uint16_t As5047::get_read_buffer()
{
    return read_buffer_;
}

/**
 * @return: angle_buffer_
 */
uint16_t As5047::get_angle_buffer()
{
    return angle_buffer_;
}

/**
 * @return: angle_offet_
 */
uint16_t As5047::get_angle_offset()
{
    return angle_offset_;
}

/**
 * @return: directions_
 */
bool As5047::get_directions_()
{
    return directions_;
}

/**
 * You get the angles from two UpdateAngleBuffer() calls before
 * @return: 14 bits absolute position
 */
int As5047::getAngle()
{
    int ans = ((int) (angle_buffer_ & kMask)) - angle_offset_;
    return directions_?ans:-ans;
}

/**
 * You get the angles from two UpdateAngleBuffer() calls before
 * @return: revolution ratio in [0,1]
 */
float As5047::getAngleRatio()
{
    return (float) getAngle() / kCountsPerRev;
}

/**
 * You get the angles from two UpdateAngleBuffer() calls before
 * @return: angle in degrees
 */
float As5047::getAngleDegrees()
{
    return getAngleRatio() * kDegPerRev;
}

/**
 * You get the angles from two UpdateAngleBuffer() calls before
 * @return: angle in radians
 */
float As5047::getAngleRadians()
{
    return getAngleRatio() * kRadPerRev;
}

/**
 * Set direction for  sensor
 * @param dir: true positive, false negative
 */
void As5047::setDirection(bool dir)
{
    directions_ = dir;
}


/**
 * Set offset for a sensor
 * @param offset: offset in counts [0,2**14-1]
 */
void As5047::setOffset(uint16_t offset)
{
    angle_offset_ = offset;
}

/**
 * Set offset for sensor
 * @param offset_ratio: offset in ratio in [0,1]
 */
void As5047::setOffsetRatio(float offset_ratio)
{
    setOffset(offset_ratio*kCountsPerRev);
}

/**
 * Set offset for sensor
 * @param offset_degrees: offset in degrees in [0,360]
 */
void As5047::setOffsetDegrees(float offset_degrees)
{
    setOffsetRatio(offset_degrees / kDegPerRev);
}

/**
 * Set offset for sensor
 * @param offset_radians: offset in radians in [0,2*pi]
 */
void As5047::setOffsetRadians(float offset_radians)
{
    setOffsetRatio(offset_radians / kRadPerRev);
}

/**
 * Select (low) chip, and wait 1 us (at least 350 ns)
 */
void As5047::SelectChip()
{
    chip_.write(0);
    wait_us(1);
}

/**
 * Deselect (high) chip, and wait 1 us (at least 350 ns)
 */
void As5047::DeselectChip()
{
    chip_.write(1);
    wait_us(1);
}

/**
 * SPI transfer to sensors
 * @param cmd: Command to send
 */
void As5047::Transfer(As5047Command cmd)
{
    SelectChip();
    spi_.lock();
    read_buffer_  = spi_.write(cmd>>8) << 8;
    read_buffer_ |= spi_.write(cmd & 0x00FF);
    spi_.unlock();
    DeselectChip();
    last_command_ = cmd;
}