/*! Save parameters to file which is specified with #path. This function saves variables when the offsets of magnetic sensor estimated successfully. @return If function fails, the return value is #AKM_FAIL. When function fails, the parameter file may collapsed. Therefore, the parameters file should be discarded. If function succeeds, the return value is #AKM_SUCCESS. @param[out] prms A pointer to #AK8975PRMS structure. Member variables are saved to the parameter file. @param[in] path A path to the setting file. */ int16 AKFS_SaveParameters(AK8975PRMS *prms, const char* path) { int16 ret = 1; FILE *fp; /*Open setting file for write. */ if ((fp = fopen(path, "w")) == NULL) { AKMERROR_STR("fopen"); return AKM_FAIL; } /* Save data to HO */ if (fprintf(fp, AKFS_PRINTF_FORMAT, "HO.x", prms->mfv_ho.u.x) < 0) { ret = 0; } if (fprintf(fp, AKFS_PRINTF_FORMAT, "HO.y", prms->mfv_ho.u.y) < 0) { ret = 0; } if (fprintf(fp, AKFS_PRINTF_FORMAT, "HO.z", prms->mfv_ho.u.z) < 0) { ret = 0; } if (fclose(fp) != 0) { AKMERROR_STR("fclose"); ret = 0; } if (ret == 0) { AKMERROR; return AKM_FAIL; } return AKM_SUCCESS; }
/*! Load parameters from file which is specified with #path. This function reads data from a beginning of the file line by line, and check parameter name sequentially. In other words, this function depends on the order of eache parameter described in the file. @return If function fails, the return value is #AKM_ERROR. When function fails, the output is undefined. Therefore, parameters which are possibly overwritten by this function should be initialized again. If function succeeds, the return value is #AKM_SUCCESS. @param[out] prms A pointer to #AKMPRMS structure. Loaded parameter is stored to the member of this structure. @param[in] path A path to the setting file. */ int16 AKFS_LoadParameters(AKMPRMS * prms, const char* path) { int16 ret; char buf[LOAD_BUF_SIZE]; AKFLOAT tmpF; FILE *fp = NULL; /* Open setting file for read. */ if ((fp = fopen(path, "r")) == NULL) { AKMERROR_STR("fopen"); return AKM_ERROR; } ret = 1; /* Load data to HO */ if (fscanf(fp, AKFS_SCANF_FORMAT, buf, &tmpF) != 2) { ret = 0; } else { if (strncmp(buf, "HO.x", sizeof(buf)) != 0) { ret = 0; } else { prms->fv_ho.u.x = tmpF; } } if (fscanf(fp, AKFS_SCANF_FORMAT, buf, &tmpF) != 2) { ret = 0; } else { if (strncmp(buf, "HO.y", sizeof(buf)) != 0) { ret = 0; } else { prms->fv_ho.u.y = tmpF; } } if (fscanf(fp, AKFS_SCANF_FORMAT, buf, &tmpF) != 2) { ret = 0; } else { if (strncmp(buf, "HO.z", sizeof(buf)) != 0) { ret = 0; } else { prms->fv_ho.u.z = tmpF; } } if (fclose(fp) != 0) { AKMERROR_STR("fclose"); ret = 0; } if (ret == 0) { AKMERROR; return AKM_ERROR; } return AKM_SUCCESS; }
/* This function is called just before a measurement sequence starts. Load parameters from a file and initialize library. This function must be called when a sequential measurement thread boots up. @return The return value is #AKM_SUCCESS. @param[in/out] mem A pointer to a handler. @param[in] path The path to a setting file to be read out. The path name should be terminated with NULL. */ int16 AKFS_Start(void *mem, const char *path) { AKMPRMS *prms; #ifdef AKM_VALUE_CHECK if (mem == NULL || path == NULL) { AKMDEBUG(AKMDATA_CHECK, "%s: Invalid mem pointer.", __FUNCTION__); return AKM_ERROR; } #endif AKMDEBUG(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path); /* Copy pointer */ prms = (AKMPRMS *)mem; /* Read setting files from a file */ if (AKFS_LoadParameters(prms, path) != AKM_SUCCESS) { AKMERROR_STR("AKFS_LoadParameters"); } /* Initialize buffer */ AKFS_InitBuffer(AKFS_HDATA_SIZE, prms->fva_hdata); AKFS_InitBuffer(AKFS_HDATA_SIZE, prms->fva_hvbuf); AKFS_InitBuffer(AKFS_ADATA_SIZE, prms->fva_avbuf); /* Initialize for AOC */ AKFS_InitAOC(&prms->s_aocv); /* Initialize magnetic status */ prms->i16_hstatus = 0; return AKM_SUCCESS; }
/*! Read sensitivity adjustment data from fuse ROM. @return If data are read successfully, the return value is #AKM_SUCCESS. Otherwise the return value is #AKM_ERROR. @param[out] regs The read ASA values. When this function succeeds, ASAX value is saved in regs[0], ASAY is saved in regs[1], ASAZ is saved in regs[2]. */ int16 AKFS_ReadConf( uint8 regs[3] ) { BYTE conf[AKM_SENSOR_CONF_SIZE]; #ifdef AKM_VALUE_CHECK if (AKM_SENSOR_CONF_SIZE != 3) { AKMERROR_STR("You may refer invalid header file."); return AKM_ERROR; } #endif if (AKD_GetSensorConf(conf) != AKD_SUCCESS) { AKMERROR; return AKM_ERROR; } regs[0] = conf[0]; regs[1] = conf[1]; regs[2] = conf[2]; AKMDEBUG(AKMDATA_DUMP, "%s: asa(dec)=%d,%d,%d\n", __FUNCTION__, regs[0], regs[1], regs[2]); return AKM_SUCCESS; }
/*! Set calculated data to device driver. @param[in] buf */ void AKD_SetYPR(const int buf[AKM_YPR_DATA_SIZE]) { if (s_fdDev < 0) { AKMERROR; return; } if (ioctl(s_fdDev, ECS_IOCTL_SET_YPR, buf) < 0) { AKMERROR_STR("ioctl"); } }
/*! Reset the e-compass. @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise the return value is #AKD_FAIL. */ int16_t AKD_Reset(void) { if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } if (ioctl(s_fdDev, ECS_IOCTL_RESET, NULL) < 0) { AKMERROR_STR("ioctl"); return AKD_FAIL; } return AKD_SUCCESS; }
/*! */ int16_t AKD_GetCloseStatus(int* status) { if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } if (ioctl(s_fdDev, ECS_IOCTL_GET_CLOSE_STATUS, status) < 0) { AKMERROR_STR("ioctl"); return AKD_FAIL; } return AKD_SUCCESS; }
/*! Acquire delay @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise the return value is #AKD_FAIL. @param[out] delay A delay in microsecond. */ int16_t AKD_GetDelay(int64_t delay[AKM_NUM_SENSORS]) { if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } if (ioctl(s_fdDev, ECS_IOCTL_GET_DELAY, delay) < 0) { AKMERROR_STR("ioctl"); return AKD_FAIL; } return AKD_SUCCESS; }
/*! Set AKM E-Compass to the specific mode. @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise the return value is #AKD_FAIL. @param[in] mode This value should be one of the AKM_MODE which is defined in header file. */ int16_t AKD_SetMode(const BYTE mode) { if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } if (ioctl(s_fdDev, ECS_IOCTL_SET_MODE, & mode) < 0) { AKMERROR_STR("ioctl"); return AKD_FAIL; } return AKD_SUCCESS; }
/*! This function is called when a measurement sequence is done. This fucntion writes parameters to file. @return The return value is #AKM_SUCCESS. @param[in] path Specify a path to the settings file. */ int16 AKFS_Stop( const char* path ) { AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path); /* Write setting files to a file */ if (AKFS_SaveParameters(&g_prms, path) != AKM_SUCCESS) { AKMERROR_STR("AKFS_Save"); } return AKM_SUCCESS; }
/*! Get magnetic sensor configuration from device. This function returns ASA value. @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise the return value is #AKD_FAIL. @param[out] data An configuration data array. The size should be larger than #AKM_SENSOR_CONF_SIZE */ int16_t AKD_GetSensorConf(BYTE data[AKM_SENSOR_CONF_SIZE]) { memset(data, 0, AKM_SENSOR_CONF_SIZE); if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } if (ioctl(s_fdDev, ECS_IOCTL_GET_CONF, data) < 0) { AKMERROR_STR("ioctl"); return AKD_FAIL; } return AKD_SUCCESS; }
/*! Get layout information from device driver, i.e. platform data. */ int16_t AKD_GetLayout(int16_t* layout) { char tmp; if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } if (ioctl(s_fdDev, ECS_IOCTL_GET_LAYOUT, &tmp) < 0) { AKMERROR_STR("ioctl"); return AKD_FAIL; } *layout = tmp; return AKD_SUCCESS; }
/*! Open device driver. This function opens both device drivers of magnetic sensor and acceleration sensor. Additionally, some initial hardware settings are done, such as measurement range, built-in filter function and etc. @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise the return value is #AKD_FAIL. */ int16_t AKD_InitDevice(void) { if (s_fdDev < 0) { // Open magnetic sensor's device driver. if ((s_fdDev = open("/dev/" AKM_MISCDEV_NAME, O_RDWR)) < 0) { AKMERROR_STR("open"); goto INIT_FAIL; } } if (Acc_InitDevice() != AKD_SUCCESS) { goto INIT_FAIL; } return AKD_SUCCESS; INIT_FAIL: AKD_DeinitDevice(); return AKD_FAIL; }
/*! This function is called when a measurement sequence is done. Save parameters to a file. This function must be called when a sequential measurement thread ends. @return The return value is #AKM_SUCCESS. @param[in/out] mem A pointer to a handler. @param[in] path The path to a setting file to be written. The path name should be terminated with NULL. */ int16 AKFS_Stop(void *mem, const char *path) { AKMPRMS *prms; #ifdef AKM_VALUE_CHECK if (mem == NULL || path == NULL) { AKMDEBUG(AKMDATA_CHECK, "%s: Invalid mem pointer.", __FUNCTION__); return AKM_ERROR; } #endif AKMDEBUG(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path); /* Copy pointer */ prms = (AKMPRMS *)mem; /* Write setting files to a file */ if (AKFS_SaveParameters(prms, path) != AKM_SUCCESS) { AKMERROR_STR("AKFS_SaveParameters"); } return AKM_SUCCESS; }
/*! Acquires data from a register or the EEPROM of the AKM E-Compass. @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise the return value is #AKD_FAIL. @param[in] address Specify the address of a register from which data is to be read. @param[out] data Specify a pointer to a data array which the read data are stored. @param[in] numberOfBytesToRead Specify the number of bytes that make up the data to read. When a pointer to an array is specified in data, this argument equals the number of elements of the array. */ int16_t AKD_RxData( const BYTE address, BYTE * data, const uint16_t numberOfBytesToRead) { int i; char buf[AKM_RWBUF_SIZE]; memset(data, 0, numberOfBytesToRead); if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } if (numberOfBytesToRead > (AKM_RWBUF_SIZE-1)) { AKMERROR; return AKD_FAIL; } buf[0] = numberOfBytesToRead; buf[1] = address; if (ioctl(s_fdDev, ECS_IOCTL_READ, buf) < 0) { AKMERROR_STR("ioctl"); return AKD_FAIL; } else { for (i = 0; i < numberOfBytesToRead; i++) { data[i] = buf[i + 1]; } #if ENABLE_AKMDEBUG AKMDEBUG(AKMDBG_MAGDRV, "addr(HEX)=%02x len=%d data(HEX)=", address, numberOfBytesToRead); for (i = 0; i < numberOfBytesToRead; i++) { AKMDEBUG(AKMDBG_MAGDRV, " %02x", data[i]); } AKMDEBUG(AKMDBG_MAGDRV, "\n"); #endif return AKD_SUCCESS; } }
void ConsoleMode(void *mem) { /*** Console Mode *********************************************/ while (AKD_TRUE) { /* Select operation */ switch (Menu_Main()) { case MODE_Measure: /* Reset flag */ g_stopRequest = 0; /* Measurement routine */ thread_main(mem); break; case MODE_Quit: return; default: AKMERROR_STR("Unknown operation mode.\n"); break; } } }
/*! Get interval of each sensors from device driver. @return If this function succeeds, the return value is #AKM_SUCCESS. Otherwise the return value is #AKM_ERROR. @param flag This variable indicates what sensor frequency is updated. @param minimum This value show the minimum loop period in all sensors. */ int16 AKFS_GetInterval( uint16* flag, int64_t* minimum ) { /* Accelerometer, Magnetometer, Fusion */ /* Delay is in nano second unit. */ /* Negative value means the sensor is disabled.*/ int64_t delay[AKM_NUM_SENSORS]; int i; #ifdef AKM_VALUE_CHECK if (AKM_NUM_SENSORS != 3) { AKMERROR_STR("You may refer invalid header file."); return AKM_ERROR; } #endif if (AKD_GetDelay(delay) != AKD_SUCCESS) { AKMERROR; return AKM_ERROR; } AKMDEBUG(AKMDATA_LOOP, "delay[A,M,O]=%lld,%lld,%lld\n", delay[0], delay[1], delay[2]); /* update */ *minimum = 1000000000; *flag = 0; for (i=0; i<AKM_NUM_SENSORS; i++) { /* Set flag */ if (delay[i] >= 0) { *flag |= 1 << i; if (*minimum > delay[i]) { *minimum = delay[i]; } } } return AKM_SUCCESS; }
/*! Writes data to a register of the AKM E-Compass. When more than one byte of data is specified, the data is written in contiguous locations starting at an address specified in \a address. @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise the return value is #AKD_FAIL. @param[in] address Specify the address of a register in which data is to be written. @param[in] data Specify data to write or a pointer to a data array containing the data. When specifying more than one byte of data, specify the starting address of the array. @param[in] numberOfBytesToWrite Specify the number of bytes that make up the data to write. When a pointer to an array is specified in data, this argument equals the number of elements of the array. */ int16_t AKD_TxData( const BYTE address, const BYTE * data, const uint16_t numberOfBytesToWrite) { int i; char buf[AKM_RWBUF_SIZE]; if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } if (numberOfBytesToWrite > (AKM_RWBUF_SIZE-2)) { AKMERROR; return AKD_FAIL; } buf[0] = numberOfBytesToWrite + 1; buf[1] = address; for (i = 0; i < numberOfBytesToWrite; i++) { buf[i + 2] = data[i]; } if (ioctl(s_fdDev, ECS_IOCTL_WRITE, buf) < 0) { AKMERROR_STR("ioctl"); return AKD_FAIL; } else { #if ENABLE_AKMDEBUG AKMDEBUG(AKMDBG_MAGDRV, "addr(HEX)=%02x data(HEX)=", address); for (i = 0; i < numberOfBytesToWrite; i++) { AKMDEBUG(AKMDBG_MAGDRV, " %02x", data[i]); } AKMDEBUG(AKMDBG_MAGDRV, "\n"); #endif return AKD_SUCCESS; } }
/*! Acquire magnetic data from AKM E-Compass. If measurement is not done, this function waits until measurement completion. @return If this function succeeds, the return value is #AKD_SUCCESS. Otherwise the return value is #AKD_FAIL. @param[out] data A magnetic data array. The size should be larger than #AKM_SENSOR_DATA_SIZE. */ int16_t AKD_GetMagneticData(BYTE data[AKM_SENSOR_DATA_SIZE]) { int ret; int i; memset(data, 0, AKM_SENSOR_DATA_SIZE); if (s_fdDev < 0) { AKMERROR; return AKD_FAIL; } for (i = 0; i < AKM_MEASURE_RETRY_NUM; i++) { ret = ioctl(s_fdDev, ECS_IOCTL_GETDATA, data); if (ret >= 0) { /* Success */ break; } if (errno != EAGAIN) { AKMERROR_STR("ioctl"); return AKD_FAIL; } AKMDEBUG(AKMDBG_MAGDRV, "Try Again."); usleep(AKM_MEASURE_TIME_US); } if (i >= AKM_MEASURE_RETRY_NUM) { AKMERROR; return AKD_FAIL; } AKMDEBUG(AKMDBG_MAGDRV, "bdata(HEX)= %02x %02x %02x %02x %02x %02x %02x %02x %02x\n", data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8]); return AKD_SUCCESS; }
/*! If this program run as console mode, measurement result will be displayed on console terminal. @return None. */ void AKFS_OutputResult( const uint16 flag, const AKSENSOR_DATA* acc, const AKSENSOR_DATA* mag, const AKSENSOR_DATA* ori ) { int buf[AKM_YPR_DATA_SIZE]; #ifdef AKM_VALUE_CHECK if (AKM_YPR_DATA_SIZE < 12) { AKMERROR_STR("You may refer invalid header file."); return; } #endif /* Store to buffer */ buf[0] = flag; /* Data flag */ buf[1] = CONVERT_ACC(acc->x); /* Ax */ buf[2] = CONVERT_ACC(acc->y); /* Ay */ buf[3] = CONVERT_ACC(acc->z); /* Az */ buf[4] = acc->status; /* Acc status */ buf[5] = CONVERT_MAG(mag->x); /* Mx */ buf[6] = CONVERT_MAG(mag->y); /* My */ buf[7] = CONVERT_MAG(mag->z); /* Mz */ buf[8] = mag->status; /* Mag status */ buf[9] = CONVERT_ORI(ori->x); /* yaw */ buf[10] = CONVERT_ORI(ori->y); /* pitch */ buf[11] = CONVERT_ORI(ori->z); /* roll */ if (g_opmode & OPMODE_CONSOLE) { /* Console mode */ Disp_Result(buf); } /* Set result to driver */ AKD_SetYPR(buf); }
/* This function is called just before a measurement sequence starts. This function reads parameters from file, then initializes algorithm parameters. @return The return value is #AKM_SUCCESS. @param[in] path Specify a path to the settings file. */ int16 AKFS_Start( const char* path ) { AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path); /* Read setting files from a file */ if (AKFS_LoadParameters(&g_prms, path) != AKM_SUCCESS) { AKMERROR_STR("AKFS_Load"); } /* Initialize buffer */ AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hdata); AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hvbuf); AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_adata); AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_avbuf); /* Initialize for AOC */ AKFS_InitAOC(&g_prms.m_aocv); /* Initialize magnetic status */ g_prms.mi_hstatus = 0; return AKM_SUCCESS; }
/*! This function parse the option. @retval 1 Parse succeeds. @retval 0 Parse failed. @param[in] argc Argument count @param[in] argv Argument vector @param[out] layout_patno */ int OptParse( int argc, char* argv[], AKFS_PATNO* layout_patno) { #ifdef WIN32 /* Static */ #if defined(AKFS_WIN32_PAT1) *layout_patno = PAT1; #elif defined(AKFS_WIN32_PAT2) *layout_patno = PAT2; #elif defined(AKFS_WIN32_PAT3) *layout_patno = PAT3; #elif defined(AKFS_WIN32_PAT4) *layout_patno = PAT4; #elif defined(AKFS_WIN32_PAT5) *layout_patno = PAT5; #else *layout_patno = PAT1; #endif g_opmode = OPMODE_CONSOLE; /*g_opmode = 0;*/ g_dbgzone = AKMDATA_DUMP | AKMDATA_DEBUG | AKMDATA_CONSOLE; #else int opt; char optVal; *layout_patno = PAT_INVALID; while ((opt = getopt(argc, argv, "sm:z:")) != -1) { switch(opt){ case 'm': optVal = (char)(optarg[0] - '0'); if ((PAT1 <= optVal) && (optVal <= PAT8)) { *layout_patno = (AKFS_PATNO)optVal; AKMDEBUG(AKMDATA_DEBUG, "%s: Layout=%d\n", __FUNCTION__, optVal); } break; case 's': g_opmode |= OPMODE_CONSOLE; break; case 'z': /* If error detected, hopefully 0 is returned. */ errno = 0; g_dbgzone = (int)strtol(optarg, (char**)NULL, 0); AKMDEBUG(AKMDATA_DEBUG, "%s: Dbg Zone=%d\n", __FUNCTION__, g_dbgzone); break; default: AKMERROR_STR("Invalid argument"); return 0; } } /* If layout is not specified with argument, get parameter from driver */ if (*layout_patno == PAT_INVALID) { int16_t n = 0; if (AKD_GetLayout(&n) == AKD_SUCCESS) { if ((PAT1 <= n) && (n <= PAT8)) { *layout_patno = (AKFS_PATNO)n; } } AKMDEBUG(AKMDATA_DEBUG, "Layout=%d\n", n); } /* Error */ if (*layout_patno == PAT_INVALID) { AKMERROR_STR("No layout is specified."); return 0; } #endif return 1; }