/*Includes ----------------------------------------------*/
#include "tool.h"
#include "bsp_time.h"
#include "nrf_delay.h"
#include "bll_imu.h"
#include "hal_qma.h"
#include "exception.h"
#include "system.h"

static uint8_t QmaSetFlag =0;//Qma配置成功标志位

static QMA_ACC_ODR_e NowQmafreq  = QMA_ACC_ODR_OFF;
static QMA_ACC_ODR_e ReqQmafreq  = QMA_ACC_ODR_OFF;

/*API --------------------------------------------------------------*/

QmaFrequency hal_get_QmaFrequency(void){
	
	QmaFrequency freq;
	
	if(QMA_ACC_ODR_OFF == NowQmafreq) freq = QMA_OFF;
	else if(QMA_ACC_ODR_12HZ5 == NowQmafreq) freq = QMA_12_5HZ;
	else if(QMA_ACC_ODR_104HZ == NowQmafreq) freq = QMA_104HZ;
	
	return freq;
	
}

void hal_qma_setFrequency(QmaFrequency freq){
	QMA_ACC_ODR_e mIFREQ = QMA_ACC_ODR_OFF;
	if(QMA_OFF == freq) mIFREQ = QMA_ACC_ODR_OFF;
	else if(QMA_12_5HZ == freq) mIFREQ = QMA_ACC_ODR_12HZ5;
	else if(QMA_104HZ == freq)mIFREQ = QMA_ACC_ODR_104HZ;
	
	else return;
	
	if(NowQmafreq == mIFREQ)return ;
	ReqQmafreq = mIFREQ;
	QmaSetFlag =1;
}

static void hal_qma_Porcess(void)
{
	static uint8_t state=0;
	switch(state){
		case 0:
			    if(1 == QmaSetFlag){
						 drv_qma_power_off();
						 if(QMA_ACC_ODR_OFF == ReqQmafreq){
							 QmaSetFlag =0;
							 NowQmafreq  = ReqQmafreq;
						 }
						 else state =1;
					}
		      break;
	  case 1:
			    if(0 == drv_qma_power_on()){
					  state =2;
					}
					else{
						Except_TxError(EXCEPT_QMA7981,"QMA7981 set fail");
						DEBUG_LOG("qma7981 config FAIL !\r\n");
						state =1;
					}
			    break;
	  case 2:
			    if(0 == drv_qma_set_acc_odr(ReqQmafreq)){
						DEBUG_LOG("qma7981 config success !\r\n");
						Except_TxError(EXCEPT_QMA7981,"QMA7981 set ok");
						NowQmafreq  = ReqQmafreq;
						QmaSetFlag =0;
					}else{
						DEBUG_LOG("qma7981 config FAIL !\r\n");
						Except_TxError(EXCEPT_QMA7981,"QMA7981 set fail");
					}
					state =0;
			    break;
		default:state =0;break;
	}
}

int	hal_qma_Init(void)
{
	int ret = -1;
	
	ret = drv_qma_Init();
	
	Process_Start(100,"qma_err",hal_qma_Porcess);
	
//	nrf_delay_ms(20);
	
//	ret += drv_qma_set_acc_odr(QMA_ACC_ODR_12HZ5);

//	if(ret < 0)ret = drv_qma_set_acc_odr(QMA_ACC_ODR_12HZ5);//再配置一次
	
	drv_qma_power_off();
	
	ReqQmafreq  = QMA_ACC_ODR_OFF;
	
	NowQmafreq  = QMA_ACC_ODR_OFF;
	
	return ret;
}