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

/* ADXL355 registers addresses */
#define ADXL355_DEVID_AD                 0x00
#define ADXL355_DEVID_MST                0x01
#define ADXL355_PARTID                   0x02
#define ADXL355_REVID                    0x03
#define ADXL355_STATUS                   0x04
#define ADXL355_FIFO_ENTRIES             0x05
#define ADXL355_TEMP2                    0x06
#define ADXL355_TEMP1                    0x07
#define ADXL355_XDATA3                   0x08
#define ADXL355_XDATA2                   0x09
#define ADXL355_XDATA1                   0x0A
#define ADXL355_YDATA3                   0x0B
#define ADXL355_YDATA2                   0x0C
#define ADXL355_YDATA1                   0x0D
#define ADXL355_ZDATA3                   0x0E
#define ADXL355_ZDATA2                   0x0F
#define ADXL355_ZDATA1                   0x10
#define ADXL355_FIFO_DATA                0x11
#define ADXL355_OFFSET_X_H               0x1E
#define ADXL355_OFFSET_X_L               0x1F
#define ADXL355_OFFSET_Y_H               0x20
#define ADXL355_OFFSET_Y_L               0x21
#define ADXL355_OFFSET_Z_H               0x22
#define ADXL355_OFFSET_Z_L               0x23
#define ADXL355_ACT_EN                   0x24
#define ADXL355_ACT_THRESH_H             0x25
#define ADXL355_ACT_THRESH_L             0x26
#define ADXL355_ACT_COUNT                0x27
#define ADXL355_FILTER                   0x28
#define ADXL355_FIFO_SAMPLES             0x29
#define ADXL355_INT_MAP                  0x2A
#define ADXL355_SYNC                     0x2B
#define ADXL355_RANGE                    0x2C
#define ADXL355_POWER_CTL                0x2D
#define ADXL355_SELF_TEST                0x2E
#define ADXL355_RESET                    0x2F

