123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884 |
- #include "bsp_spi.h"
- #include "bsp_time.h"
- #include "nrf_delay.h"
- #include "drv_lsm6ds3tr_c.h"
- #include "exception.h"
- #include "system.h"
- #define BOOT_TIME 15
- #define WAIT_TIME_A 100
- #define WAIT_TIME_G_01 150
- #define WAIT_TIME_G_02 50
- #define MIN_ST_LIMIT_mg 90.0f
- #define MAX_ST_LIMIT_mg 1700.0f
- #define MIN_ST_LIMIT_mdps 150000.0f
- #define MAX_ST_LIMIT_mdps 700000.0f
- #define ST_PASS 1U
- #define ST_FAIL 0U
- #define OP_TIMEOUT 200
- #define ACC_OUT_XYZ_WORD_NUM 3
- #define GRY_OUT_XYZ_WORD_NUM 3
- #define MAG_OUT_XYZ_WORD_NUM 3
- #define TIMESTAMP_OUT_WORD_NUM 3
- #define QMC6310_DATA_OUT_X_LSB_REG 0x01
- #define HUB_MAG_DEV_ADDR 0x1C
- #define LSM_IIC_QMC_ADDR (0x1c<<1)
- #define LSM_IIC_SA0 PIN_FRONT_SPI_MISO
- #define LSM_IIC_NCS PIN_FRONT_SPI_nCS
- #define LSM_IIC_SCL PIN_FRONT_SPI_SCLK
- #define LSM_IIC_SDA PIN_FRONT_SPI_MOSI
- #define LSM_IIC_SCL_SET nrf_gpio_pin_write(LSM_IIC_SCL,1)
- #define LSM_IIC_SCL_CLR nrf_gpio_pin_write(LSM_IIC_SCL,0)
- #define LSM_IIC_SDA_SET nrf_gpio_pin_write(LSM_IIC_SDA,1)
- #define LSM_IIC_SDA_CLR nrf_gpio_pin_write(LSM_IIC_SDA,0)
- #define LSM_IIC_SDA_READ nrf_gpio_pin_read(LSM_IIC_SDA)
- typedef union
- {
- int16_t i16bit[3];
- uint8_t u8bit[6];
-
- } axis3bit16_u;
- typedef struct drv_lsm6ds3tr_c
- {
- stmdev_ctx_t ctx;
-
- lsm6ds3tr_c_emb_sh_read_t raw_data;
-
- lsm6ds3tr_c_all_sources_t endop;
-
- uint32_t op_timeout;
-
- lsm_data_t cur_data;
-
- drv_lsm_config_param_t cur_param;
-
- } Drv_Lsm6ds3tr_c_t;
- static lsm6ds3tr_c_sh_cfg_read_t sub_qmc6310 = {
- .slv_add = HUB_MAG_DEV_ADDR,
- .slv_subadd = QMC6310_DATA_OUT_X_LSB_REG,
- .slv_len = MAG_OUT_XYZ_WORD_NUM * 2,
- };
- static Drv_Lsm6ds3tr_c_t ob_lsm6ds3tr_c;
- static axis3bit16_u temp_data_raw[4];
- static uint32_t spi_write_error_counter = 0;
- static uint32_t spi_read_error_counter = 0;
- static uint8_t whoamI, rst;
- static int16_t self_check_data_raw[3];
- static float val_st_off[3];
- static float val_st_on[3];
- static float test_val[3];
- static uint8_t st_result;
- static uint8_t drdy;
- static uint8_t i;
- static uint8_t j;
- static void platform_delay_ms(uint32_t ms)
- {
- nrf_delay_ms(ms);
- }
- static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
- {
- int32_t ierror = 0;
-
- if(SPI0_OnlyWriteReg(BOARD_SPI0_CS0_IO, reg, (uint8_t *)bufp, len))
- {
- ierror = -1;
- spi_write_error_counter++;
- }
- return ierror;
- }
- static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
- {
- int32_t ierror = 0;
-
- uint32_t errcode = SPI0_OnlyReadReg(BOARD_SPI0_CS0_IO, reg, bufp, len);
- if(errcode)
- {
- ierror = -1;
- spi_read_error_counter++;
- }
-
- return ierror;
- }
- static void drv_lsm_spi_error_report_process(void)
- {
- char buff[30]={0};
-
- if(spi_write_error_counter > 0)
- {
- sprintf(buff,"spi_write_err,%d\r\n",spi_write_error_counter);
- Except_TxError(EXCEPT_SPI_RW,(const char *)buff);
- }
-
- if(spi_read_error_counter > 0)
- {
- sprintf(buff,"spi_read_err,%d\r\n",spi_read_error_counter);
- Except_TxError(EXCEPT_SPI_RW,(const char *)buff);
- }
- spi_write_error_counter = 0;
- spi_read_error_counter = 0;
- }
- static void LSM_IIC_Start(void)
- {
- LSM_IIC_SDA_SET;
- LSM_IIC_SCL_SET;
- nrf_delay_us(5);
- LSM_IIC_SDA_CLR;
- nrf_delay_us(5);
- LSM_IIC_SCL_CLR;
- }
- static void LSM_IIC_Stop(void)
- {
- LSM_IIC_SCL_CLR;
- LSM_IIC_SDA_CLR;
- nrf_delay_us(5);
- LSM_IIC_SCL_SET;
- LSM_IIC_SDA_SET;
- nrf_delay_us(5);
- }
- static void LSM_IIC_ACK(void)
- {
- LSM_IIC_SCL_CLR;
- nrf_delay_us(1);
- LSM_IIC_SDA_CLR;
- nrf_delay_us(1);
- LSM_IIC_SCL_SET;
- nrf_delay_us(1);
- LSM_IIC_SCL_CLR;
- }
- static void LSM_IIC_NoACK(void)
- {
- LSM_IIC_SCL_CLR;
- LSM_IIC_SDA_SET;
- nrf_delay_us(1);
- LSM_IIC_SCL_SET;
- nrf_delay_us(1);
- LSM_IIC_SCL_CLR;
- }
- static bool LSM_IIC_WaitACK(void)
- {
- uint8_t tim = 0;
- LSM_IIC_SDA_SET;
- nrf_delay_us(1);
- LSM_IIC_SCL_SET;
- while(LSM_IIC_SDA_READ){
- if(++tim>=50){
- LSM_IIC_Stop();
- return false;
- }
- nrf_delay_us(1);
- }
- LSM_IIC_SCL_CLR;
- return true;
- }
- static void LSM_IIC_SendByte(uint8_t _byte)
- {
- uint8_t i = 0;
- for (i=0; i<8; i++){
- LSM_IIC_SCL_CLR;
- nrf_delay_us(5);
- if (_byte&0x80) LSM_IIC_SDA_SET;
- else LSM_IIC_SDA_CLR;
- _byte <<= 1;
- LSM_IIC_SCL_SET;
- nrf_delay_us(5);
- }
- LSM_IIC_SCL_CLR;
- }
- static uint8_t LSM_IIC_RecByte(void)
- {
- uint8_t i = 0;
- uint8_t rec_byte;
-
- LSM_IIC_SDA_SET;
- for (i=0; i<8; i++){
- rec_byte <<= 1;
- LSM_IIC_SCL_CLR;
- nrf_delay_us(5);
- LSM_IIC_SCL_SET;
- nrf_delay_us(5);
- if (LSM_IIC_SDA_READ) rec_byte |= 0x01;
- }
- LSM_IIC_SCL_CLR;
- return rec_byte;
- }
- static bool LSM_IIC_WriteBytes(uint8_t add,uint8_t reg,uint8_t* p,uint8_t len)
- {
- uint8_t i = 0;
- LSM_IIC_Start();
- LSM_IIC_SendByte(add);
- if(!LSM_IIC_WaitACK()) return false;
- LSM_IIC_SendByte(reg);
- if(!LSM_IIC_WaitACK()) return false;
- for(i=0;i<len;i++){
- LSM_IIC_SendByte(p[i]);
- if(!LSM_IIC_WaitACK()) return false;
- }
- LSM_IIC_Stop();
- return true;
- }
- static bool LSM_IIC_ReadBytes(uint8_t add,uint8_t reg,uint8_t* p,uint8_t len)
- {
- uint8_t i = 0;
- LSM_IIC_Start();
- LSM_IIC_SendByte(add);
- if(!LSM_IIC_WaitACK()) return false;
- LSM_IIC_SendByte(reg);
- if(!LSM_IIC_WaitACK()) return false;
- LSM_IIC_Start();
- LSM_IIC_SendByte(add+1);
- if(!LSM_IIC_WaitACK()) return false;
- for(i=0;i<len-1;i++){
- p[i] = LSM_IIC_RecByte(); LSM_IIC_ACK();
- }
- p[i] = LSM_IIC_RecByte(); LSM_IIC_NoACK();
- LSM_IIC_Stop();
- return true;
- }
- static void LSM_IIC_Init(void)
- {
- nrf_gpio_cfg(
- LSM_IIC_SCL,
- NRF_GPIO_PIN_DIR_OUTPUT,
- NRF_GPIO_PIN_INPUT_DISCONNECT,
- NRF_GPIO_PIN_NOPULL,
- NRF_GPIO_PIN_S0S1,
- NRF_GPIO_PIN_NOSENSE);
-
- nrf_gpio_cfg_watcher(LSM_IIC_SCL);
- nrf_gpio_pin_set(LSM_IIC_SCL);
-
- nrf_gpio_cfg(
- LSM_IIC_SDA,
- NRF_GPIO_PIN_DIR_OUTPUT,
- NRF_GPIO_PIN_INPUT_DISCONNECT,
- NRF_GPIO_PIN_NOPULL,
- NRF_GPIO_PIN_S0D1,
- NRF_GPIO_PIN_NOSENSE);
-
- nrf_gpio_cfg_watcher(LSM_IIC_SDA);
- nrf_gpio_pin_set(LSM_IIC_SDA);
-
- nrf_gpio_cfg_output(LSM_IIC_NCS);nrf_gpio_pin_write(LSM_IIC_NCS,1);
-
- nrf_gpio_cfg_output(LSM_IIC_SA0);
- }
- static void LSM_IIC_UnInit(void)
- {
- nrf_gpio_cfg_default(LSM_IIC_SCL);
- nrf_gpio_cfg_default(LSM_IIC_SDA);
- nrf_gpio_cfg_default(LSM_IIC_NCS);
- nrf_gpio_cfg_default(LSM_IIC_SA0);
- }
- static int read_hub(uint8_t device_addr, uint8_t reg, uint8_t* data,uint8_t len)
- {
- int32_t ret;
- uint8_t val;
- lsm6ds3tr_c_odr_fifo_t r_fifo_odr;
- lsm6ds3tr_c_odr_xl_t acc_odr;
- lsm6ds3tr_c_odr_g_t gry_odr;
- lsm6ds3tr_c_ctrl10_c_t ctrl10_c;
-
-
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
- lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&acc_odr);
- if(acc_odr != LSM6DS3TR_C_XL_ODR_OFF)return -1;
- ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_OFF;
-
-
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_OFF);
- lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&gry_odr);
- if(gry_odr != LSM6DS3TR_C_GY_ODR_OFF)return -1;
- ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_OFF;
-
-
- lsm6ds3tr_c_fifo_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_DISABLE);
- lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr);
- if(r_fifo_odr != LSM6DS3TR_C_FIFO_DISABLE) return -1;
- ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_OFF;
-
-
-
- lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_DISABLE);
-
- val = 0xFF;
- lsm6ds3tr_c_sh_master_get(&ob_lsm6ds3tr_c.ctx, &val);
-
- if(val != PROPERTY_DISABLE)return -1;
-
-
-
- lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_DISABLE);
-
- lsm6ds3tr_c_read_reg(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_CTRL10_C,(uint8_t *)&ctrl10_c, 1);
-
- if(ctrl10_c.func_en != PROPERTY_DISABLE)return -1;
-
-
- lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
-
- lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != 1)return -1;
-
-
-
-
- SPI0_Disable();
-
- LSM_IIC_Init();
-
- if(LSM_IIC_ReadBytes(device_addr,reg,data,len))
- {
- ret = 0;
- }
- else
- {
- ret = -1;
- SEGGER_RTT_printf(0,"-------->LSM_IIC_ReadBytes err\n");
- }
-
-
-
- LSM_IIC_UnInit();
-
- SPI0_Init();
-
-
-
- lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,0);
-
- lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != 0)return -1;
-
- return ret;
- }
- static int write_hub(uint8_t device_addr, uint8_t reg, uint8_t* data,uint8_t len)
- {
- int32_t ret;
- uint8_t val;
- lsm6ds3tr_c_odr_fifo_t r_fifo_odr;
- lsm6ds3tr_c_odr_xl_t acc_odr;
- lsm6ds3tr_c_odr_g_t gry_odr;
- lsm6ds3tr_c_ctrl10_c_t ctrl10_c;
-
-
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
- lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&acc_odr);
- if(acc_odr != LSM6DS3TR_C_XL_ODR_OFF)return -1;
- ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_OFF;
-
-
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_OFF);
- lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&gry_odr);
- if(gry_odr != LSM6DS3TR_C_GY_ODR_OFF)return -1;
- ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_OFF;
-
-
- lsm6ds3tr_c_fifo_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_DISABLE);
- lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr);
- if(r_fifo_odr != LSM6DS3TR_C_FIFO_DISABLE) return -1;
- ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_OFF;
-
-
-
- lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_DISABLE);
-
- val = 0xFF;
- lsm6ds3tr_c_sh_master_get(&ob_lsm6ds3tr_c.ctx, &val);
-
- if(val != PROPERTY_DISABLE)return -1;
-
-
-
- lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_DISABLE);
-
- lsm6ds3tr_c_read_reg(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_CTRL10_C,(uint8_t *)&ctrl10_c, 1);
-
- if(ctrl10_c.func_en != PROPERTY_DISABLE)return -1;
-
-
- lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
-
- lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != 1)return -1;
-
-
-
-
- SPI0_Disable();
-
- LSM_IIC_Init();
-
- if(LSM_IIC_WriteBytes(device_addr,reg,data,len))
- {
- ret = 0;
- }
- else
- {
- ret = -1;
- SEGGER_RTT_printf(0,"-------->LSM_IIC_WriteBytes err\n");
- }
-
-
-
- LSM_IIC_UnInit();
-
- SPI0_Init();
-
-
-
- lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,0);
-
- lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != 0)return -1;
-
- return ret;
- }
- static int self_check(void)
- {
- lsm6ds3tr_c_mem_bank_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_USER_BANK);
-
- platform_delay_ms(BOOT_TIME);
-
- lsm6ds3tr_c_device_id_get(&ob_lsm6ds3tr_c.ctx, &whoamI);
- if(whoamI != LSM6DS3TR_C_ID)
- {
- DEBUG_LOG("LSM self_check error!!!\r\n");
- return -1;
- }
-
- lsm6ds3tr_c_reset_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_reset_get(&ob_lsm6ds3tr_c.ctx, &rst);
- ob_lsm6ds3tr_c.op_timeout--;
- } while(rst && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
-
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_52Hz);
-
- lsm6ds3tr_c_xl_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_4g);
-
- platform_delay_ms(WAIT_TIME_A);
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while(!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
-
- memset(val_st_off, 0x00, 3 * sizeof(float));
- for (i = 0; i < 5; i++) {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
- for (j = 0; j < 3; j++) {
- val_st_off[j] += lsm6ds3tr_c_from_fs4g_to_mg(self_check_data_raw[j]);
- }
- }
-
- for (i = 0; i < 3; i++) {
- val_st_off[i] /= 5.0f;
- }
-
- lsm6ds3tr_c_xl_self_test_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ST_NEGATIVE);
-
- platform_delay_ms(WAIT_TIME_A);
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
-
- memset(val_st_on, 0x00, 3 * sizeof(float));
- for (i = 0; i < 5; i++) {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
- for (j = 0; j < 3; j++) {
- val_st_on[j] += lsm6ds3tr_c_from_fs4g_to_mg(self_check_data_raw[j]);
- }
- }
-
- for (i = 0; i < 3; i++) {
- val_st_on[i] /= 5.0f;
- }
-
- for (i = 0; i < 3; i++) {
- test_val[i] = fabs((val_st_on[i] - val_st_off[i]));
- }
-
- st_result = ST_PASS;
-
- for (i = 0; i < 3; i++) {
- if (( MIN_ST_LIMIT_mg > test_val[i] ) ||
- ( test_val[i] > MAX_ST_LIMIT_mg)) {
- st_result = ST_FAIL;
- }
- }
-
- lsm6ds3tr_c_xl_self_test_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ST_DISABLE);
-
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
-
-
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_208Hz);
-
- lsm6ds3tr_c_gy_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_2000dps);
-
- platform_delay_ms(WAIT_TIME_G_01);
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
-
- memset(val_st_off, 0x00, 3 * sizeof(float));
-
- for (i = 0; i < 5; i++) {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
- for (j = 0; j < 3; j++) {
- val_st_off[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
- self_check_data_raw[j]);
- }
- }
-
-
- for (i = 0; i < 3; i++) {
- val_st_off[i] /= 5.0f;
- }
-
- lsm6ds3tr_c_gy_self_test_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ST_POSITIVE);
-
- platform_delay_ms(WAIT_TIME_G_02);
-
- memset(val_st_on, 0x00, 3 * sizeof(float));
- for (i = 0; i < 5; i++) {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
- for (j = 0; j < 3; j++) {
- val_st_on[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
- self_check_data_raw[j]);
- }
- }
-
- for (i = 0; i < 3; i++) {
- val_st_on[i] /= 5.0f;
- }
-
- for (i = 0; i < 3; i++) {
- test_val[i] = fabs((val_st_on[i] - val_st_off[i]));
- }
-
- for (i = 0; i < 3; i++) {
- if (( MIN_ST_LIMIT_mdps > test_val[i] ) ||
- ( test_val[i] > MAX_ST_LIMIT_mdps)) {
- st_result = ST_FAIL;
- }
- }
-
- lsm6ds3tr_c_gy_self_test_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ST_DISABLE);
-
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_OFF);
- if (st_result == ST_PASS) {
- DEBUG_LOG("LSM Self Test - PASS\r\n");
-
- lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
-
- rst = 0Xff;
- lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&rst);
-
- if(rst != 1)return -1;
-
- }else{
- DEBUG_LOG("Self Test - FAIL\r\n");
-
- lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
-
- rst = 0Xff;
- lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&rst);
-
- if(rst != 1)return -1;
-
- return -1;
- }
- return 0;
- }
- int drv_lsm_Init(void)
- {
-
- memset(&ob_lsm6ds3tr_c.cur_data,0,sizeof(ob_lsm6ds3tr_c.cur_data));
- ob_lsm6ds3tr_c.cur_param.acc_fs = LSM_ACC_FS_2G;
- ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_OFF;
- ob_lsm6ds3tr_c.cur_param.gry_fs = LSM_GRY_FS_250DPS;
- ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_OFF;
- ob_lsm6ds3tr_c.cur_param.mag_fs = LSM_MAG_FS_30GS;
- ob_lsm6ds3tr_c.cur_param.mag_odr = LSM_MAG_ODR_OFF;
- ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_OFF;
- ob_lsm6ds3tr_c.cur_param.timestamp_switch = LSM_TIMESTAMP_OFF;
- ob_lsm6ds3tr_c.cur_param.timestamp_resolution = LSM_TIMESTAMP_6MS4;
- ob_lsm6ds3tr_c.cur_param.acc_power_mode = LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
- ob_lsm6ds3tr_c.cur_param.gry_power_mode = LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;
- ob_lsm6ds3tr_c.ctx.read_reg = platform_read;
- ob_lsm6ds3tr_c.ctx.write_reg = platform_write;
- ob_lsm6ds3tr_c.op_timeout = 0;
-
- nrf_gpio_cfg(
- PIN_FRONT_SENSE_POWER,
- NRF_GPIO_PIN_DIR_OUTPUT,
- NRF_GPIO_PIN_INPUT_DISCONNECT,
- NRF_GPIO_PIN_NOPULL,
- NRF_GPIO_PIN_H0H1,
- NRF_GPIO_PIN_NOSENSE);
- nrf_gpio_cfg_output(BOARD_SPI0_MISO_IO);
- nrf_gpio_cfg_output(BOARD_SPI0_MOSI_IO);
- nrf_gpio_cfg_output(BOARD_SPI0_CLK_IO);
- nrf_gpio_cfg_output(PIN_FRONT_SPI_nCS);
- nrf_gpio_pin_write(BOARD_SPI0_MISO_IO,0);
- nrf_gpio_pin_write(BOARD_SPI0_MOSI_IO,0);
- nrf_gpio_pin_write(BOARD_SPI0_CLK_IO,0);
- nrf_gpio_pin_write(PIN_FRONT_SPI_nCS,0);
- nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,0);
- platform_delay_ms(3);
- nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
- platform_delay_ms(3);
-
- SPI0_Init();
-
- Process_Start(1000,"drv_lsm_spi_error_report_process",drv_lsm_spi_error_report_process);
-
- return self_check();
- }
- int drv_lsm_power_off(void)
- {
-
- SPI0_Disable();
-
-
- nrf_gpio_cfg(
- PIN_FRONT_SENSE_POWER,
- NRF_GPIO_PIN_DIR_OUTPUT,
- NRF_GPIO_PIN_INPUT_DISCONNECT,
- NRF_GPIO_PIN_NOPULL,
- NRF_GPIO_PIN_H0H1,
- NRF_GPIO_PIN_NOSENSE);
-
- nrf_gpio_cfg_output(BOARD_SPI0_MISO_IO);
-
- nrf_gpio_pin_write(BOARD_SPI0_MISO_IO,0);
- nrf_gpio_pin_write(BOARD_SPI0_MOSI_IO,0);
- nrf_gpio_pin_write(BOARD_SPI0_CLK_IO,0);
- nrf_gpio_pin_write(PIN_FRONT_SPI_nCS,0);
- nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,0);
-
- return 0;
- }
- int drv_lsm_power_on(void)
- {
- nrf_gpio_pin_write(PIN_FRONT_SENSE_POWER,1);
-
- nrf_gpio_cfg_input(BOARD_SPI0_MISO_IO, (nrf_gpio_pin_pull_t)NRFX_SPIM_MISO_PULL_CFG);
-
-
- memset(&ob_lsm6ds3tr_c.cur_data,0,sizeof(ob_lsm6ds3tr_c.cur_data));
- ob_lsm6ds3tr_c.cur_param.acc_fs = LSM_ACC_FS_2G;
- ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_OFF;
- ob_lsm6ds3tr_c.cur_param.gry_fs = LSM_GRY_FS_250DPS;
- ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_OFF;
- ob_lsm6ds3tr_c.cur_param.mag_fs = LSM_MAG_FS_30GS;
- ob_lsm6ds3tr_c.cur_param.mag_odr = LSM_MAG_ODR_OFF;
- ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_OFF;
- ob_lsm6ds3tr_c.cur_param.timestamp_switch = LSM_TIMESTAMP_OFF;
- ob_lsm6ds3tr_c.cur_param.timestamp_resolution = LSM_TIMESTAMP_6MS4;
- ob_lsm6ds3tr_c.cur_param.acc_power_mode = LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
- ob_lsm6ds3tr_c.cur_param.gry_power_mode = LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;
- ob_lsm6ds3tr_c.op_timeout = 0;
-
-
- SPI0_Disable();
-
- SPI0_Init();
-
-
- return 0;
- }
- int drv_lsm_set_acc_power_mode(LSM_ACC_POWER_MODE_e acc_power_mode)
- {
- lsm6ds3tr_c_xl_hm_mode_t val;
-
- if(ob_lsm6ds3tr_c.cur_param.acc_power_mode != acc_power_mode)
- {
- switch(acc_power_mode)
- {
- case LSM_ACC_POWER_MODE_HIGH_PERFORMANCE:
- lsm6ds3tr_c_xl_power_mode_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_XL_HIGH_PERFORMANCE);
-
- lsm6ds3tr_c_xl_power_mode_get(&ob_lsm6ds3tr_c.ctx,&val);
- if(val != LSM6DS3TR_C_XL_HIGH_PERFORMANCE)return -1;
-
- ob_lsm6ds3tr_c.cur_param.acc_power_mode = LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
- break;
- case LSM_ACC_POWER_MODE_NORMAL:
- lsm6ds3tr_c_xl_power_mode_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_XL_NORMAL);
-
- lsm6ds3tr_c_xl_power_mode_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_XL_NORMAL)return -1;
-
- ob_lsm6ds3tr_c.cur_param.acc_power_mode = LSM_ACC_POWER_MODE_NORMAL;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_set_gry_power_mode(LSM_GRY_POWER_MODE_e gry_power_mode)
- {
- lsm6ds3tr_c_g_hm_mode_t val;
-
- if(ob_lsm6ds3tr_c.cur_param.gry_power_mode != gry_power_mode)
- {
- switch(gry_power_mode)
- {
- case LSM_GRY_POWER_MODE_HIGH_PERFORMANCE:
- lsm6ds3tr_c_gy_power_mode_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_GY_HIGH_PERFORMANCE);
-
- lsm6ds3tr_c_gy_power_mode_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM_GRY_POWER_MODE_HIGH_PERFORMANCE)return -1;
-
- ob_lsm6ds3tr_c.cur_param.gry_power_mode = LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;
- break;
- case LSM_GRY_POWER_MODE_NORMAL:
- lsm6ds3tr_c_gy_power_mode_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_GY_NORMAL);
-
- lsm6ds3tr_c_gy_power_mode_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM_GRY_POWER_MODE_NORMAL)return -1;
-
- ob_lsm6ds3tr_c.cur_param.gry_power_mode = LSM_GRY_POWER_MODE_NORMAL;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_set_timestamp_resolution(LSM_TIMESTAMP_RESOLUTION_e timestamp_resolution)
- {
- lsm6ds3tr_c_timer_hr_t val;
-
- if(ob_lsm6ds3tr_c.cur_param.timestamp_resolution != timestamp_resolution)
- {
- switch(timestamp_resolution)
- {
- case LSM_TIMESTAMP_6MS4:
-
- lsm6ds3tr_c_timestamp_res_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_LSB_6ms4);
-
- lsm6ds3tr_c_timestamp_res_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_LSB_6ms4)return -1;
-
- ob_lsm6ds3tr_c.cur_param.timestamp_resolution = LSM_TIMESTAMP_6MS4;
- break;
- case LSM_TIMESTAMP_25US:
-
- lsm6ds3tr_c_timestamp_res_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_LSB_25us);
-
- lsm6ds3tr_c_timestamp_res_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_LSB_25us)return -1;
-
- ob_lsm6ds3tr_c.cur_param.timestamp_resolution = LSM_TIMESTAMP_25US;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_set_timestamp_switch(LSM_TIMESTAMP_SWITCH_e timestamp_switch)
- {
- uint8_t val;
-
- if(ob_lsm6ds3tr_c.cur_param.timestamp_switch != timestamp_switch)
- {
- switch(timestamp_switch)
- {
- case LSM_TIMESTAMP_ON:
-
- lsm6ds3tr_c_timestamp_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);
-
- lsm6ds3tr_c_timestamp_get(&ob_lsm6ds3tr_c.ctx, &val);
-
- if(val != PROPERTY_ENABLE)return -1;
-
- ob_lsm6ds3tr_c.cur_param.timestamp_switch = LSM_TIMESTAMP_ON;
- break;
- case LSM_TIMESTAMP_OFF:
-
- lsm6ds3tr_c_timestamp_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_DISABLE);
-
- lsm6ds3tr_c_timestamp_get(&ob_lsm6ds3tr_c.ctx, &val);
-
- if(val != PROPERTY_DISABLE)return -1;
-
- ob_lsm6ds3tr_c.cur_param.timestamp_switch = LSM_TIMESTAMP_OFF;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_set_fifo_odr(LSM_FIFO_ODR_e fifo_odr, LSM_ACC_ODR_e acc_odr, LSM_GRY_ODR_e gry_odr, LSM_MAG_ODR_e mag_odr, LSM_TIMESTAMP_SWITCH_e timestamp_switch)
- {
- uint8_t val;
- lsm6ds3tr_c_odr_fifo_t r_fifo_odr;
- lsm6ds3tr_c_fifo_mode_t r_fifo_mode;
- lsm6ds3tr_c_dec_fifo_xl_t r_fifo_xl;
- lsm6ds3tr_c_dec_fifo_gyro_t r_fifo_gy;
- lsm6ds3tr_c_dec_ds3_fifo_t r_fifo_ds3;
- lsm6ds3tr_c_dec_ds4_fifo_t r_fifo_ds4;
- uint16_t fifo_watermark;
- uint16_t pattern_len = 0;
-
- if(ob_lsm6ds3tr_c.cur_param.fifo_odr != fifo_odr)
- {
- switch(fifo_odr)
- {
- case LSM_FIFO_ODR_OFF:
-
- lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_DISABLE);
-
- lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != PROPERTY_DISABLE)return -1;
-
-
- lsm6ds3tr_c_fifo_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_DISABLE);
-
- lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr);
-
- if(r_fifo_odr != LSM6DS3TR_C_FIFO_DISABLE) return -1;
-
- ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_OFF;
- break;
- case LSM_FIFO_ODR_104HZ:
-
- lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_ENABLE);
-
- lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != PROPERTY_ENABLE)return -1;
-
-
- lsm6ds3tr_c_fifo_mode_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_STREAM_MODE);
-
- lsm6ds3tr_c_fifo_mode_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_mode);
-
- if(r_fifo_mode != LSM6DS3TR_C_STREAM_MODE)return -1;
-
-
- if(acc_odr == LSM_ACC_ODR_104HZ)
- {
- lsm6ds3tr_c_fifo_xl_batch_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_XL_NO_DEC);
-
- lsm6ds3tr_c_fifo_xl_batch_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_xl);
-
- if(r_fifo_xl != LSM6DS3TR_C_FIFO_XL_NO_DEC)return -1;
-
- pattern_len += ACC_OUT_XYZ_WORD_NUM;
- }
-
- if(gry_odr == LSM_GRY_ODR_104HZ)
- {
- lsm6ds3tr_c_fifo_gy_batch_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_GY_NO_DEC);
-
- lsm6ds3tr_c_fifo_gy_batch_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_gy);
-
- if(r_fifo_gy != LSM6DS3TR_C_FIFO_GY_NO_DEC)return -1;
-
- pattern_len += GRY_OUT_XYZ_WORD_NUM;
- }
-
- if(mag_odr != LSM_MAG_ODR_OFF)
- {
- lsm6ds3tr_c_fifo_dataset_3_batch_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_FIFO_DS3_NO_DEC);
-
- lsm6ds3tr_c_fifo_dataset_3_batch_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_ds3);
-
- if(r_fifo_ds3 != LSM6DS3TR_C_FIFO_DS3_NO_DEC)return -1;
-
- pattern_len += MAG_OUT_XYZ_WORD_NUM;
- }
-
- if(timestamp_switch == LSM_TIMESTAMP_ON)
- {
- lsm6ds3tr_c_fifo_dataset_4_batch_set(&ob_lsm6ds3tr_c.ctx,LSM6DS3TR_C_FIFO_DS4_NO_DEC);
-
- lsm6ds3tr_c_fifo_dataset_4_batch_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_ds4);
-
- if(r_fifo_ds4 != LSM6DS3TR_C_FIFO_DS4_NO_DEC)return -1;
-
- pattern_len += TIMESTAMP_OUT_WORD_NUM;
- }
-
-
- lsm6ds3tr_c_fifo_watermark_set(&ob_lsm6ds3tr_c.ctx, pattern_len);
-
- lsm6ds3tr_c_fifo_watermark_get(&ob_lsm6ds3tr_c.ctx,&fifo_watermark);
-
- if(fifo_watermark != pattern_len)return -1;
-
-
- lsm6ds3tr_c_fifo_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_FIFO_104Hz);
-
- lsm6ds3tr_c_fifo_data_rate_get(&ob_lsm6ds3tr_c.ctx,&r_fifo_odr);
-
- if(r_fifo_odr != LSM6DS3TR_C_FIFO_104Hz) return -1;
-
- ob_lsm6ds3tr_c.cur_param.fifo_odr = LSM_FIFO_ODR_104HZ;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_set_acc_fs(LSM_ACC_FS_e acc_fs)
- {
- lsm6ds3tr_c_fs_xl_t val;
-
- if(ob_lsm6ds3tr_c.cur_param.acc_fs != acc_fs)
- {
- switch(acc_fs)
- {
- case LSM_ACC_FS_2G:
- lsm6ds3tr_c_xl_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_2g);
-
- lsm6ds3tr_c_xl_full_scale_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_2g)return -1;
-
- ob_lsm6ds3tr_c.cur_param.acc_fs = LSM_ACC_FS_2G;
- break;
- case LSM_ACC_FS_16G:
- lsm6ds3tr_c_xl_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_16g);
-
- lsm6ds3tr_c_xl_full_scale_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_16g)return -1;
-
- ob_lsm6ds3tr_c.cur_param.acc_fs = LSM_ACC_FS_16G;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_set_acc_odr(LSM_ACC_ODR_e acc_odr)
- {
- lsm6ds3tr_c_odr_xl_t val;
-
- if(ob_lsm6ds3tr_c.cur_param.acc_odr != acc_odr)
- {
- switch(acc_odr)
- {
- case LSM_ACC_ODR_OFF:
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
-
- lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_XL_ODR_OFF)return -1;
-
- ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_OFF;
- break;
- case LSM_ACC_ODR_104HZ:
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_104Hz);
-
- lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_XL_ODR_104Hz)return -1;
-
- ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_104HZ;
- break;
- case LSM_ACC_ODR_12HZ5:
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_12Hz5);
-
- lsm6ds3tr_c_xl_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_XL_ODR_12Hz5)return -1;
-
- ob_lsm6ds3tr_c.cur_param.acc_odr = LSM_ACC_ODR_12HZ5;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_set_gry_fs(LSM_GRY_FS_e gry_fs)
- {
- lsm6ds3tr_c_fs_g_t val;
-
- if(ob_lsm6ds3tr_c.cur_param.gry_fs != gry_fs)
- {
- switch(gry_fs)
- {
- case LSM_GRY_FS_250DPS:
- lsm6ds3tr_c_gy_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_250dps);
-
- lsm6ds3tr_c_gy_full_scale_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_250dps)return -1;
-
- ob_lsm6ds3tr_c.cur_param.gry_fs = LSM_GRY_FS_250DPS;
- break;
- case LSM_GRY_FS_2000DPS:
- lsm6ds3tr_c_gy_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_2000dps);
-
- lsm6ds3tr_c_gy_full_scale_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_2000dps)return -1;
-
- ob_lsm6ds3tr_c.cur_param.gry_fs = LSM_GRY_FS_2000DPS;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_set_gry_odr(LSM_GRY_ODR_e gry_odr)
- {
- lsm6ds3tr_c_odr_g_t val;
- if(ob_lsm6ds3tr_c.cur_param.gry_odr != gry_odr)
- {
- switch(gry_odr)
- {
- case LSM_GRY_ODR_OFF:
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_OFF);
-
- lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_GY_ODR_OFF)return -1;
-
- ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_OFF;
- break;
- case LSM_GRY_ODR_104HZ:
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_104Hz);
-
- lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_GY_ODR_104Hz)return -1;
-
- ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_104HZ;
- break;
- case LSM_GRY_ODR_12HZ5:
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_12Hz5);
-
- lsm6ds3tr_c_gy_data_rate_get(&ob_lsm6ds3tr_c.ctx,&val);
-
- if(val != LSM6DS3TR_C_GY_ODR_12Hz5)return -1;
-
- ob_lsm6ds3tr_c.cur_param.gry_odr = LSM_GRY_ODR_12HZ5;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_get_mag_fs_flow(void)
- {
- return 1;
- }
- int drv_lsm_set_mag_fs(LSM_MAG_FS_e mag_fs, int flow)
- {
- int ret;
- uint8_t data;
-
- if(flow <= 0 || flow >= 2)return -1;
-
- if(ob_lsm6ds3tr_c.cur_param.mag_fs != mag_fs)
- {
- switch(mag_fs)
- {
- case LSM_MAG_FS_30GS:
- data = LSM_MAG_FS_30GS;
- ret = write_hub(LSM_IIC_QMC_ADDR,0x0b,&data,1);
- if(ret == -1)return -1;
- data = 0xFF;
- ret = read_hub(LSM_IIC_QMC_ADDR,0x0b,&data,1);
- if(ret == -1 || data != LSM_MAG_FS_30GS)return -1;
- ob_lsm6ds3tr_c.cur_param.mag_fs = LSM_MAG_FS_30GS;
- break;
- }
- }
-
- return 0;
- }
- int drv_lsm_get_mag_odr_flow(void)
- {
- return 5;
- }
- int drv_lsm_set_mag_odr(LSM_MAG_ODR_e mag_odr, bool set_hub, int flow)
- {
- int ret;
- uint8_t data;
-
- if(flow <= 0 || flow >= 6)return -1;
-
- if(ob_lsm6ds3tr_c.cur_param.mag_odr != mag_odr)
- {
- switch(flow)
- {
- case 1:
-
- if(mag_odr != LSM_MAG_ODR_OFF)
- {
- data = 0x06;
- ret = write_hub(LSM_IIC_QMC_ADDR,0x29,&data,1);
- }
- else
- {
- ret = 0;
- }
- break;
-
- case 2:
-
- if(mag_odr != LSM_MAG_ODR_OFF)
- {
- data = 0xFF;
- ret = read_hub(LSM_IIC_QMC_ADDR,0x29,&data,1);
- if(ret == -1 || data != 0x06)ret = -1;
- else ret = 0;
- }
- else
- {
- ret = 0;
- }
- break;
-
- case 3:
-
- data = mag_odr;
- ret = write_hub(LSM_IIC_QMC_ADDR,0x0a,&data,1);
- break;
-
- case 4:
- data = 0xFF;
- ret = read_hub(LSM_IIC_QMC_ADDR,0x0a,&data,1);
- if(ret == -1 || data != mag_odr)ret = -1;
- else{
- ret = 0;
- }
- break;
-
- case 5:
-
- if(set_hub)
- {
-
- lsm6ds3tr_c_sh_slv0_cfg_read(&ob_lsm6ds3tr_c.ctx, &sub_qmc6310);
-
- lsm6ds3tr_c_mem_bank_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_BANK_A);
-
- data = 0xFF;
- lsm6ds3tr_c_read_reg(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_SLV0_SUBADD,&data, 1);
-
- lsm6ds3tr_c_mem_bank_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_USER_BANK);
-
- if(data != QMC6310_DATA_OUT_X_LSB_REG)return -1;
-
-
- lsm6ds3tr_c_sh_num_of_dev_connected_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_SLV_0);
- lsm6ds3tr_c_func_en_set(&ob_lsm6ds3tr_c.ctx,PROPERTY_ENABLE);
-
- lsm6ds3tr_c_sh_master_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);
-
- ret = 0;
- ob_lsm6ds3tr_c.cur_param.mag_odr = mag_odr;
- }
- else{
- ret = 0;
- ob_lsm6ds3tr_c.cur_param.mag_odr = mag_odr;
- }
- break;
- }
- }
- else
- {
- return 0;
- }
-
- return ret;
- }
- int drv_lsm_get_config_param(drv_lsm_config_param_t *p_param)
- {
- *p_param = ob_lsm6ds3tr_c.cur_param;
-
- return 0;
- }
- int drv_lsm_get_fifo_group_num(void)
- {
- uint8_t wt;
- uint16_t num = 0;
- uint16_t num_pattern = 0;
- uint16_t pattern_len = 0;
-
-
- lsm6ds3tr_c_fifo_wtm_flag_get(&ob_lsm6ds3tr_c.ctx, &wt);
-
- if(wt)
- {
-
- lsm6ds3tr_c_fifo_data_level_get(&ob_lsm6ds3tr_c.ctx, &num);
-
- if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF){
- pattern_len += ACC_OUT_XYZ_WORD_NUM;
- }
- if(ob_lsm6ds3tr_c.cur_param.gry_odr != LSM_GRY_ODR_OFF){
- pattern_len += GRY_OUT_XYZ_WORD_NUM;
- }
- if(ob_lsm6ds3tr_c.cur_param.mag_odr != LSM_MAG_ODR_OFF){
- pattern_len += MAG_OUT_XYZ_WORD_NUM;
- }
- if(ob_lsm6ds3tr_c.cur_param.timestamp_switch == LSM_TIMESTAMP_ON){
- pattern_len += TIMESTAMP_OUT_WORD_NUM;
- }
- num_pattern = num / pattern_len;
- }
-
-
- return num_pattern;
- }
- int drv_lsm_get_fifo_data(lsm_data_t *p_data)
- {
- int ret;
- int read_len = 0;
- int read_num = 0;
-
-
- if(ob_lsm6ds3tr_c.cur_param.gry_odr != LSM_GRY_ODR_OFF){
-
- read_len += GRY_OUT_XYZ_WORD_NUM * sizeof(int16_t);
- }
- if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF){
-
- read_len += ACC_OUT_XYZ_WORD_NUM * sizeof(int16_t);
- }
- if(ob_lsm6ds3tr_c.cur_param.mag_odr != LSM_MAG_ODR_OFF){
-
- read_len += MAG_OUT_XYZ_WORD_NUM * sizeof(int16_t);
- }
- if(ob_lsm6ds3tr_c.cur_param.timestamp_switch == LSM_TIMESTAMP_ON){
-
- read_len += TIMESTAMP_OUT_WORD_NUM * sizeof(int16_t);
- }
-
-
- memset(temp_data_raw,0,sizeof(temp_data_raw));
-
-
- ret = lsm6ds3tr_c_fifo_raw_data_get(&ob_lsm6ds3tr_c.ctx, temp_data_raw[0].u8bit,read_len);
- if(ret == -1)return -1;
-
-
- if(p_data != NULL)
- {
- if(ob_lsm6ds3tr_c.cur_param.gry_odr != LSM_GRY_ODR_OFF){
-
- p_data->gry[0] = temp_data_raw[read_num].i16bit[0];
- p_data->gry[1] = temp_data_raw[read_num].i16bit[1];
- p_data->gry[2] = temp_data_raw[read_num].i16bit[2];
-
- read_num++;
- }
- if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF){
-
- p_data->acc[0] = temp_data_raw[read_num].i16bit[0];
- p_data->acc[1] = temp_data_raw[read_num].i16bit[1];
- p_data->acc[2] = temp_data_raw[read_num].i16bit[2];
-
- read_num++;
- }
- if(ob_lsm6ds3tr_c.cur_param.mag_odr != LSM_MAG_ODR_OFF){
-
- p_data->mag[0] = temp_data_raw[read_num].i16bit[0];
- p_data->mag[1] = temp_data_raw[read_num].i16bit[1];
- p_data->mag[2] = temp_data_raw[read_num].i16bit[2];
-
- read_num++;
- }
- if(ob_lsm6ds3tr_c.cur_param.timestamp_switch == LSM_TIMESTAMP_ON){
-
-
-
- p_data->fifo_timestamp = ((temp_data_raw[read_num].u8bit[1] << 16) | (temp_data_raw[read_num].u8bit[0] << 8) | (temp_data_raw[read_num].u8bit[3]))*25;
- }
- }
- return 0;
- }
- int drv_lsm_get_acc_data(lsm_data_t *p_data)
- {
- lsm6ds3tr_c_reg_t reg;
-
- if(ob_lsm6ds3tr_c.cur_param.acc_odr != LSM_ACC_ODR_OFF)
- {
-
-
- lsm6ds3tr_c_status_reg_get(&ob_lsm6ds3tr_c.ctx, ®.status_reg);
-
- if(reg.status_reg.xlda)
- {
-
- memset(temp_data_raw[0].i16bit, 0x00, 3 * sizeof(int16_t));
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx,temp_data_raw[0].i16bit);
-
- p_data->acc[0] = temp_data_raw[0].i16bit[0];
- p_data->acc[1] = temp_data_raw[0].i16bit[1];
- p_data->acc[2] = temp_data_raw[0].i16bit[2];
- }
- else return -1;
- }
- else
- {
- return -1;
- }
-
- return 0;
- }
- int drv_lsm_self_check_1(void)
- {
-
- lsm6ds3tr_c_device_id_get(&ob_lsm6ds3tr_c.ctx, &whoamI);
- if(whoamI != LSM6DS3TR_C_ID)
- {
- DEBUG_LOG("LSM self_check error!!!\r\n");
- return -1;
- }
-
- lsm6ds3tr_c_reset_set(&ob_lsm6ds3tr_c.ctx, PROPERTY_ENABLE);
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_reset_get(&ob_lsm6ds3tr_c.ctx, &rst);
- ob_lsm6ds3tr_c.op_timeout--;
- } while(rst && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
-
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_833Hz);
-
- lsm6ds3tr_c_xl_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_4g);
-
- return 0;
- }
- int drv_lsm_self_check_2(void)
- {
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while(!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
-
- memset(val_st_off, 0x00, 3 * sizeof(float));
- for (i = 0; i < 5; i++) {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
- for (j = 0; j < 3; j++) {
- val_st_off[j] += lsm6ds3tr_c_from_fs4g_to_mg(self_check_data_raw[j]);
- }
- }
-
- for (i = 0; i < 3; i++) {
- val_st_off[i] /= 5.0f;
- }
-
- lsm6ds3tr_c_xl_self_test_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ST_NEGATIVE);
-
- return 0;
- }
- int drv_lsm_self_check_3(void)
- {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
-
- memset(val_st_on, 0x00, 3 * sizeof(float));
- for (i = 0; i < 5; i++) {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_xl_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_acceleration_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
- for (j = 0; j < 3; j++) {
- val_st_on[j] += lsm6ds3tr_c_from_fs4g_to_mg(self_check_data_raw[j]);
- }
- }
-
- for (i = 0; i < 3; i++) {
- val_st_on[i] /= 5.0f;
- }
-
- for (i = 0; i < 3; i++) {
- test_val[i] = fabs((val_st_on[i] - val_st_off[i]));
- }
-
- st_result = ST_PASS;
-
- for (i = 0; i < 3; i++) {
- if (( MIN_ST_LIMIT_mg > test_val[i] ) ||
- ( test_val[i] > MAX_ST_LIMIT_mg)) {
- st_result = ST_FAIL;
- }
- }
-
- lsm6ds3tr_c_xl_self_test_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ST_DISABLE);
-
- lsm6ds3tr_c_xl_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_XL_ODR_OFF);
-
-
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_833Hz);
-
- lsm6ds3tr_c_gy_full_scale_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_2000dps);
-
- return 0;
- }
- int drv_lsm_self_check_4(void)
- {
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
-
- memset(val_st_off, 0x00, 3 * sizeof(float));
-
- for (i = 0; i < 5; i++) {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
- for (j = 0; j < 3; j++) {
- val_st_off[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
- self_check_data_raw[j]);
- }
- }
-
-
- for (i = 0; i < 3; i++) {
- val_st_off[i] /= 5.0f;
- }
-
- lsm6ds3tr_c_gy_self_test_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ST_POSITIVE);
-
- return 0;
- }
- int drv_lsm_self_check_5(void)
- {
- memset(val_st_on, 0x00, 3 * sizeof(float));
- for (i = 0; i < 5; i++) {
-
- ob_lsm6ds3tr_c.op_timeout = OP_TIMEOUT * 10;
- do {
- lsm6ds3tr_c_gy_flag_data_ready_get(&ob_lsm6ds3tr_c.ctx, &drdy);
- ob_lsm6ds3tr_c.op_timeout--;
- } while (!drdy && ob_lsm6ds3tr_c.op_timeout!=0);
- if(ob_lsm6ds3tr_c.op_timeout == 0)return -1;
-
- lsm6ds3tr_c_angular_rate_raw_get(&ob_lsm6ds3tr_c.ctx, self_check_data_raw);
- for (j = 0; j < 3; j++) {
- val_st_on[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps(
- self_check_data_raw[j]);
- }
- }
-
- for (i = 0; i < 3; i++) {
- val_st_on[i] /= 5.0f;
- }
-
- for (i = 0; i < 3; i++) {
- test_val[i] = fabs((val_st_on[i] - val_st_off[i]));
- }
-
- for (i = 0; i < 3; i++) {
- if (( MIN_ST_LIMIT_mdps > test_val[i] ) ||
- ( test_val[i] > MAX_ST_LIMIT_mdps)) {
- st_result = ST_FAIL;
- }
- }
-
- lsm6ds3tr_c_gy_self_test_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ST_DISABLE);
-
- lsm6ds3tr_c_gy_data_rate_set(&ob_lsm6ds3tr_c.ctx, LSM6DS3TR_C_GY_ODR_OFF);
- if (st_result == ST_PASS) {
- DEBUG_LOG("LSM Self Test - PASS\r\n");
-
- lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
-
- rst = 0Xff;
- lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&rst);
-
- if(rst != 1)return -1;
-
- }else{
- DEBUG_LOG("Self Test - FAIL\r\n");
-
- lsm6ds3tr_c_sh_pass_through_set(&ob_lsm6ds3tr_c.ctx,1);
-
- rst = 0Xff;
- lsm6ds3tr_c_sh_pass_through_get(&ob_lsm6ds3tr_c.ctx,&rst);
-
- if(rst != 1)return -1;
-
- return -1;
- }
-
- return 0;
- }
|