1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213 |
- #include "tool.h"
- #include "nrf_delay.h"
- #include "bsp_time.h"
- #include "exception.h"
- #include "system.h"
- #include "drv_qmc6310_v2.h"
- #include "drv_lsm6ds3tr_c.h"
- #include "fml_imu.h"
- #define FML_IMU_DATA_GROUP_NUM_MAX 20
- #define FML_IMU_MONITOR_DATA_ERR_SUM_MAX 200
- typedef enum {
- FML_IMU_CONFIG_STAGE_DONE,
-
- FML_IMU_CONFIG_STAGE_IN_PROGRESS,
-
- FML_IMU_CONFIG_STAGE_FAIL,
-
- } FML_IMU_CONFIG_STAGE_e;
- typedef union
- {
- drv_lsm_config_param_t lsm;
-
- drv_qmc_config_param_t qmc;
-
- } drv_param_u;
- typedef struct fml_imu
- {
-
- FML_IMU_CONFIG_STAGE_e stage;
-
- drv_param_u drv_param[FML_IMU_DIR_NUM];
-
- fml_imu_param_t config_param[FML_IMU_DIR_NUM];
-
- int cur_data_num[FML_IMU_DIR_NUM];
-
- fml_imu_data_t cur_data[FML_IMU_DIR_NUM][FML_IMU_DATA_GROUP_NUM_MAX];
-
- uint32_t config_flow;
-
- fml_imu_config_cb config_cb;
-
- fml_imu_data_notify_cb data_notify_cb;
-
- int16_t last_f_acc[3];
-
- int16_t last_f_gry[3];
-
- int16_t last_f_mag[3];
-
- int16_t last_b_mag[3];
-
- int16_t last_f_acc_err_sum;
-
- int16_t last_f_gry_err_sum;
-
- int16_t last_f_mag_err_sum;
-
- int16_t last_b_mag_err_sum;
-
- int16_t except_data_occur_sum[FML_IMU_DIR_NUM];
-
- } Fml_Imu_t;
- static Fml_Imu_t ob_fml_imu;
- static void fml_imu_macro_conversion(FML_IMU_DIR_e dir, fml_imu_param_t *config_param, drv_param_u *drv_param)
- {
- switch(dir)
- {
- case FML_IMU_DIR_FRONT:
-
- switch(config_param->acc_odr)
- {
- case FML_IMU_ACC_ODR_OFF:
- drv_param->lsm.acc_odr = LSM_ACC_ODR_OFF;
- break;
- case FML_IMU_ACC_ODR_104HZ:
- drv_param->lsm.acc_odr = LSM_ACC_ODR_104HZ;
- break;
- case FML_IMU_ACC_ODR_12HZ5:
- drv_param->lsm.acc_odr = LSM_ACC_ODR_12HZ5;
- break;
- }
-
- switch(config_param->gry_odr)
- {
- case FML_IMU_GRY_ODR_OFF:
- drv_param->lsm.gry_odr = LSM_GRY_ODR_OFF;
- break;
- case FML_IMU_GRY_ODR_104HZ:
- drv_param->lsm.gry_odr = LSM_GRY_ODR_104HZ;
- break;
- case FML_IMU_GRY_ODR_12HZ5:
- drv_param->lsm.gry_odr = LSM_GRY_ODR_12HZ5;
- break;
- }
-
- switch(config_param->mag_odr)
- {
- case FML_IMU_MAG_ODR_OFF:
- drv_param->lsm.mag_odr = LSM_MAG_ODR_OFF;
- break;
- case FML_IMU_MAG_ODR_10HZ:
- drv_param->lsm.mag_odr = LSM_MAG_ODR_10HZ;
- break;
- case FML_IMU_MAG_ODR_100HZ:
- drv_param->lsm.mag_odr = LSM_MAG_ODR_100HZ;
- break;
- case FML_IMU_MAG_ODR_200HZ:
- drv_param->lsm.mag_odr = LSM_MAG_ODR_200HZ;
- break;
- }
-
- switch(config_param->fifo_odr)
- {
- case FML_IMU_FIFO_ODR_OFF:
- drv_param->lsm.fifo_odr = LSM_FIFO_ODR_OFF;
- break;
- case FML_IMU_FIFO_ODR_104HZ:
- drv_param->lsm.fifo_odr = LSM_FIFO_ODR_104HZ;
- break;
- }
-
- switch(config_param->acc_power_mode)
- {
- case FML_IMU_ACC_POWER_MODE_HIGH_PERFORMANCE:
- drv_param->lsm.acc_power_mode = LSM_ACC_POWER_MODE_HIGH_PERFORMANCE;
- break;
- case FML_IMU_ACC_POWER_MODE_NORMAL:
- drv_param->lsm.acc_power_mode = LSM_ACC_POWER_MODE_NORMAL;
- break;
- }
-
- switch(config_param->gry_power_mode)
- {
- case FML_IMU_GRY_POWER_MODE_HIGH_PERFORMANCE:
- drv_param->lsm.gry_power_mode = LSM_GRY_POWER_MODE_HIGH_PERFORMANCE;
- break;
- case FML_IMU_GRY_POWER_MODE_NORMAL:
- drv_param->lsm.gry_power_mode = LSM_GRY_POWER_MODE_NORMAL;
- break;
- }
-
- switch(config_param->acc_fs)
- {
- case FML_IMU_ACC_FS_2G:
- drv_param->lsm.acc_fs = LSM_ACC_FS_2G;
- break;
- case FML_IMU_ACC_FS_16G:
- drv_param->lsm.acc_fs = LSM_ACC_FS_16G;
- break;
- }
-
- switch(config_param->gry_fs)
- {
- case FML_IMU_GRY_FS_250DPS:
- drv_param->lsm.gry_fs = LSM_GRY_FS_250DPS;
- break;
- case FML_IMU_GRY_FS_2000DPS:
- drv_param->lsm.gry_fs = LSM_GRY_FS_2000DPS;
- break;
- }
-
- switch(config_param->mag_fs)
- {
- case FML_IMU_MAG_FS_30GS:
- drv_param->lsm.mag_fs = LSM_MAG_FS_30GS;
- break;
- }
-
- switch(config_param->timestamp_resolution)
- {
- case FML_IMU_TIMESTAMP_6MS4:
- drv_param->lsm.timestamp_resolution = LSM_TIMESTAMP_6MS4;
- break;
- case FML_IMU_TIMESTAMP_25US:
- drv_param->lsm.timestamp_resolution = LSM_TIMESTAMP_25US;
- break;
- }
-
- switch(config_param->timestamp_switch)
- {
- case FML_IMU_TIMESTAMP_OFF:
- drv_param->lsm.timestamp_switch = LSM_TIMESTAMP_OFF;
- break;
- case FML_IMU_TIMESTAMP_ON:
- drv_param->lsm.timestamp_switch = LSM_TIMESTAMP_ON;
- break;
- }
- break;
-
- case FML_IMU_DIR_BACK:
-
- switch(config_param->mag_odr)
- {
- case FML_IMU_MAG_ODR_OFF:
- drv_param->qmc.mag_odr = QMC_MAG_ODR_OFF;
- break;
- case FML_IMU_MAG_ODR_10HZ:
- drv_param->qmc.mag_odr = QMC_MAG_ODR_10HZ;
- break;
- case FML_IMU_MAG_ODR_100HZ:
- drv_param->qmc.mag_odr = QMC_MAG_ODR_100HZ;
- break;
- case FML_IMU_MAG_ODR_200HZ:
- drv_param->qmc.mag_odr = QMC_MAG_ODR_200HZ;
- break;
- }
-
- switch(config_param->mag_fs)
- {
- case FML_IMU_MAG_FS_30GS:
- drv_param->qmc.mag_fs = QMC_MAG_FS_30GS;
- break;
- }
- break;
-
- default:
- break;
- }
- }
- static int fml_imu_intergrated_setting(void)
- {
- int ret;
- int lsm_mag_flow = drv_lsm_get_mag_odr_flow();
- int qmc_mag_flow = drv_qmc6310_get_mag_odr_flow();
- drv_param_u lsm_drv_param;
- drv_param_u qmc_drv_param;
- static uint32_t tim = 0;
-
- switch(ob_fml_imu.config_flow)
- {
- case 0:
-
- ret = drv_lsm_power_off();
- if(ret != 0)return -1;
-
- ret = drv_qmc6310_power_off();
- if(ret != 0)return -1;
-
-
- ob_fml_imu.cur_data_num[FML_IMU_DIR_FRONT] = 0;
-
- ob_fml_imu.cur_data_num[FML_IMU_DIR_BACK] = 0;
-
- tim = 0;
- ob_fml_imu.config_flow = 1;
- break;
-
- case 1:
-
- if(tim == 0){
- tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-tim>=200){
- tim = 0;
- ob_fml_imu.config_flow = 2;
- }
- break;
- case 2:
-
- ret = drv_lsm_power_on();
- if(ret != 0)return -1;
-
- ret = drv_qmc6310_power_on();
- if(ret != 0)return -1;
-
- tim = 0;
- ob_fml_imu.config_flow = 3;
- break;
-
- case 3:
-
- if(tim == 0){
- tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-tim>=200){
- tim = 0;
- ob_fml_imu.config_flow = 4;
- }
- break;
-
- case 4:
-
- ret = drv_lsm_self_check_1();
- if(ret != 0)return -1;
- tim = 0;
- ob_fml_imu.config_flow = 5;
- break;
-
- case 5:
-
- if(tim == 0){
- tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-tim>=100){
- tim = 0;
- ob_fml_imu.config_flow = 6;
- }
- break;
-
- case 6:
-
- ret = drv_lsm_self_check_2();
- if(ret != 0)return -1;
- tim = 0;
- ob_fml_imu.config_flow = 7;
- break;
-
- case 7:
-
- if(tim == 0){
- tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-tim>=100){
- tim = 0;
- ob_fml_imu.config_flow = 8;
- }
- break;
-
- case 8:
-
- ret = drv_lsm_self_check_3();
- if(ret != 0)return -1;
- tim = 0;
- ob_fml_imu.config_flow = 9;
- break;
-
- case 9:
-
- if(tim == 0){
- tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-tim>=150){
- tim = 0;
- ob_fml_imu.config_flow = 10;
- }
- break;
-
- case 10:
-
- ret = drv_lsm_self_check_4();
- if(ret != 0)return -1;
- tim = 0;
- ob_fml_imu.config_flow = 11;
- break;
-
- case 11:
-
- if(tim == 0){
- tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-tim>=50){
- tim = 0;
- ob_fml_imu.config_flow = 12;
- }
- break;
-
- case 12:
-
- ret = drv_lsm_self_check_5();
- if(ret != 0)return -1;
- tim = 0;
- ob_fml_imu.config_flow = 13;
- break;
-
- case 13:
-
- fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &lsm_drv_param);
- fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param);
-
- if(ob_fml_imu.config_param[FML_IMU_DIR_FRONT].fifo_odr != FML_IMU_FIFO_ODR_OFF)
- {
- for(int i=1; i <=lsm_mag_flow;i++)
- {
- ret = drv_lsm_set_mag_odr(lsm_drv_param.lsm.mag_odr, 1, i);
- if(ret != 0)return -1;
- }
- tim = 0;
- ob_fml_imu.config_flow = 14;
- }
- else
- {
- for(int i=1; i <= lsm_mag_flow; i++)
- {
- ret = drv_lsm_set_mag_odr(lsm_drv_param.lsm.mag_odr, 0, i);
- if(ret != 0)return -1;
- }
- tim = 0;
- ob_fml_imu.config_flow = 14;
- }
- break;
-
- case 14:
-
- fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &lsm_drv_param);
- fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param);
-
- ret = drv_lsm_set_gry_odr(lsm_drv_param.lsm.gry_odr);
- if(ret != 0)return -1;
-
- ret = drv_lsm_set_acc_odr(lsm_drv_param.lsm.acc_odr);
- if(ret != 0)return -1;
- tim = 0;
- ob_fml_imu.config_flow = 15;
- break;
-
- case 15:
-
- if(tim == 0){
- tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-tim>=20){
- tim = 0;
- ob_fml_imu.config_flow = 16;
- }
- break;
-
- case FML_IMU_CONFIG_FLOW_DONE:
-
- break;
-
- default:
-
- fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &lsm_drv_param);
- fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param);
-
- ret = drv_lsm_set_acc_power_mode(lsm_drv_param.lsm.acc_power_mode);
- if(ret != 0)return -1;
- ob_fml_imu.config_flow++;
-
- ret = drv_lsm_set_gry_power_mode(lsm_drv_param.lsm.gry_power_mode);
- if(ret != 0)return -1;
- ob_fml_imu.config_flow++;
-
- ret = drv_lsm_set_acc_fs(lsm_drv_param.lsm.acc_fs);
- if(ret != 0)return -1;
- ob_fml_imu.config_flow++;
-
- ret = drv_lsm_set_gry_fs(lsm_drv_param.lsm.gry_fs);
- if(ret != 0)return -1;
- ob_fml_imu.config_flow++;
-
- ret = drv_lsm_set_timestamp_resolution(lsm_drv_param.lsm.timestamp_resolution);
- if(ret != 0)return -1;
- ob_fml_imu.config_flow++;
-
- ret = drv_lsm_set_timestamp_switch(lsm_drv_param.lsm.timestamp_switch);
- if(ret != 0)return -1;
- ob_fml_imu.config_flow++;
-
- ret = drv_lsm_set_fifo_odr(lsm_drv_param.lsm.fifo_odr, \
- lsm_drv_param.lsm.acc_odr, \
- lsm_drv_param.lsm.gry_odr, \
- lsm_drv_param.lsm.mag_odr, \
- lsm_drv_param.lsm.timestamp_switch);
- if(ret != 0)return -1;
- ob_fml_imu.config_flow++;
-
- for(int i = 1; i <=qmc_mag_flow; i++)
- {
- ret = drv_qmc6310_set_mag_odr(qmc_drv_param.qmc.mag_odr,i);
- if(ret != 0)return -1;
- ob_fml_imu.config_flow++;
- }
-
- ob_fml_imu.config_flow = FML_IMU_CONFIG_FLOW_DONE;
- break;
- }
- return 0;
- }
- static int fml_imu_read_data_lsm(void)
- {
- int i;
- int fifo_group_num;
- lsm_data_t temp_lsm_data;
-
- if(ob_fml_imu.config_param[FML_IMU_DIR_FRONT].fifo_odr != FML_IMU_FIFO_ODR_OFF)
- {
-
- fifo_group_num = drv_lsm_get_fifo_group_num();
- fifo_group_num = fifo_group_num <= FML_IMU_DATA_GROUP_NUM_MAX ? fifo_group_num : FML_IMU_DATA_GROUP_NUM_MAX;
-
- if(fifo_group_num > 0)
- {
- for(i=0; i<fifo_group_num; i++)
- {
- if(drv_lsm_get_fifo_data(&temp_lsm_data) != -1)
- {
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].acc[0] = temp_lsm_data.acc[0];
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].acc[1] = temp_lsm_data.acc[1];
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].acc[2] = temp_lsm_data.acc[2];
-
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].gry[0] = temp_lsm_data.gry[0];
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].gry[1] = temp_lsm_data.gry[1];
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].gry[2] = temp_lsm_data.gry[2];
-
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].mag[0] = temp_lsm_data.mag[0];
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].mag[1] = temp_lsm_data.mag[1];
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].mag[2] = temp_lsm_data.mag[2];
-
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][i].fifo_timestamp = temp_lsm_data.fifo_timestamp;
- }
- else
- {
- break;
- }
- }
- }
- else
- {
- return 0;
- }
-
- return i;
- }
-
- else if(ob_fml_imu.config_param[FML_IMU_DIR_FRONT].acc_odr != FML_IMU_ACC_ODR_OFF && \
- ob_fml_imu.config_param[FML_IMU_DIR_FRONT].gry_odr == FML_IMU_GRY_ODR_OFF && \
- ob_fml_imu.config_param[FML_IMU_DIR_FRONT].mag_odr == FML_IMU_MAG_ODR_OFF \
- )
- {
- if(drv_lsm_get_acc_data(&temp_lsm_data) != -1)
- {
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][0].acc[0] = temp_lsm_data.acc[0];
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][0].acc[1] = temp_lsm_data.acc[1];
- ob_fml_imu.cur_data[FML_IMU_DIR_FRONT][0].acc[2] = temp_lsm_data.acc[2];
-
- return 1;
- }
- }
-
- return 0;
- }
- static int fml_imu_read_data_qmc(void)
- {
- qmc_data_t temp_qmc_data;
-
- if(ob_fml_imu.config_param[FML_IMU_DIR_BACK].mag_odr != FML_IMU_MAG_ODR_OFF)
- {
- if(drv_qmc6310_get_mag_data(&temp_qmc_data) != -1)
- {
- ob_fml_imu.cur_data[FML_IMU_DIR_BACK][0].mag[0] = temp_qmc_data.mag[0];
- ob_fml_imu.cur_data[FML_IMU_DIR_BACK][0].mag[1] = temp_qmc_data.mag[1];
- ob_fml_imu.cur_data[FML_IMU_DIR_BACK][0].mag[2] = temp_qmc_data.mag[2];
-
- return 1;
- }
- }
- return 0;
- }
- static int hal_ser_imu_read_data(FML_IMU_DIR_e dir)
- {
- int ret = 0;
-
- switch(dir)
- {
- case FML_IMU_DIR_FRONT:
- ret = fml_imu_read_data_lsm();
- break;
-
- case FML_IMU_DIR_BACK:
- ret = fml_imu_read_data_qmc();
- break;
-
- default:
- break;
- }
-
- return ret;
- }
- static void monitor_sensor_data(int16_t *f_acc, int16_t *f_gry, int16_t *f_mag, int16_t *b_mag)
- {
-
- if(f_acc != NULL)
- {
- if(
- ob_fml_imu.last_f_acc[0] == f_acc[0] && \
- ob_fml_imu.last_f_acc[1] == f_acc[1] && \
- ob_fml_imu.last_f_acc[2] == f_acc[2]
- )
- {
- ob_fml_imu.last_f_acc_err_sum++;
-
- if(ob_fml_imu.last_f_acc_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){
- Except_SetExceptype(EXCEPT_DATA_FRONT_ACC);
- }
-
- }else{
- ob_fml_imu.last_f_acc_err_sum = 0;
- }
-
- ob_fml_imu.last_f_acc[0] = f_acc[0];
- ob_fml_imu.last_f_acc[1] = f_acc[1];
- ob_fml_imu.last_f_acc[2] = f_acc[2];
- }
-
- if(f_gry != NULL)
- {
- if(
- ob_fml_imu.last_f_gry[0] == f_gry[0] && \
- ob_fml_imu.last_f_gry[1] == f_gry[1] && \
- ob_fml_imu.last_f_gry[2] == f_gry[2]
- )
- {
- ob_fml_imu.last_f_gry_err_sum++;
-
- if(ob_fml_imu.last_f_gry_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){
- Except_SetExceptype(EXCEPT_DATA_FRONT_GRY);
- }
-
- }else{
- ob_fml_imu.last_f_gry_err_sum = 0;
- }
-
- ob_fml_imu.last_f_gry[0] = f_gry[0];
- ob_fml_imu.last_f_gry[1] = f_gry[1];
- ob_fml_imu.last_f_gry[2] = f_gry[2];
- }
-
- if(f_mag != NULL)
- {
- if(
- ob_fml_imu.last_f_mag[0] == f_mag[0] && \
- ob_fml_imu.last_f_mag[1] == f_mag[1] && \
- ob_fml_imu.last_f_mag[2] == f_mag[2]
- )
- {
- ob_fml_imu.last_f_mag_err_sum++;
-
- if(ob_fml_imu.last_f_mag_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){
- Except_SetExceptype(EXCEPT_DATA_FRONT_MAG);
- }
-
- }else{
- ob_fml_imu.last_f_mag_err_sum = 0;
- }
-
- ob_fml_imu.last_f_mag[0] = f_mag[0];
- ob_fml_imu.last_f_mag[1] = f_mag[1];
- ob_fml_imu.last_f_mag[2] = f_mag[2];
- }
-
- if(b_mag != NULL)
- {
- if(
- ob_fml_imu.last_b_mag[0] == b_mag[0] && \
- ob_fml_imu.last_b_mag[1] == b_mag[1] && \
- ob_fml_imu.last_b_mag[2] == b_mag[2]
- )
- {
- ob_fml_imu.last_b_mag_err_sum++;
-
- if(ob_fml_imu.last_b_mag_err_sum >= FML_IMU_MONITOR_DATA_ERR_SUM_MAX){
- Except_SetExceptype(EXCEPT_DATA_BACK_MAG);
- }
-
- }else{
- ob_fml_imu.last_b_mag_err_sum = 0;
- }
-
- ob_fml_imu.last_b_mag[0] = b_mag[0];
- ob_fml_imu.last_b_mag[1] = b_mag[1];
- ob_fml_imu.last_b_mag[2] = b_mag[2];
- }
-
- if(Except_IsError(EXCEPT_DATA_FRONT_ACC) || Except_IsError(EXCEPT_DATA_FRONT_GRY) || Except_IsError(EXCEPT_DATA_FRONT_MAG))
- {
- ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_FRONT]++;
-
- if(ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_FRONT] > 1)
- {
- if(Except_IsError(EXCEPT_DATA_FRONT_ACC)){Except_TxError(EXCEPT_DATA_FRONT_ACC,"front_acc_data_error");DEBUG_LOG("front_acc_data_error\n");}
- if(Except_IsError(EXCEPT_DATA_FRONT_GRY)){Except_TxError(EXCEPT_DATA_FRONT_GRY,"front_gry_data_error");DEBUG_LOG("front_gry_data_error\n");}
- if(Except_IsError(EXCEPT_DATA_FRONT_MAG)){Except_TxError(EXCEPT_DATA_FRONT_MAG,"front_mag_data_error");DEBUG_LOG("front_mag_data_error\n");}
- }
-
- drv_lsm_power_off();
- drv_lsm_power_on();
-
- fml_imu_start_config();
-
- Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC);
- Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY);
- Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG);
- }
- else
- {
- ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_FRONT] = 0;
-
- Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC);
- Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY);
- Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG);
- }
-
-
- if(Except_IsError(EXCEPT_DATA_BACK_MAG))
- {
- ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_BACK]++;
-
- if(ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_BACK] > 1)
- {
- if(Except_IsError(EXCEPT_DATA_BACK_MAG))Except_TxError(EXCEPT_DATA_BACK_MAG,"back_mag_data_error");
- }
-
- drv_qmc6310_power_off();
- drv_qmc6310_power_on();
-
- fml_imu_start_config();
-
- Except_ClearExceptype(EXCEPT_DATA_BACK_MAG);
- }
- else
- {
- ob_fml_imu.except_data_occur_sum[FML_IMU_DIR_BACK] = 0;
-
- Except_ClearExceptype(EXCEPT_DATA_BACK_MAG);
- }
- }
- static void monitor_sensor_no_data(int cur_front_data_num, int cur_back_data_num)
- {
- static uint32_t front_no_data_tim = 0;
- static uint32_t back_no_data_tim = 0;
- static char buf[255];
-
-
- if(cur_front_data_num == 0)
- {
- if(ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr != LSM_ACC_ODR_OFF || \
- ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.gry_odr != LSM_GRY_ODR_OFF || \
- ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr != LSM_MAG_ODR_OFF)
- {
-
- if(front_no_data_tim == 0){
- front_no_data_tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-front_no_data_tim>=1000){
- sprintf(buf,"Front Sensor short circuit\r\n");
- Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf);
- front_no_data_tim = 0;
-
- drv_lsm_power_off();
- drv_lsm_power_on();
-
- fml_imu_start_config();
- }
- }
- }
- else front_no_data_tim = 0;
-
-
-
- if(cur_back_data_num == 0)
- {
- if(ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc.mag_odr != QMC_MAG_ODR_OFF)
- {
-
- if(back_no_data_tim == 0){
- back_no_data_tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-back_no_data_tim>=1000){
- sprintf(buf,"Back Sensor short circuit\r\n");
- Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf);
- back_no_data_tim = 0;
-
- drv_qmc6310_power_off();
- drv_qmc6310_power_on();
-
- fml_imu_start_config();
- }
- }
- }
- else back_no_data_tim = 0;
- }
- static void fml_imu_monitor_sensor_data_process(int cur_front_data_num, int cur_back_data_num)
- {
- int i = 0;
- int16_t group_num = 0;
- static int16_t f_acc[3];
- static int16_t f_gry[3];
- static int16_t f_mag[3];
- static int16_t b_mag[3];
- fml_imu_data_t data;
- static uint32_t last_tim = 0;
-
-
- monitor_sensor_no_data(cur_front_data_num, cur_back_data_num);
-
-
-
- if(
- ob_fml_imu.stage == FML_IMU_CONFIG_STAGE_DONE && \
- ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \
- ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.gry_odr == LSM_GRY_ODR_104HZ && \
- (ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_100HZ)
- )
- {
- if(TIME_GetTicks()-last_tim >= FullPower_Interval)
- {
- last_tim = TIME_GetTicks();
-
- group_num = fml_imu_get_data_num(FML_IMU_DIR_FRONT);
-
- if(group_num > 0)
- {
- for(i=0;i<group_num;i++)
- {
- fml_imu_get_data(FML_IMU_DIR_FRONT, i, &data);
- f_gry[0] = data.gry[0];f_gry[1] = data.gry[1];f_gry[2] = data.gry[2];
- f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
- f_mag[0] = data.mag[0];f_mag[1] = data.mag[1];f_mag[2] = data.mag[2];
-
- monitor_sensor_data(f_acc, f_gry, f_mag, NULL);
- }
- }
-
- if(fml_imu_get_data_num(FML_IMU_DIR_BACK) >= 1){
- fml_imu_get_data(FML_IMU_DIR_BACK, 0, &data);
- b_mag[0] = data.mag[0];b_mag[1] = data.mag[1];b_mag[2] = data.mag[2];
- monitor_sensor_data(NULL, NULL, NULL, b_mag);
- }
- }
- }
- else if(
- ob_fml_imu.stage == FML_IMU_CONFIG_STAGE_DONE && \
- ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_104HZ && \
- (ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_200HZ || ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.mag_odr == LSM_MAG_ODR_100HZ)
- )
- {
- if(TIME_GetTicks()-last_tim >= LowPower_Interval)
- {
- last_tim = TIME_GetTicks();
-
- group_num = fml_imu_get_data_num(FML_IMU_DIR_FRONT);
-
- if(group_num > 0)
- {
- for(i=0;i<group_num;i++)
- {
- fml_imu_get_data(FML_IMU_DIR_FRONT, i, &data);
- f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
- f_mag[0] = data.mag[0];f_mag[1] = data.mag[1];f_mag[2] = data.mag[2];
-
- monitor_sensor_data(f_acc, NULL, f_mag, NULL);
- }
- }
- }
- }
- else if(ob_fml_imu.stage == FML_IMU_CONFIG_STAGE_DONE && ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm.acc_odr == LSM_ACC_ODR_12HZ5)
- {
- if(TIME_GetTicks()-last_tim >= StandByPower_Interval)
- {
- last_tim = TIME_GetTicks();
-
- group_num = fml_imu_get_data_num(FML_IMU_DIR_FRONT);
-
- if(group_num > 0)
- {
- for(i=0;i<group_num;i++)
- {
- fml_imu_get_data(FML_IMU_DIR_FRONT, i, &data);
- f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
-
- monitor_sensor_data(f_acc, NULL, NULL, NULL);
- }
- }
-
- }
- }
- }
- static void fml_imu_Process(void)
- {
- int data_num[FML_IMU_DIR_NUM];
- uint32_t dir_bit;
- static char buf[255];
- static uint32_t config_err_report_tim = 0;
-
- switch(ob_fml_imu.stage)
- {
- case FML_IMU_CONFIG_STAGE_DONE:
- dir_bit = 0;
-
- data_num[FML_IMU_DIR_FRONT] = hal_ser_imu_read_data(FML_IMU_DIR_FRONT);
- if(data_num[FML_IMU_DIR_FRONT] > 0){
- ob_fml_imu.cur_data_num[FML_IMU_DIR_FRONT] = data_num[FML_IMU_DIR_FRONT];
- dir_bit |= (1 << FML_IMU_DIR_FRONT);
- }
-
- data_num[FML_IMU_DIR_BACK] = hal_ser_imu_read_data(FML_IMU_DIR_BACK);
- if(data_num[FML_IMU_DIR_BACK] > 0){
- ob_fml_imu.cur_data_num[FML_IMU_DIR_BACK] = data_num[FML_IMU_DIR_BACK];
- dir_bit |= (1 << FML_IMU_DIR_BACK);
- }
-
- fml_imu_monitor_sensor_data_process(data_num[FML_IMU_DIR_FRONT], data_num[FML_IMU_DIR_BACK]);
-
- if(dir_bit != 0 && ob_fml_imu.data_notify_cb != NULL)ob_fml_imu.data_notify_cb(dir_bit);
- break;
-
- case FML_IMU_CONFIG_STAGE_IN_PROGRESS:
- if(fml_imu_intergrated_setting() == -1)
- {
-
- ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_FAIL;
-
- if(ob_fml_imu.config_cb != NULL)ob_fml_imu.config_cb(ob_fml_imu.config_flow);
-
- sprintf(buf,"front_imu_suspend_overflow:0x%x\r\n",ob_fml_imu.config_flow);
- Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf);
-
- fml_imu_close(FML_IMU_DIR_FRONT);
- fml_imu_close(FML_IMU_DIR_BACK);
-
- Process_SetHoldOn(fml_imu_Process,0);
- }
- else
- {
- if(ob_fml_imu.config_flow == FML_IMU_CONFIG_FLOW_DONE)
- {
-
- ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_DONE;
-
- if(ob_fml_imu.config_cb != NULL)ob_fml_imu.config_cb(ob_fml_imu.config_flow);
-
- drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm);
-
- drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc);
-
- Process_SetHoldOn(fml_imu_Process,0);
- }
- }
- break;
-
- default:
-
- if(config_err_report_tim == 0){
- config_err_report_tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-config_err_report_tim>=1000){
- sprintf(buf,"front_imu_suspend_overflow:0x%x\r\n",ob_fml_imu.config_flow);
- Except_TxError(EXCEPT_IMU_SUSPEND_OVERFLOW,buf);
- config_err_report_tim = 0;
- }
- break;
- }
- }
- int fml_imu_Init(void)
- {
- int ret;
-
-
-
-
- memset(&ob_fml_imu,0,sizeof(ob_fml_imu));
-
-
- ret = drv_lsm_Init();
-
- ret += drv_qmc6310_Init();
-
-
- drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm);
-
-
- drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc);
-
-
-
-
- ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_DONE;
-
- Process_Start(0,"fml_imu_Process",fml_imu_Process);
-
-
- if(ret < 0)return -1;
-
- return 0;
-
- }
- int fml_imu_config_param(FML_IMU_DIR_e dir, const fml_imu_param_t *param)
- {
- if(param == NULL || dir >= FML_IMU_DIR_NUM)return -1;
-
- memcpy(&ob_fml_imu.config_param[dir],param,sizeof(fml_imu_param_t));
-
- return 0;
- }
- int fml_imu_register_config_callback(fml_imu_config_cb cb)
- {
- if(cb == NULL)return -1;
-
- ob_fml_imu.config_cb = cb;
-
- return 0;
- }
- int fml_imu_register_data_notify_callback(fml_imu_data_notify_cb cb)
- {
- if(cb == NULL)return -1;
-
- ob_fml_imu.data_notify_cb = cb;
-
- return 0;
- }
- int fml_imu_start_config(void)
- {
- drv_param_u lsm_drv_param;
- drv_param_u qmc_drv_param;
-
-
- drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm);
-
-
- drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc);
-
-
- fml_imu_macro_conversion(FML_IMU_DIR_FRONT, &ob_fml_imu.config_param[FML_IMU_DIR_FRONT], &lsm_drv_param);
- fml_imu_macro_conversion(FML_IMU_DIR_BACK, &ob_fml_imu.config_param[FML_IMU_DIR_BACK], &qmc_drv_param);
-
- if((memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm,&lsm_drv_param.lsm,sizeof(lsm_drv_param.lsm)) != 0) || \
- (memcmp(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc,&qmc_drv_param.qmc,sizeof(qmc_drv_param.qmc)) != 0)
- )
- {
- ob_fml_imu.stage = FML_IMU_CONFIG_STAGE_IN_PROGRESS;
- ob_fml_imu.config_flow = 0;
-
- Process_SetHoldOn(fml_imu_Process,1);
-
- return 0;
- }
-
- return -1;
- }
- int fml_imu_get_data_num(FML_IMU_DIR_e dir)
- {
- if(dir >= FML_IMU_DIR_NUM)return 0;
-
- return ob_fml_imu.cur_data_num[dir];
- }
- int fml_imu_get_data(FML_IMU_DIR_e dir, int index, fml_imu_data_t *pdata)
- {
- if(index < 0 || pdata == NULL || dir >= FML_IMU_DIR_NUM)return -1;
-
- *pdata = ob_fml_imu.cur_data[dir][index];
-
- return 0;
- }
- int fml_imu_close(FML_IMU_DIR_e dir)
- {
- if(dir >= FML_IMU_DIR_NUM)return -1;
-
- switch(dir)
- {
- case FML_IMU_DIR_FRONT:
- drv_lsm_power_on();
- drv_lsm_power_off();
- memset(&ob_fml_imu.config_param[FML_IMU_DIR_FRONT],0,sizeof(ob_fml_imu.config_param[FML_IMU_DIR_FRONT]));
-
- drv_lsm_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_FRONT].lsm);
-
- ob_fml_imu.config_flow = FML_IMU_CONFIG_FLOW_DONE;
- break;
-
- case FML_IMU_DIR_BACK:
- drv_qmc6310_power_on();
- drv_qmc6310_power_off();
- memset(&ob_fml_imu.config_param[FML_IMU_DIR_BACK],0,sizeof(ob_fml_imu.config_param[FML_IMU_DIR_BACK]));
-
- drv_qmc6310_get_config_param(&ob_fml_imu.drv_param[FML_IMU_DIR_BACK].qmc);
-
- ob_fml_imu.config_flow = FML_IMU_CONFIG_FLOW_DONE;
- break;
-
- default:
- break;
- }
- return 0;
- }
|