/*
 * Gyro_XV7011BB.c
 *
 *  Created on: July 12, 2020
 *      Author: lqh
 */
#include "Gyro_XV7011BB.h"
#include "main.h"

/* XV7011BB Gyroscope registers addresses */
#define GYRO_DSP_CTL1            0x01
#define GYRO_DSP_CTL2            0x02
#define GYRO_DSP_CTL3            0x03
#define GYRO_STS_RD              0x04
#define GYRO_SLP_IN              0x05
#define GYRO_SLP_OUT             0x06
#define GYRO_ST_BY               0x07
#define GYRO_TEMP_RD             0x08
#define GYRO_SW_RST              0x09
#define GYRO_DAT_ACCON           0x0A
#define GYRO_OUT_CTL1            0x0B
#define GYRO_AUTO_C              0x0C
#define GYRO_DSP_RES             0x0D
#define GYRO_MEM_LOAD            0x1B
#define GYRO_TSDATA_FORMAT       0x1C
#define GYRO_IF_CTL              0x1F

int Gyro_XV7011BB_init(Gyro_XV7011BB_t *Gyro, const char *name, SPI_DEV_t *dev) {
	uint8_t TxdBuf[2];
	uint8_t RxdBuf[2];
	int rslt;

	Gyro->name = name;
	Gyro->_dev = dev;
	Gyro->read_cnt = 0;
	Gyro->cnt = 0;
	Gyro->last_cnt = 0;
	Gyro->pps = 0;

	SPI_DEV_begin(dev, -1);

	//Software Reset Chip(Command: Just Specifying an Address)
	TxdBuf[0] = GYRO_SW_RST;  //bit7=0,means write; bit7=1, means read
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 1, NULL, 0, 2);
	SPI_DEV_unselect(dev);

	//Waite 200ms To Be sure The GYRO ReStart Up
	HAL_Delay(200);

	//Disnable IIC, Enable 4-wire SPI
	TxdBuf[0] = GYRO_IF_CTL; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
	TxdBuf[1] = 0;  //SPISel=0(4-wire SPI), IIC_EN=0(Disnable)
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
	SPI_DEV_unselect(dev);

	//Set LPF(DSP Setting2)
	TxdBuf[0] = GYRO_DSP_CTL2; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
	TxdBuf[1] = 0x23;  //LPF Order = 4, LPF=50Hz
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
	SPI_DEV_unselect(dev);

	//Set ODR(DSP Setting3)
	TxdBuf[0] = GYRO_DSP_CTL3; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
	TxdBuf[1] = 0x01;  //ODR=FS/2
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
	SPI_DEV_unselect(dev);

	//Set Angular rate data format:24bit
	TxdBuf[0] = GYRO_OUT_CTL1; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
	TxdBuf[1] = 0x5;  //DataFormat=1(24bit), OutCtl[1:0]=01(Angular rate output)
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
	SPI_DEV_unselect(dev);

	//Set Temperature data format:12bit
	TxdBuf[0] = GYRO_TSDATA_FORMAT; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
	TxdBuf[1] = 0x4B;  //DataFormat=10(12bit), MISO/SA0=10
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
	SPI_DEV_unselect(dev);

	//Sleep Out(Start)  (Command: Just Specifying an Address)
	TxdBuf[0] = GYRO_SLP_OUT; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 1, NULL, 0, 2);
	SPI_DEV_unselect(dev);

	//Init GYRO Global Variable
	Gyro->Gyro = 0;
	Gyro->Temp = 0;

	//based on config
	Gyro->lsb_per_deg_sec = 17920.0f;
	Gyro->lsb_per_degC = 16.0f;

	//Waite 200ms To Be sure The GYRO Sensor Start Up
	HAL_Delay(200);

	//verify spi communication and device start
	TxdBuf[0] = (GYRO_STS_RD | 0xA0); //bit7=1,means Read; bit6/5=01,means 1st GYRO
	SPI_DEV_select(Gyro[0]._dev);
	SPI_DEV_transfer(Gyro[0]._dev, TxdBuf, 2, RxdBuf, 2, 2);
	SPI_DEV_unselect(Gyro[0]._dev);

	if ((RxdBuf[1]&0x07) == 0x01)	{
		rslt = 0;
	}
	else {
		rslt = -1;
	}

	SPI_DEV_end(dev);


	return rslt;
}

int Gyro_XV7011BB_update(Gyro_XV7011BB_t *Gyro) {
	uint8_t TxdBuf[4];
	uint8_t RxdBuf[4];
	int32_t Temp;
	int32_t Gyr;

	if (SPI_DEV_begin(Gyro->_dev, 1)) //½øÈëÁÙ½çÇø
			{
		TxdBuf[0] = (GYRO_DAT_ACCON | 0xA0); //bit7=1,means Read; bit6/5=01,means 1st GYRO
		SPI_DEV_select(Gyro[0]._dev);
		SPI_DEV_transfer(Gyro[0]._dev, TxdBuf, 4, RxdBuf, 4, 2);
		SPI_DEV_unselect(Gyro[0]._dev);

		Gyr = ((RxdBuf[1] << 16) | (RxdBuf[2] << 8) | RxdBuf[3]);
		if ((Gyr & 0x00800000) != 0)   //Sign-extending for raw data
				{
			Gyr |= 0xFF000000;
		}

		//Read Tempture of GYRO1
		TxdBuf[0] = (GYRO_TEMP_RD | 0xA0); //bit7=1,means Read; bit6/5=01,means 1st GYRO
		SPI_DEV_select(Gyro[0]._dev);
		SPI_DEV_transfer(Gyro[0]._dev, TxdBuf, 3, RxdBuf, 3, 2);
		SPI_DEV_unselect(Gyro[0]._dev);

		Temp = ((RxdBuf[1] << 4) | (RxdBuf[2] >> 4));
		if ((Temp & 0x00000800) != 0)   //Sign-extending for raw data
				{
			Temp |= 0xFFFFF000;
		}
		SPI_DEV_end(Gyro->_dev);

		taskENTER_CRITICAL();
		Gyro[0].Gyro += Gyr;
		Gyro[0].Temp += Temp;
		Gyro->read_cnt++;
		taskEXIT_CRITICAL();

		Gyro->cnt++;
		return true;
	}

	return false;
}

int Gyro_XV7011BB_read(Gyro_XV7011BB_t *Gyro, float rollrate[1], float Temp[1]) {
	int cnt;

	cnt = Gyro->read_cnt;
	if (cnt > 0) {
		taskENTER_CRITICAL();
		rollrate[0] = Gyro[0].Gyro / (cnt*Gyro->lsb_per_deg_sec);
		Temp[0] = Gyro[0].Temp / (cnt*Gyro->lsb_per_degC);
		Gyro->Gyro = 0;
		Gyro->Temp = 0;
		Gyro->read_cnt = 0;
		taskEXIT_CRITICAL();
	}
	return cnt;
}

void Gyro_XV7011BB_stats(Gyro_XV7011BB_t *Gyro) {
	Gyro->pps = Gyro->cnt - Gyro->last_cnt;
	Gyro->last_cnt = Gyro->cnt;
}