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

/* MS5611 Presure registers addresses */
#define PRESURE_RESET            0x1E
#define PRESURE_CVD1_OSR256      0x40
#define PRESURE_CVD1_OSR512      0x42
#define PRESURE_CVD1_OSR1024     0x44
#define PRESURE_CVD1_OSR2048     0x46
#define PRESURE_CVD1_OSR4096     0x48
#define PRESURE_CVD2_OSR256      0x50
#define PRESURE_CVD2_OSR512      0x52
#define PRESURE_CVD2_OSR1024     0x54
#define PRESURE_CVD2_OSR2048     0x56
#define PRESURE_CVD2_OSR4096     0x58
#define PRESURE_ADC_READ         0x00
#define PRESURE_PROM_SETUP       0xA0
#define PRESURE_PROM_COF1        0xA2
#define PRESURE_PROM_COF2        0xA4
#define PRESURE_PROM_COF3        0xA6
#define PRESURE_PROM_COF4        0xA8
#define PRESURE_PROM_COF5        0xAA
#define PRESURE_PROM_COF6        0xAC
#define PRESURE_PROM_CRC         0xAE

static unsigned char crc4(unsigned int n_prom[]) {
	int cnt; // simple counter
	unsigned int n_rem; // crc reminder
	unsigned int crc_read; // original value of the crc
	unsigned char n_bit;
	n_rem = 0x00;
	crc_read = n_prom[7]; //save read CRC
	n_prom[7] = (0xFF00 & (n_prom[7])); //CRC byte is replaced by 0
	for (cnt = 0; cnt < 16; cnt++) // operation is performed on bytes
			{ // choose LSB or MSB
		if (cnt % 2 == 1)
			n_rem ^= (unsigned short) ((n_prom[cnt >> 1]) & 0x00FF);
		else
			n_rem ^= (unsigned short) (n_prom[cnt >> 1] >> 8);
		for (n_bit = 8; n_bit > 0; n_bit--) {
			if (n_rem & (0x8000)) {
				n_rem = (n_rem << 1) ^ 0x3000;
			} else {
				n_rem = (n_rem << 1);
			}
		}
	}
	n_rem = (0x000F & (n_rem >> 12)); // // final 4-bit reminder is CRC code
	n_prom[7] = crc_read; // restore the crc_read to its original place
	return (n_rem ^ 0x00);
}

/** MS5611压力补偿算法 */
void Ms5611_Pressure_Compensat(Baro_MS5611_t *Baro) {
	int64_t dT;
	int64_t dT1;
	int64_t OFF, SENS;
	int64_t TmpVar;
	uint64_t UnsignTmpVar;

	dT1 = (Baro->Ms5611_Cof[4] << 8);
	dT = Baro->Ms5611_Raw_Temp - dT1;
	TmpVar = (dT * Baro->Ms5611_Cof[5]) >> 16;
	TmpVar = TmpVar >> 7;
	Baro->temp = 2000 + TmpVar;   //w温度最终输出

	TmpVar = (Baro->Ms5611_Cof[3] * dT) >> 7;
	UnsignTmpVar = (uint64_t) Baro->Ms5611_Cof[1] << 16;
	OFF = (int64_t) UnsignTmpVar + TmpVar;
	TmpVar = (Baro->Ms5611_Cof[2] * dT) >> 8;
	SENS = ((uint64_t) Baro->Ms5611_Cof[0] << 15) + TmpVar;
	TmpVar = (Baro->Ms5611_Raw_Pressure * SENS) >> 16;
	TmpVar = (TmpVar >> 5) - OFF;
	TmpVar = TmpVar >> 15;

	Baro->pressure = (int32_t) TmpVar;
	Baro->seq++;
}

