/*
 * Angle_AS5600.c
 *
 *  Created on: Jun 22, 2020
 *      Author: matth
 */

#include "Angle_AS5600.h"

bool Ang_AS5600_init(Ang_AS5600_t *ang, const char *name, I2C_DEV_t *dev)
{
	ang->name = name;
	ang->_dev = dev;
	ang->angle = 0;

	return true;
}

bool Ang_AS5600_update(Ang_AS5600_t *ang)
{
	bool rslt;
	rslt = false;
	uint8_t buff[4];

    if (!I2C_DEV_begin(ang->_dev,1)) {
        return rslt;
    }

    if (I2C_DEV_read_registers(ang->_dev, AS5600_STATUS, buff, 3))
    {
    	ang->status = ((buff[0] & 0x38) >> 2);
    	if (ang->status == 0x8)
    	{
    	   ang->status = 0u;
           ang->angle = (((buff[1]&0x0F)<<8)+buff[2])*0.087890625f;
    	   rslt = true;
    	}
    }
    else
    {
    	ang->status = 1u;
    }

    I2C_DEV_end(ang->_dev);
    return rslt;
}