/** 初始化ADXL加速度计*/
int Acc_ADXL355_init(Acc_ADXL355_t *Acc, const char *name, SPI_DEV_t *dev,
		GPIO_EXIT_t *exti) {
	int rslt;
	uint8_t TxdBuf[4];
	uint8_t RxdBuf[4];
	uint8_t ChipId, MemsId, PartId;  //Read ADXL355 Chip Infomation

	rslt = 0;

	Acc->name = name;
	Acc->_dev = dev;
	Acc->exti = exti;

	Acc->Ax = 0;
	Acc->Ay = 0;
	Acc->Az = 0;
	Acc->At = 0;
	Acc->read_cnt = 0;
	Acc->exti->cnt = 0;

	Acc->cnt = 0;
	Acc->last_cnt = 0;
	Acc->pps = 0;

	SPI_DEV_begin(Acc->_dev, -1);

	/* Quick verification test for boards */
	TxdBuf[0] = ((ADXL355_DEVID_AD << 1) | 0x01); //Read ChipId/MemsId/PartId, Lowbit = 1 means Read
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 4, RxdBuf, 4, 2);
	SPI_DEV_unselect(dev);

	ChipId = RxdBuf[1];
	MemsId = RxdBuf[2];
	PartId = RxdBuf[3];

	//If Target Chip is ADXL355, Set Chip Range & Turn on Accelerometer Mode
	if ((ChipId == 0xAD) && (MemsId == 0x1D) && (PartId == 0xED)) {

		TxdBuf[0] = ADXL355_RANGE << 1; //Write ADXL355_POWER_CTL, Lowbit 0 means write
		TxdBuf[1] = 0x83;  //RANG +-8G, High speed mode, Int1/Int2 active low
		Acc->lsb_per_g = 64000.0f;
		SPI_DEV_select(dev);
		SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
		SPI_DEV_unselect(dev);

		//Set LowPass Filter & ODR
		TxdBuf[0] = ADXL355_FILTER << 1; //Write ADXL355_POWER_CTL, Lowbit 0 means write
		TxdBuf[1] = 0x02; //Disable HPF, LPF=250Hz, ODR=1000Hz
		SPI_DEV_select(dev);
		SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
		SPI_DEV_unselect(dev);

		/* Turn On Meas Accelerometer Mode */
		TxdBuf[0] = ADXL355_POWER_CTL << 1; //Write ADXL355_POWER_CTL, Lowbit 0 means write
		TxdBuf[1] = 0;  //STANDBY bit = 0, Means Turn on Measure Mode
		SPI_DEV_select(dev);
		SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
		SPI_DEV_unselect(dev);
	} else {
		rslt = -1;
	}
	SPI_DEV_end(Acc->_dev);

	if (!rslt)
	{
		int i;
		float N[3];
		float Temp[1];

		TxdBuf[0] = ADXL355_SELF_TEST << 1; //Write ADXL355_POWER_CTL, Lowbit 0 means write
		TxdBuf[1] = 1;  // test mode 1, test force 0
		SPI_DEV_begin(Acc->_dev, -1);
		SPI_DEV_select(Acc->_dev);
		SPI_DEV_transfer(Acc->_dev, TxdBuf, 2, NULL, 0, 2);
		SPI_DEV_unselect(Acc->_dev);
		SPI_DEV_end(Acc->_dev);
		for (i=0;i<5;++i)
		{
			HAL_Delay(5);
			Acc_ADXL355_update(Acc);
		}
		for (i=0;i<3;++i)
		{
			N[i] = 9999;
		}
		Temp[0] = 9999;
		Acc_ADXL355_read(Acc, N, Temp);
		for (i=0;i<3;++i)
		{
			if (N[i] > 0.2f || N[i] < -0.2f)
			{
				rslt |= (1>>i);
			}
		}
		if (Temp[0] > 150.0f || Temp[0] < -40.0f)
		{
			rslt |= (1>>3);
		}

		TxdBuf[0] = ADXL355_SELF_TEST << 1; //Write ADXL355_POWER_CTL, Lowbit 0 means write
		TxdBuf[1] = 3;  // test mode 1, test force 0
		SPI_DEV_begin(Acc->_dev, -1);
		SPI_DEV_select(Acc->_dev);
		SPI_DEV_transfer(Acc->_dev, TxdBuf, 2, NULL, 0, 2);
		SPI_DEV_unselect(Acc->_dev);
		SPI_DEV_end(Acc->_dev);
		for (i=0;i<5;++i)
		{
			HAL_Delay(5);
			Acc_ADXL355_update(Acc);
		}
		for (i=0;i<3;++i)
		{
			N[i] = 9999;
		}
		Temp[0] = 9999;
		Acc_ADXL355_read(Acc, N, Temp);
		if (N[0] > 0.5f || N[0] < 0.2f)
		{
			rslt |= (1>>0);
		}
		if (N[1] > 0.5f || N[1] < 0.2f)
		{
			rslt |= (1>>1);
		}
		if (N[2] > 1.5f || N[2] < 1.1f)
		{
			rslt |= (1>>2);
		}
		if (Temp[0] > 150.0f || Temp[0] < -40.0f)
		{
			rslt |= (1>>3);
		}

		TxdBuf[0] = ADXL355_SELF_TEST << 1; //Write ADXL355_POWER_CTL, Lowbit 0 means write
		TxdBuf[1] = 0;  // test mode 1, test force 0
		SPI_DEV_begin(Acc->_dev, -1);
		SPI_DEV_select(Acc->_dev);
		SPI_DEV_transfer(Acc->_dev, TxdBuf, 2, NULL, 0, 2);
		SPI_DEV_unselect(Acc->_dev);
		SPI_DEV_end(Acc->_dev);
	}

	return rslt;
}

/** 关闭ADXL355芯片 */
bool Acc_ADXL355_Standby(Acc_ADXL355_t *Acc) {
	uint8_t TxdBuf[2];

	TxdBuf[0] = ADXL355_POWER_CTL << 1; //Write ADXL355_POWER_CTL, Lowbit 0 means write
	TxdBuf[1] = 1;  //STANDBY bit = 1, turn to standby mode 待机模式

	if (!SPI_DEV_begin(Acc->_dev, 10)) {
		return false;
	}

	SPI_DEV_select(Acc->_dev);
	SPI_DEV_transfer(Acc->_dev, TxdBuf, 2, NULL, 0, 2);
	SPI_DEV_unselect(Acc->_dev);

	SPI_DEV_end(Acc->_dev);
	return true;
}

