/*!
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;
}