/** MS5611气压高度计初始化 */
int MS5611_Pressure_init(Baro_MS5611_t *Baro, const char *name, SPI_DEV_t *dev) {
	uint8_t TxdBuf[4];
	uint8_t RxdBuf[4];
	unsigned int C[8];
	int rslt;
	int i;

	Baro->name = name;
	Baro->_dev = dev;
	Baro->status = 0;

	Baro->cnt = 0;
	Baro->last_cnt = 0;
	Baro->pps = 0;
	Baro->seq = 0;

	SPI_DEV_begin(dev, -1);

	//Reset Chip(Command: Just Specifying an Address)
	TxdBuf[0] = PRESURE_RESET;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 1, NULL, 0, 2);
	SPI_DEV_unselect(dev);

	//Wait 2.8(5)ms to be sure chip reseted
	HAL_Delay(5);

	//Read PROM Data(Start Measure)
	//Read C1
	TxdBuf[0] = PRESURE_PROM_COF1;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 3, RxdBuf, 3, 2);
	SPI_DEV_unselect(dev);
	Baro->Ms5611_Cof[0] = (RxdBuf[1] << 8 | RxdBuf[2]);

	//Read C2
	TxdBuf[0] = PRESURE_PROM_COF2;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 3, RxdBuf, 3, 2);
	SPI_DEV_unselect(dev);
	Baro->Ms5611_Cof[1] = (RxdBuf[1] << 8 | RxdBuf[2]);

	//Read C3
	TxdBuf[0] = PRESURE_PROM_COF3;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 3, RxdBuf, 3, 2);
	SPI_DEV_unselect(dev);
	Baro->Ms5611_Cof[2] = (RxdBuf[1] << 8 | RxdBuf[2]);

	//Read C4
	TxdBuf[0] = PRESURE_PROM_COF4;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 3, RxdBuf, 3, 2);
	SPI_DEV_unselect(dev);
	Baro->Ms5611_Cof[3] = (RxdBuf[1] << 8 | RxdBuf[2]);

	//Read C5
	TxdBuf[0] = PRESURE_PROM_COF5;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 3, RxdBuf, 3, 2);
	SPI_DEV_unselect(dev);
	Baro->Ms5611_Cof[4] = (RxdBuf[1] << 8 | RxdBuf[2]);

	//Read C6
	TxdBuf[0] = PRESURE_PROM_COF6;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 3, RxdBuf, 3, 2);
	SPI_DEV_unselect(dev);
	Baro->Ms5611_Cof[5] = (RxdBuf[1] << 8 | RxdBuf[2]);

	//Read CRC
	TxdBuf[0] = PRESURE_PROM_CRC;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 3, RxdBuf, 3, 2);
	SPI_DEV_unselect(dev);

	C[7] = (RxdBuf[1] << 8 | RxdBuf[2]);

	//Read Setup State
	TxdBuf[0] = PRESURE_PROM_SETUP;
	SPI_DEV_select(dev);
	SPI_DEV_transfer(dev, TxdBuf, 3, RxdBuf, 3, 2);
	SPI_DEV_unselect(dev);

	Baro->Ms5611_Setup = (RxdBuf[1] << 8 | RxdBuf[2]);

	//check CRC and setup
	C[0] = Baro->Ms5611_Setup;
	for (i = 0; i < 6; ++i) {
		C[i + 1] = Baro->Ms5611_Cof[i];
	}

	if ((crc4(C) & 0x0F) != (C[7] & 0x0F)) {
		rslt = -1;
	} else {
		//Convert D2 Start 1 time(osr = 4096, 0.012mbar)
		TxdBuf[0] = PRESURE_CVD2_OSR4096;
		SPI_DEV_select(dev);
		SPI_DEV_transfer(dev, TxdBuf, 1, RxdBuf, 1, 2);
		SPI_DEV_unselect(dev);

		//Wait 10ms to be sure D2 conver over
		HAL_Delay(10);

		//Read D2 information
		TxdBuf[0] = PRESURE_ADC_READ;
		SPI_DEV_select(dev);
		SPI_DEV_transfer(dev, TxdBuf, 4, RxdBuf, 4, 2);
		SPI_DEV_unselect(dev);

		Baro->Ms5611_Raw_Temp = ((RxdBuf[1] << 16) | (RxdBuf[2] << 8)
				| RxdBuf[3]);

		//Convert D1 Start 1 time(osr = 4096)
		TxdBuf[0] = PRESURE_CVD1_OSR4096;
		SPI_DEV_select(dev);
		SPI_DEV_transfer(dev, TxdBuf, 1, NULL, 0, 2);
		SPI_DEV_unselect(dev);

		//Wait 10ms to be sure D1 conver over
		HAL_Delay(10);

		//Read D1 information
		TxdBuf[0] = PRESURE_ADC_READ;
		SPI_DEV_select(dev);
		SPI_DEV_transfer(dev, TxdBuf, 4, RxdBuf, 4, 2);
		SPI_DEV_unselect(dev);

		Baro->Ms5611_Raw_Pressure = ((RxdBuf[1] << 16) | (RxdBuf[2] << 8)
				| RxdBuf[3]);

		//Preasure & Temperature compensat
		Ms5611_Pressure_Compensat(Baro);

		//Init Preasure State Flag
		Baro->PresurReadFlag = MS5611_D1_CONVERTING;
		//Convert D1 Start 1 time(osr = 4096)
		TxdBuf[0] = PRESURE_CVD1_OSR4096;
		SPI_DEV_select(dev);
		SPI_DEV_transfer(dev, TxdBuf, 1, NULL, 0, 2);
		SPI_DEV_unselect(dev);

		rslt = 0;
	}
	SPI_DEV_end(dev);

	return rslt;
}

/** MS5611数据读取 */
int MS5611_Pressure_update(Baro_MS5611_t *Baro) {
	uint8_t TxdBuf[6];
	uint8_t RxdBuf[6];
	uint32_t ms5611_read_data;
	int rslt;

	if (SPI_DEV_begin(Baro->_dev, 0)) //进入临界区
			{
		TxdBuf[0] = PRESURE_ADC_READ;
		SPI_DEV_select(Baro->_dev);
		SPI_DEV_transfer(Baro->_dev, TxdBuf, 4, RxdBuf, 4, 2);
		SPI_DEV_unselect(Baro->_dev);

		if (((RxdBuf[0] << 16) | (RxdBuf[1] << 8) | RxdBuf[2]) != 0) {
			ms5611_read_data =
					((RxdBuf[1] << 16) | (RxdBuf[2] << 8) | RxdBuf[3]);

			if (Baro->PresurReadFlag == MS5611_D1_CONVERTING) { //read d1, and start d2
				Baro->Ms5611_Raw_Pressure = ms5611_read_data;
				Baro->PresurReadFlag = MS5611_D2_CONVERTING;
				TxdBuf[0] = PRESURE_CVD2_OSR4096; //Convert D2 Start 1 time(osr = 4096)
			} else {   //read d2, and start d1
				Baro->Ms5611_Raw_Temp = ms5611_read_data;
				Baro->PresurReadFlag = MS5611_D1_CONVERTING;
				TxdBuf[0] = PRESURE_CVD1_OSR4096; //Convert D1 Start 1 time(osr = 4096)
			}

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

			//Preasure & Temperature compensat
			Ms5611_Pressure_Compensat(Baro);
			rslt = 0;
			Baro->cnt++;
		} else {
			rslt = -2;
		}

		SPI_DEV_end(Baro->_dev);
	} else {
		rslt = -1;
	}

	return rslt;
}

void MS5611_Pressure_stats(Baro_MS5611_t *Baro) {
	Baro->pps = Baro->cnt - Baro->last_cnt;
	Baro->last_cnt = Baro->cnt;
}