shoe_mcu/hal/hal_imu/hal_attitude.c

/********************** Í·Îļþ *************************/
#include "usr.h"
#include "hal_imu.h"
#include "hal_step.h"
#include "system.h"
#include "bsp_time.h"
#include "hal_ble_client.h"
#include "hal_flash.h"
#include "hal_attitude.h"
#include "drv_iic_01.h"
#include "drv_mpu9250.h"
#include "bmi160_support.h"

static uint8_t Attmode =0;
static uint8_t QeqAttMode =0;
static uint8_t ReadTemp =0;

typedef struct _hal_att{
	int16_t  acc[3];
	int16_t  gry[3];
	int16_t  temperature;
}hal_att_t;

static hal_att_t mIhal_att_t ={0};

int16_t hal_att_GetAcc_Z(void)
{
	return mIhal_att_t.acc[2];
}

int16_t* hal_att_GetAcc(void)
{
	return mIhal_att_t.acc;
}

int16_t* hal_att_GetGry(void)
{
	return mIhal_att_t.gry;
}

int16_t hal_att_Get_temperature(void)
{
	return mIhal_att_t.temperature;
}

void hal_att_read_acc(void){
	
  #if MPU9250_USE_ENABLE && MPU9250_READ_ENABLE
	MPU9250_Read_Acc((int16_t*)acc);
	#endif
	
	#if BMI160_USE_ENABLE && BMI160_READ_ENABLE
	struct bmi160_accel_t 	accelxyz;
	bmi160_read_accel_xyz(&accelxyz);
	mIhal_att_t.acc[0] = accelxyz.x;
	mIhal_att_t.acc[1] = accelxyz.y;
	mIhal_att_t.acc[2] = accelxyz.z;
	#endif  
}

void hal_att_read_Acc_Gry_temp(void){
	
	#if MPU9250_USE_ENABLE && MPU9250_READ_ENABLE
	if(!MPU9250_Read((int16_t*)mIhal_att_t.acc,(int16_t*)mIhal_att_t.gry,(int16_t*)mIhal_att_t.mag,(int16_t*)&mIhal_att_t.temperature)){
		SEGGER_RTT_printf(0,"===============================>MPU9250_Read fail\n");
	}
	#endif
	
	#if BMI160_USE_ENABLE && BMI160_READ_ENABLE
	uint32_t 	err = 0;
	struct bmi160_accel_t 	accelxyz;
	struct bmi160_gyro_t		gyroxyz;

	err = bmi160_read_gyro_xyz(&gyroxyz);
	err += bmi160_read_accel_xyz(&accelxyz);
//	s16		 									temp;
//	err += bmi160_get_temp(&temp);
	if(err){
		SEGGER_RTT_printf(0,"===============================>hal_attitude fail\n");
	}else{
		mIhal_att_t.acc[0] = accelxyz.x;
		mIhal_att_t.acc[1] = accelxyz.y;
		mIhal_att_t.acc[2] = accelxyz.z;
		mIhal_att_t.gry[0] = gyroxyz.x;
		mIhal_att_t.gry[1] = gyroxyz.y;
		mIhal_att_t.gry[2] = gyroxyz.z;
//		mIhal_att_t.temperature = temp;
	}
	#endif
}

static void hal_att_Initialize(void){
	#if MPU9250_USE_ENABLE
	IIC_01_Init();
	for(int i=0;i<5;i++){
		nrf_delay_ms(500);
		if(MPU9250_Reset()) break;
		app_err_Set(ERR_NUM_IMU_MPU9250,1);
		SEGGER_RTT_printf(0,"MPU9250_Reset fail!!\n");
	}
	for(int i=0;i<20;i++){
		nrf_delay_ms(200);
		if(MPU9250_Acc_Init()) break;
		app_err_Set(ERR_NUM_IMU_MPU9250,1);
		SEGGER_RTT_printf(0,"MPU9250_Acc_Init fail!!\n");
	}
	for(int i=0;i<20;i++){
		nrf_delay_ms(500);
		if(MPU9250_MageInit()) break;
		app_err_Set(ERR_NUM_IMU_MPU9250,1);
		SEGGER_RTT_printf(0,"MPU9250_MageInit fail!!\n");
	}
  #endif
	
	#if BMI160_USE_ENABLE
	
	u8 v_accel_power_mode_stat_u8 =0;
	for(uint8_t i=0;i<20;i++){
		nrf_delay_ms(200);
		bmi160_initialize_sensor(ACCEL_PEDOMETER);
		bmi160_get_accel_power_mode_stat(&v_accel_power_mode_stat_u8);
		if(2 == v_accel_power_mode_stat_u8) {
			Attmode = Att_Lowpower;
			QeqAttMode = Att_Lowpower;
			break;//µÍ¹¦ºÄacc
		}
		app_err_Set(ERR_NUM_IMU_BMI160,1);
		SEGGER_RTT_printf(0,"bmi160_initialize_sensor(ACCEL_PEDOMETER) fail!!\n");
	}
	#endif
}

static void hal_attitude_process(void){
	#if MPU9250_USE_ENABLE
	if(QeqAttMode != Attmode && ReadTemp <=21){
		if(QeqAttMode == Att_GameMode){
			if(MPU9250_Init())Attmode = QeqAttMode;
			else {
				ReadTemp++;
				SEGGER_RTT_printf(0,"MPU9250 into game fail\n");
			}
		}else{
			if(MPU9250_Acc_Init())Attmode = QeqAttMode;
			else {
				ReadTemp++;
				SEGGER_RTT_printf(0,"MPU9250 into lower fail\n");
			}
		}
	}
	#endif
	
	#if  BMI160_USE_ENABLE
	uint8_t mode =0;
	if(QeqAttMode != Attmode && ReadTemp <=21){
	    if(QeqAttMode == Att_GameMode){
	       mode = STANDARD_UI_IMU;
	    }else{
	       mode = ACCEL_PEDOMETER_FIFO;
	    }
			if(0 ==  bmi160_initialize_sensor(mode)){
				Attmode = QeqAttMode;
			}else if(ReadTemp++ >=20){
	      SEGGER_RTT_printf(0,"bmi160_initialize_sensor,mode: fail!!\n");
			}
	}
	#endif
}

uint8_t hal_attitude_GetMode(uint8_t mode){
	if(Attmode == mode)return 1;
	else return 0;
}

void hal_attitude_setMode(uint8_t mode){
	#if MPU9250_USE_ENABLE
	if(QeqAttMode != mode)MPU9250_Reset();
	#endif
	QeqAttMode = mode;
	ReadTemp =0;
}

void hal_attitude_init(void)
{
	SPI_Init();
	hal_att_Initialize();
	Process_Start(200,"hal_attitude_process",hal_attitude_process);
}