/** 重新启动ADXL355 */
bool Acc_ADXL355_Awake(Acc_ADXL355_t *Acc) {
	uint8_t TxdBuf[2];

	TxdBuf[0] = ADXL355_POWER_CTL << 1; //Write ADXL355_POWER_CTL, Lowbit 0 means write
	TxdBuf[1] = 0;  //STANDBY bit = 0, turn to measurement mode 测量模式

	if (!SPI_DEV_begin(Acc->_dev, 10)) {
		return false;
	}

	SPI_DEV_select(Acc->_dev);
	SPI_DEV_transfer(Acc->_dev, TxdBuf, 2, NULL, 0, 2);
	SPI_DEV_unselect(Acc->_dev);

	SPI_DEV_end(Acc->_dev);
	return true;
}

/** 读取ADXL355值 */
bool Acc_ADXL355_update(Acc_ADXL355_t *Acc) {
	uint8_t TxdBuf[16];
	uint8_t RxdBuf[16];
	int i;
	int32_t TempAcceleratorData;
	bool rslt;

	if (SPI_DEV_begin(Acc->_dev, 0)) //进入临界区
	{
		TxdBuf[0] = ((ADXL355_TEMP2 << 1) | 1); //Read PARTID, Lowbit = 1 means Read
		for (i = 1; i < 12; i++) {
			TxdBuf[i] = 0xAA;     //Dummy Byte 虚拟字节
		}
		SPI_DEV_select(Acc->_dev);
		rslt = SPI_DEV_transfer(Acc->_dev, TxdBuf, 12, RxdBuf, 12, 3);
		SPI_DEV_unselect(Acc->_dev);

		SPI_DEV_end(Acc->_dev);  //退出临界区

		Acc->exti->cnt = 0; //中断计数清零

		if (rslt) {
			Acc->cnt++;
			taskENTER_CRITICAL();
			Acc->read_cnt++;

			//Data combination for Ax Raw Data
			TempAcceleratorData = (((RxdBuf[3] << 16) | (RxdBuf[4] << 8)
					| RxdBuf[5]) >> 4);
			if ((TempAcceleratorData & 0x00080000) != 0) //Sign-extending for raw data
					{
				TempAcceleratorData |= 0xFFF00000;
			}
			Acc->Ax += TempAcceleratorData; //Accumulated Value of ax  x轴

			//Data combination for Ay Raw Data
			TempAcceleratorData = (((RxdBuf[6] << 16) | (RxdBuf[7] << 8)
					| RxdBuf[8]) >> 4);
			if ((TempAcceleratorData & 0x00080000) != 0) //Sign-extending for raw data
					{
				TempAcceleratorData |= 0xFFF00000;
			}
			Acc->Ay += TempAcceleratorData; //Accumulated Value of ay   y轴

			//Data combination for Az Raw Data
			TempAcceleratorData = (((RxdBuf[9] << 16) | (RxdBuf[10] << 8)
					| RxdBuf[11]) >> 4);
			if ((TempAcceleratorData & 0x00080000) != 0) //Sign-extending for raw data
					{
				TempAcceleratorData |= 0xFFF00000;
			}
			Acc->Az += TempAcceleratorData; //Accumulated Value of az   z轴

			//Data combination for Temperature Raw Data
			TempAcceleratorData = ((RxdBuf[1] << 8) | RxdBuf[2]);
			if ((TempAcceleratorData & 0x00000800) != 0) //Sign-extending for raw data
					{
				TempAcceleratorData |= 0xFFFFF000;
			}
			Acc->At += TempAcceleratorData; //Accumulated Value of Temperature 温度
			taskEXIT_CRITICAL();
		}
	} else {
		rslt = false;
	}

	return rslt;
}

int Acc_ADXL355_read(Acc_ADXL355_t *Acc, float N[3], float Temp[1]) {
	int cnt = Acc->read_cnt;
	if (cnt > 0)  //采样次数完成，ADXL355待机，准备上报数据
	{
		taskENTER_CRITICAL();
		N[0] = Acc->Ax / (cnt * Acc->lsb_per_g); //ax
		N[1] = Acc->Ay / (cnt * Acc->lsb_per_g); //ay
		N[2] = Acc->Az / (cnt * Acc->lsb_per_g); //az
		Temp[0] = 25 - (((Acc->At / cnt) - 1852) / 9.05f);   //temp

		Acc->Ax = 0;
		Acc->Ay = 0;
		Acc->Az = 0;
		Acc->At = 0;

		Acc->read_cnt = 0;
		taskEXIT_CRITICAL();
	}
	return cnt;
}

void Acc_ADXL355_stats(Acc_ADXL355_t *Acc) {
	Acc->pps = Acc->cnt - Acc->last_cnt;
	Acc->last_cnt = Acc->cnt;
}