/* 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;
}
/*!
Output main menu to stdout and wait for user input from stdin.
@return Selected mode.
*/
MODE Menu_Main(void)
{
char msg[20];
memset(msg, 0, sizeof(msg));
AKMDEBUG(DBG_LEVEL1,
" -------------------- AK8975 Console Application -------------------- \n"
" 1. Start measurement. \n"
" 2. Self-test. \n"
" Q. Quit application. \n"
" --------------------------------------------------------------------- \n"
" Please select a number.\n"
" ---> ");
fgets(msg, 10, stdin);
AKMDEBUG(DBG_LEVEL1, "\n");
/* BUG : If 2-digits number is input, */
/* only the first character is compared. */
if (!strncmp(msg, "1", 1)) {
return MODE_Measure;
} else if (!strncmp(msg, "2", 1)) {
return MODE_SelfTest;
} else if (strncmp(msg, "Q", 1) == 0 || strncmp(msg, "q", 1) == 0) {
return MODE_Quit;
} else {
return MODE_ERROR;
}
}
/*!
Print result
*/
void Disp_Result(int buf[YPR_DATA_SIZE])
{
AKMDEBUG(DBG_LEVEL1,
"Flag=%d\n", buf[0]);
AKMDEBUG(DBG_LEVEL1,
"Acc(%d):%8.2f, %8.2f, %8.2f\n",
buf[4], REVERT_ACC(buf[1]), REVERT_ACC(buf[2]), REVERT_ACC(buf[3]));
AKMDEBUG(DBG_LEVEL1,
"Mag(%d):%8.2f, %8.2f, %8.2f\n",
buf[8], REVERT_MAG(buf[5]), REVERT_MAG(buf[6]), REVERT_MAG(buf[7]));
AKMDEBUG(DBG_LEVEL1,
"Ori(%d)=%8.2f, %8.2f, %8.2f\n",
buf[8], REVERT_ORI(buf[9]), REVERT_ORI(buf[10]), REVERT_ORI(buf[11]));
}
/*!
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;
}
/*!
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_FAIL.
@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_ReadAK8975FUSEROM(
uint8 regs[3]
)
{
/* Set to FUSE ROM access mode */
if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Read values. ASAX, ASAY, ASAZ */
if (AKD_RxData(AK8975_FUSE_ASAX, regs, 3) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Set to PowerDown mode */
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
AKMDEBUG(DBG_LEVEL2, "%s: asa(dec)=%d,%d,%d\n",
__FUNCTION__, regs[0], regs[1], regs[2]);
return AKM_SUCCESS;
}
/*! Release allocated memory. This function must be called at the end of using
APIs. Currently this function is empty.
@return None
@param[in/out] mem A pointer to a handler.
*/
void AKFS_Release(void *mem)
{
#ifdef AKM_VALUE_CHECK
if (mem == NULL) {
AKMDEBUG(AKMDATA_CHECK, "%s: Invalid mem pointer.", __FUNCTION__);
return;
}
#endif
/* Do nothing */
}
/*! 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;
}
/*! Initialize #AKMPRMS structure and make APIs ready to use. This function
must be called before application uses any APIs in this file. If #AKMPRMS
are already initialized, this function discards all existing data. When APIs
are not used anymore, #AKM_Release function must be called at the end.
When this function succeeds, form factor number is set to 0.
@return #AKM_SUCCESS on success. #AKM_ERROR if an error occurred.
@param[in/out] mem A pointer to a handler.
@param[in] hpat Specify a layout pattern number. The number is determined
according to the mount orientation of the magnetometer.
@param[in] regs[3] Specify the ASA values which are read out from
fuse ROM. regs[0] is ASAX, regs[1] is ASAY, regs[2] is ASAZ.
*/
int16 AKFS_Init(
void *mem,
const AKFS_PATNO hpat,
const uint8 regs[]
)
{
AKMPRMS *prms;
#ifdef AKM_VALUE_CHECK
if (mem == NULL) {
AKMDEBUG(AKMDATA_CHECK, "%s: Invalid mem pointer.", __FUNCTION__);
return AKM_ERROR;
}
#endif
AKMDEBUG(AKMDATA_DUMP, "%s: hpat=%d, r[0]=0x%02X, r[1]=0x%02X, r[2]=0x%02X\n",
__FUNCTION__, hpat, regs[0], regs[1], regs[2]);
/* Copy pointer */
prms = (AKMPRMS *)mem;
/* Clear all data. */
memset(prms, 0, sizeof(AKMPRMS));
/* Sensitivity */
prms->fv_hs.u.x = AKM_MAG_SENSE;
prms->fv_hs.u.y = AKM_MAG_SENSE;
prms->fv_hs.u.z = AKM_MAG_SENSE;
prms->fv_as.u.x = AKM_ACC_SENSE;
prms->fv_as.u.y = AKM_ACC_SENSE;
prms->fv_as.u.z = AKM_ACC_SENSE;
/* Copy ASA values */
prms->i8v_asa.u.x = regs[0];
prms->i8v_asa.u.y = regs[1];
prms->i8v_asa.u.z = regs[2];
/* Copy layout pattern */
prms->e_hpat = hpat;
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;
}
}
/*!
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[],
AKMD_PATNO* hlayout_patno)
{
int opt;
char optVal;
/* Initial value */
*hlayout_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)) {
*hlayout_patno = (AKMD_PATNO)optVal;
}
break;
case 'z':
/* If error detected, hopefully 0 is returned. */
g_dbgzone = (int)strtol(optarg, (char**)NULL, 0);
break;
case 's':
g_opmode |= OPMODE_CONSOLE;
break;
default:
ALOGE("%s: Invalid argument", argv[0]);
return 0;
}
}
AKMDEBUG(AKMDBG_DEBUG, "%s: Mode=0x%04X\n", __FUNCTION__, g_opmode);
AKMDEBUG(AKMDBG_DEBUG, "%s: Layout=%d\n", __FUNCTION__, *hlayout_patno);
AKMDEBUG(AKMDBG_DEBUG, "%s: Dbg Zone=0x%04X\n", __FUNCTION__, g_dbgzone);
return 1;
}
/*!
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;
}
/*!
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;
}
}
/*!
This function is called when new accelerometer data is available. The output
vector format and coordination system follow the Android definition.
@return The return value is #AKM_SUCCESS when function succeeds. Otherwise
the return value is #AKM_ERROR.
@param[in] acc A set of measurement data from accelerometer. X axis value
should be in acc[0], Y axis value should be in acc[1], Z axis value should be
in acc[2].
@param[in] status A status of accelerometer. This status indicates the result
of acceleration data, i.e. overflow, success or fail, etc.
*/
int16 AKFS_Get_ACCELEROMETER(
void *mem,
const int16 acc[3],
const int16 status,
AKFLOAT *ax,
AKFLOAT *ay,
AKFLOAT *az,
int16 *accuracy
)
{
AKMPRMS *prms;
#ifdef AKM_VALUE_CHECK
if (mem == NULL) {
AKMDEBUG(AKMDATA_CHECK, "%s: Invalid mem pointer.", __FUNCTION__);
return AKM_ERROR;
}
if (ax == NULL || ay == NULL || az == NULL || accuracy == NULL) {
AKMDEBUG(AKMDATA_CHECK, "%s: Invalid data pointer.", __FUNCTION__);
return AKM_ERROR;
}
#endif
/* Copy pointer */
prms = (AKMPRMS *)mem;
if (AKFS_Set_ACCELEROMETER(prms, acc, status) != AKM_SUCCESS) {
return AKM_ERROR;
}
/* Success */
*ax = prms->fv_avec.u.x;
*ay = prms->fv_avec.u.y;
*az = prms->fv_avec.u.z;
*accuracy = 3;
return AKM_SUCCESS;
}
/*!
This function is called when new magnetometer data is available. The output
vector format and coordination system follow the Android definition.
@return The return value is #AKM_SUCCESS.
Otherwise the return value is #AKM_ERROR.
@param[in/out] mem A pointer to a handler.
@param[in] mag A set of measurement data from magnetometer. X axis value
should be in mag[0], Y axis value should be in mag[1], Z axis value should be
in mag[2].
@param[in] status A status of magnetometer. This status indicates the result
of measurement data, i.e. overflow, success or fail, etc.
*/
int16 AKFS_Get_MAGNETIC_FIELD(
void *mem,
const int16 mag[3],
const int16 status,
AKFLOAT *hx,
AKFLOAT *hy,
AKFLOAT *hz,
int16 *accuracy
)
{
AKMPRMS *prms;
//#define AKM_VALUE_CHECK
#ifdef AKM_VALUE_CHECK
if (mem == NULL) {
AKMDEBUG(AKMDATA_CHECK, "%s: Invalid mem pointer.", __FUNCTION__);
return AKM_ERROR;
}
if (hx == NULL || hy == NULL || hz == NULL || accuracy == NULL) {
AKMDEBUG(AKMDATA_CHECK, "%s: Invalid data pointer.", __FUNCTION__);
return AKM_ERROR;
}
#endif
/* Copy pointer */
prms = (AKMPRMS *)mem;
if (AKFS_Set_MAGNETIC_FIELD(prms, mag, status) != AKM_SUCCESS) {
return AKM_ERROR;
}
/* Success */
*hx = prms->fv_hvec.u.x;
*hy = prms->fv_hvec.u.y;
*hz = prms->fv_hvec.u.z;
*accuracy = prms->i16_hstatus;
return AKM_SUCCESS;
}
/*!
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;
}
/*!
This is main function.
*/
int main(int argc, char **argv)
{
AK8963PRMS prms;
int retValue = 0;
AKMD_PATNO pat;
int16 outbit;
/* Show the version info of this software. */
Disp_StartMessage();
#if ENABLE_AKMDEBUG
/* Register signal handler */
signal(SIGINT, signal_handler);
#endif
#if ENABLE_FORMATION
RegisterFormClass(&s_formClass);
#endif
/* Open device driver. */
if (AKD_InitDevice() != AKD_SUCCESS) {
retValue = ERROR_INITDEVICE;
goto THE_END_OF_MAIN_FUNCTION;
}
/* Parse command-line options */
if (OptParse(argc, argv, &pat, &outbit) == 0) {
retValue = ERROR_OPTPARSE;
goto THE_END_OF_MAIN_FUNCTION;
}
/* Initialize parameters structure. */
InitAK8963PRMS(&prms);
/* Put argument to PRMS. */
prms.m_layout = pat;
prms.m_outbit = outbit;
/* Read Fuse ROM */
if (ReadAK8963FUSEROM(&prms) != AKRET_PROC_SUCCEED) {
retValue = ERROR_FUSEROM;
goto THE_END_OF_MAIN_FUNCTION;
}
/* Here is the Main Loop */
if (g_opmode) {
/*** Console Mode *********************************************/
while (AKD_TRUE) {
/* Select operation */
switch (Menu_Main()) {
case MODE_FctShipmntTestBody:
FctShipmntTest_Body(&prms);
break;
case MODE_MeasureSNG:
/* Read Parameters from file. */
if (LoadParameters(&prms) == 0) {
SetDefaultPRMS(&prms);
}
/* Reset flag */
g_stopRequest = 0;
/* Measurement routine */
MeasureSNGLoop(&prms);
/* Write Parameters to file. */
SaveParameters(&prms);
break;
case MODE_Quit:
goto THE_END_OF_MAIN_FUNCTION;
break;
default:
AKMDEBUG(DBG_LEVEL0, "Unknown operation mode.\n");
break;
}
}
} else {
/*** Daemon Mode *********************************************/
while (g_mainQuit == AKD_FALSE) {
int st = 0;
/* Wait until device driver is opened. */
if (AKD_GetOpenStatus(&st) != AKD_SUCCESS) {
retValue = ERROR_GETOPEN_STAT;
goto THE_END_OF_MAIN_FUNCTION;
}
if (st == 0) {
ALOGI("Suspended.");
} else {
ALOGI("Compass Opened.");
/* Read Parameters from file. */
if (LoadParameters(&prms) == 0) {
SetDefaultPRMS(&prms);
}
/* Reset flag */
g_stopRequest = 0;
/* Start measurement thread. */
if (startClone(&prms) == 0) {
retValue = ERROR_STARTCLONE;
goto THE_END_OF_MAIN_FUNCTION;
}
/* Wait until device driver is closed. */
if (AKD_GetCloseStatus(&st) != AKD_SUCCESS) {
retValue = ERROR_GETCLOSE_STAT;
g_mainQuit = AKD_TRUE;
}
/* Wait thread completion. */
g_stopRequest = 1;
pthread_join(s_thread, NULL);
ALOGI("Compass Closed.");
/* Write Parameters to file. */
SaveParameters(&prms);
}
}
}
THE_END_OF_MAIN_FUNCTION:
/* Close device driver. */
AKD_DeinitDevice();
/* Show the last message. */
Disp_EndMessage(retValue);
return retValue;
}
/*!
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[],
AKMD_PATNO* layout_patno,
int16* outbit)
{
int opt;
char optVal;
*layout_patno = PAT_INVALID;
*outbit = OUTBIT_INVALID;
while ((opt = getopt(argc, argv, "sm:o:z:")) != -1) {
switch(opt){
case 'm':
optVal = (char)(optarg[0] - '0');
if ((PAT1 <= optVal) && (optVal <= PAT8)) {
*layout_patno = (AKMD_PATNO)optVal;
AKMDEBUG(DBG_LEVEL2, "%s: Layout=%d\n", __FUNCTION__, optVal);
}
break;
case 'o':
optVal = (char)(optarg[0] - '0');
if ((optVal == OUTBIT_14) || (optVal == OUTBIT_16)) {
*outbit = optVal;
AKMDEBUG(DBG_LEVEL2, "%s: outbit=%d\n", __FUNCTION__, *outbit);
}
break;
case 'z':
/* If error detected, hopefully 0 is returned. */
errno = 0;
g_dbgzone = (int)strtol(optarg, (char**)NULL, 0);
AKMDEBUG(DBG_LEVEL2, "%s: Dbg Zone=%d\n", __FUNCTION__, g_dbgzone);
break;
case 's':
g_opmode = 1;
break;
default:
ALOGE("%s: Invalid argument", argv[0]);
return 0;
}
}
/* If layout is not specified with argument, get parameter from driver */
if (*layout_patno == PAT_INVALID) {
int16_t n;
if (AKD_GetLayout(&n) == AKD_SUCCESS) {
if ((PAT1 <= n) && (n <= PAT8)) {
*layout_patno = (AKMD_PATNO)n;
}
}
}
/* Error */
if (*layout_patno == PAT_INVALID) {
ALOGE("No layout is specified.");
return 0;
}
/* If outbit is not specified with argument, get parameter from driver */
if (*outbit == OUTBIT_INVALID) {
int16_t b;
if (AKD_GetOutbit(&b) == AKD_SUCCESS) {
if ((b == OUTBIT_14) || (b == OUTBIT_16)) {
*outbit = b;
}
}
}
/* Error */
if (*outbit == OUTBIT_INVALID) {
ALOGE("No outbit is specified.");
return 0;
}
return 1;
}
/*!
Carry out self-test.
@return If this function succeeds, the return value is #AKM_SUCCESS.
Otherwise the return value is #AKM_FAIL.
*/
int16 AKFS_SelfTest(void)
{
BYTE i2cData[SENSOR_DATA_SIZE];
BYTE asa[3];
AKFLOAT hdata[3];
int16 ret;
/* Set to FUSE ROM access mode */
if (AKD_SetMode(AK8975_MODE_FUSE_ACCESS) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Read values from ASAX to ASAZ */
if (AKD_RxData(AK8975_FUSE_ASAX, asa, 3) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Set to PowerDown mode */
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Set to self-test mode */
i2cData[0] = 0x40;
if (AKD_TxData(AK8975_REG_ASTC, i2cData, 1) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/* Set to Self-test mode */
if (AKD_SetMode(AK8975_MODE_SELF_TEST) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
/*
Wait for DRDY pin changes to HIGH.
Get measurement data from AK8975
*/
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
AKMERROR;
return AKM_FAIL;
}
hdata[0] = AK8975_HDATA_CONVERTER(i2cData[2], i2cData[1], asa[0]);
hdata[1] = AK8975_HDATA_CONVERTER(i2cData[4], i2cData[3], asa[1]);
hdata[2] = AK8975_HDATA_CONVERTER(i2cData[6], i2cData[5], asa[2]);
/* Test */
ret = 1;
if ((hdata[0] < AK8975_SELFTEST_MIN_X) ||
(AK8975_SELFTEST_MAX_X < hdata[0])) {
ret = 0;
}
if ((hdata[1] < AK8975_SELFTEST_MIN_Y) ||
(AK8975_SELFTEST_MAX_Y < hdata[1])) {
ret = 0;
}
if ((hdata[2] < AK8975_SELFTEST_MIN_Z) ||
(AK8975_SELFTEST_MAX_Z < hdata[2])) {
ret = 0;
}
AKMDEBUG(DBG_LEVEL2, "Test(%s):%8.2f, %8.2f, %8.2f\n",
(ret ? "Success" : "fail"), hdata[0], hdata[1], hdata[2]);
if (ret) {
return AKM_SUCCESS;
} else {
return AKM_FAIL;
}
}
/*!
This is main function.
*/
int main(int argc, char **argv)
{
AKSCPRMS prms;
int retValue = 0;
/* Show the version info of this software. */
Disp_StartMessage();
#if ENABLE_AKMDEBUG
/* Register signal handler */
signal(SIGINT, signal_handler);
#endif
#if ENABLE_FORMATION
RegisterFormClass(&s_formClass);
#endif
/* Initialize parameters structure. */
InitAKSCPRMS(&prms);
/* Parse command-line options */
if (OptParse(argc, argv, &prms.m_hlayout) == 0) {
retValue = ERROR_OPTPARSE;
goto THE_END_OF_MAIN_FUNCTION;
}
/* Open device driver. */
if (AKD_InitDevice() != AKD_SUCCESS) {
retValue = ERROR_INITDEVICE;
goto THE_END_OF_MAIN_FUNCTION;
}
/* If layout is not specified with argument, get parameter from driver */
if (prms.m_hlayout == PAT_INVALID) {
int16_t n;
if (AKD_GetLayout(&n) == AKD_SUCCESS) {
if ((PAT1 <= n) && (n <= PAT8)) {
prms.m_hlayout = (AKMD_PATNO)n;
}
}
/* Error */
if (prms.m_hlayout == PAT_INVALID) {
ALOGE("Magnetic sensor's layout is specified.");
retValue = ERROR_HLAYOUT;
goto THE_END_OF_MAIN_FUNCTION;
}
}
/* Read Fuse ROM */
if (ReadFUSEROM(&prms) != AKRET_PROC_SUCCEED) {
retValue = ERROR_FUSEROM;
goto THE_END_OF_MAIN_FUNCTION;
}
/* PDC */
LoadPDC(&prms);
/* Here is the Main Loop */
if (g_opmode & OPMODE_CONSOLE) {
/*** Console Mode *********************************************/
while (AKD_TRUE) {
/* Select operation */
switch (Menu_Main()) {
case MODE_FST:
FST_Body();
break;
case MODE_MeasureSNG:
/* Read Parameters from file. */
if (LoadParameters(&prms) == 0) {
SetDefaultPRMS(&prms);
}
/* Reset flag */
g_stopRequest = 0;
/* Measurement routine */
MeasureSNGLoop(&prms);
/* Write Parameters to file. */
SaveParameters(&prms);
break;
case MODE_OffsetCalibration:
/* Read Parameters from file. */
if (LoadParameters(&prms) == 0) {
SetDefaultPRMS(&prms);
}
/* measure offset (NOT sensitivity) */
if (SimpleCalibration(&prms) == AKRET_PROC_SUCCEED) {
SaveParameters(&prms);
}
break;
case MODE_Quit:
goto THE_END_OF_MAIN_FUNCTION;
break;
default:
AKMDEBUG(AKMDBG_DEBUG, "Unknown operation mode.\n");
break;
}
}
} else {
/*** Daemon Mode *********************************************/
while (g_mainQuit == AKD_FALSE) {
int st = 0;
/* Wait until device driver is opened. */
if (AKD_GetOpenStatus(&st) != AKD_SUCCESS) {
retValue = ERROR_GETOPEN_STAT;
goto THE_END_OF_MAIN_FUNCTION;
}
if (st == 0) {
AKMDEBUG(AKMDBG_DEBUG, "Suspended.");
} else {
AKMDEBUG(AKMDBG_DEBUG, "Compass Opened.");
/* Read Parameters from file. */
if (LoadParameters(&prms) == 0) {
SetDefaultPRMS(&prms);
}
/* Reset flag */
g_stopRequest = 0;
/* Start measurement thread. */
if (startClone(&prms) == 0) {
retValue = ERROR_STARTCLONE;
goto THE_END_OF_MAIN_FUNCTION;
}
/* Wait until device driver is closed. */
if (AKD_GetCloseStatus(&st) != AKD_SUCCESS) {
retValue = ERROR_GETCLOSE_STAT;
g_mainQuit = AKD_TRUE;
}
/* Wait thread completion. */
g_stopRequest = 1;
pthread_join(s_thread, NULL);
AKMDEBUG(AKMDBG_DEBUG, "Compass Closed.");
/* Write Parameters to file. */
SaveParameters(&prms);
}
}
}
THE_END_OF_MAIN_FUNCTION:
/* Close device driver. */
AKD_DeinitDevice();
/* Show the last message. */
Disp_EndMessage(retValue);
return retValue;
}
int main(int argc, char **argv)
{
int retValue = 0;
AKMPRMS prms;
AKFS_PATNO pat;
uint8 regs[3];
/* Show the version info of this software. */
Disp_StartMessage();
#if ENABLE_AKMDEBUG
/* Register signal handler */
signal(SIGINT, signal_handler);
#endif
/* Open device driver */
if(AKD_InitDevice() != AKD_SUCCESS) {
retValue = ERROR_INITDEVICE;
goto MAIN_QUIT;
}
/* Parse command-line options */
/* This function calls device driver function to get layout */
if (OptParse(argc, argv, &pat) == 0) {
retValue = ERROR_OPTPARSE;
goto MAIN_QUIT;
}
/* Self Test */
/*
if (g_opmode & OPMODE_FST){
if (AKFS_SelfTest() != AKD_SUCCESS) {
retValue = ERROR_SELF_TEST;
goto MAIN_QUIT;
}
}*/
/* OK, then start */
if (AKFS_ReadConf(regs) != AKM_SUCCESS) {
retValue = ERROR_READ_FUSE;
goto MAIN_QUIT;
}
/* Initialize library. */
if (AKFS_Init(&prms, pat, regs) != AKM_SUCCESS) {
retValue = ERROR_INIT;
goto MAIN_QUIT;
}
/* Start console mode */
if (g_opmode & OPMODE_CONSOLE) {
ConsoleMode((void *)&prms);
goto MAIN_QUIT;
}
/*** Start Daemon ********************************************/
while (g_mainQuit == AKD_FALSE) {
int st = 0;
/* Wait until device driver is opened. */
if (AKD_GetOpenStatus(&st) != AKD_SUCCESS) {
retValue = ERROR_GETOPEN_STAT;
goto MAIN_QUIT;
}
if (st == 0) {
AKMDEBUG(AKMDATA_LOOP, "Suspended.");
} else {
AKMDEBUG(AKMDATA_LOOP, "Compass Opened.");
/* Reset flag */
g_stopRequest = 0;
/* Start measurement thread. */
if (startClone((void *)&prms) == 0) {
retValue = ERROR_STARTCLONE;
goto MAIN_QUIT;
}
/* Wait until device driver is closed. */
if (AKD_GetCloseStatus(&st) != AKD_SUCCESS) {
retValue = ERROR_GETCLOSE_STAT;
g_mainQuit = AKD_TRUE;
}
/* Wait thread completion. */
g_stopRequest = 1;
pthread_join(s_thread, NULL);
AKMDEBUG(AKMDATA_LOOP, "Compass Closed.");
}
}
MAIN_QUIT:
/* Release library */
AKFS_Release(&prms);
/* Close device driver. */
AKD_DeinitDevice();
/* Show the last message. */
Disp_EndMessage(retValue);
return retValue;
}
/*!
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;
}
/*!
A thread function which is raised when measurement is started.
@param[in] args This parameter is not used currently.
*/
static void* thread_main(void* args)
{
AKMPRMS *prms;
BYTE i2cData[AKM_SENSOR_DATA_SIZE]; /* ST1 ~ ST2 */
int16 mag[3];
int16 mstat;
int16 acc[3];
struct timespec tsstart= {0, 0};
struct timespec tsend = {0, 0};
struct timespec doze;
int64_t minimum;
uint16 flag;
AKSENSOR_DATA sv_acc;
AKSENSOR_DATA sv_mag;
AKSENSOR_DATA sv_ori;
AKFLOAT tmpx, tmpy, tmpz;
int16 tmp_accuracy;
prms = (AKMPRMS *)args;
minimum = -1;
/* Initialize library functions and device */
if (AKFS_Start(prms, CSPEC_SETTING_FILE) != AKM_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
while (g_stopRequest != AKM_TRUE) {
/* Beginning time */
if (clock_gettime(CLOCK_MONOTONIC, &tsstart) < 0) {
AKMERROR;
goto MEASURE_END;
}
/* Get interval */
if (AKFS_GetInterval(&flag, &minimum) != AKM_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
if ((flag & ACC_DATA_READY) || (flag & FUSION_DATA_READY)) {
/* Get accelerometer */
if (AKD_GetAccelerationData(acc) != AKD_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
/* Calculate accelerometer vector */
if (AKFS_Get_ACCELEROMETER(prms, acc, 0, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
sv_acc.x = tmpx;
sv_acc.y = tmpy;
sv_acc.z = tmpz;
sv_acc.status = tmp_accuracy;
} else {
flag &= ~ACC_DATA_READY;
flag &= ~FUSION_DATA_READY;
}
}
if ((flag & MAG_DATA_READY) || (flag & FUSION_DATA_READY)) {
/* Set to measurement mode */
if (AKD_SetMode(AKM_MODE_SNG_MEASURE) != AKD_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
/* Wait for DRDY and get data from device */
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
AKMERROR;
goto MEASURE_END;
}
/* raw data to x,y,z value */
mag[0] = (int)((int16_t)(i2cData[2]<<8)+((int16_t)i2cData[1]));
mag[1] = (int)((int16_t)(i2cData[4]<<8)+((int16_t)i2cData[3]));
mag[2] = (int)((int16_t)(i2cData[6]<<8)+((int16_t)i2cData[5]));
mstat = i2cData[0] | i2cData[7];
/* Calculate magnetic field vector */
if (AKFS_Get_MAGNETIC_FIELD(prms, mag, mstat, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
sv_mag.x = tmpx;
sv_mag.y = tmpy;
sv_mag.z = tmpz;
sv_mag.status = tmp_accuracy;
} else {
flag &= ~MAG_DATA_READY;
flag &= ~FUSION_DATA_READY;
}
}
if (flag & FUSION_DATA_READY) {
if (AKFS_Get_ORIENTATION(prms, &tmpx, &tmpy, &tmpz, &tmp_accuracy) == AKM_SUCCESS) {
sv_ori.x = tmpx;
sv_ori.y = tmpy;
sv_ori.z = tmpz;
sv_ori.status = tmp_accuracy;
} else {
flag &= ~FUSION_DATA_READY;
}
}
/* Output result */
AKFS_OutputResult(flag, &sv_acc, &sv_mag, &sv_ori);
/* Ending time */
if (clock_gettime(CLOCK_MONOTONIC, &tsend) < 0) {
AKMERROR;
goto MEASURE_END;
}
/* Calculate duration */
doze = AKFS_CalcSleep(&tsend, &tsstart, minimum);
AKMDEBUG(AKMDATA_LOOP, "Sleep: %6.2f msec\n", (doze.tv_nsec/1000000.0f));
nanosleep(&doze, NULL);
#ifdef WIN32
if (_kbhit()) {
_getch();
break;
}
#endif
}
MEASURE_END:
/* Set to PowerDown mode */
if (AKD_SetMode(AKM_MODE_POWERDOWN) != AKD_SUCCESS) {
AKMERROR;
}
/* Save parameters */
if (AKFS_Stop(prms, CSPEC_SETTING_FILE) != AKM_SUCCESS) {
AKMERROR;
}
return ((void*)0);
}
/*!
Get orientation sensor's elements. The vector format and coordination system
follow the Android definition. Before this function is called, magnetic
field vector and acceleration vector should be stored in the buffer by
calling #AKFS_Get_MAGNETIC_FIELD and #AKFS_Get_ACCELEROMETER.
@return The return value is #AKM_SUCCESS when function succeeds. Otherwise
the return value is #AKM_ERROR.
@param[out] azimuth Azimuthal angle in degree.
@param[out] pitch Pitch angle in degree.
@param[out] roll Roll angle in degree.
@param[out] accuracy Accuracy of orientation sensor.
*/
int16 AKFS_Get_ORIENTATION(
void *mem,
AKFLOAT *azimuth,
AKFLOAT *pitch,
AKFLOAT *roll,
int16 *accuracy
)
{
int16 akret;
AKMPRMS *prms;
#ifdef AKM_VALUE_CHECK
if (mem == NULL) {
AKMDEBUG(AKMDATA_CHECK, "%s: Invalid mem pointer.", __FUNCTION__);
return AKM_ERROR;
}
if (pitch== NULL || pitch == NULL || roll == NULL || accuracy == NULL) {
AKMDEBUG(AKMDATA_CHECK, "%s: Invalid data pointer.", __FUNCTION__);
return AKM_ERROR;
}
#endif
/* Copy pointer */
prms = (AKMPRMS *)mem;
/* Azimuth calculation */
/* hvbuf[in] : Android coordinate, sensitivity adjusted, */
/* offset subtracted. */
/* avbuf[in] : Android coordinate, sensitivity adjusted, */
/* offset subtracted. */
/* azimuth[out]: Android coordinate and unit (degree). */
/* pitch [out]: Android coordinate and unit (degree). */
/* roll [out]: Android coordinate and unit (degree). */
akret = AKFS_Direction(
AKFS_HDATA_SIZE,
prms->fva_hvbuf,
CSPEC_HNAVE_D,
AKFS_ADATA_SIZE,
prms->fva_avbuf,
CSPEC_ANAVE_D,
&prms->f_azimuth,
&prms->f_pitch,
&prms->f_roll
);
if (akret == AKFS_ERROR) {
AKMERROR;
return AKM_ERROR;
}
/* Success */
*azimuth = prms->f_azimuth;
*pitch = prms->f_pitch;
*roll = prms->f_roll;
*accuracy = 3;
/* Debug output */
AKMDEBUG(AKMDATA_ORI, "Ori(?):%8.2f, %8.2f, %8.2f\n",
*azimuth, *pitch, *roll);
return AKM_SUCCESS;
}