/*!
Output measurement result. If this application is run as ConsoleMode,
the measurement result is output to console. If this application is run as
DaemonMode, the measurement result is written to device driver.
@param[in] prms pointer to #AK8975PRMS structure.
*/
void Disp_MeasurementResultHook(AK8975PRMS * prms)
{
/* 若 "不是" console mode, 则... */
if (!s_opmode) {
short rbuf[12] = { 0 };
rbuf[0] = prms->m_theta; // yaw
rbuf[1] = prms->m_phi180; // pitch
rbuf[2] = prms->m_eta90; // roll
rbuf[3] = 25; // tmp (AK8975 doesn't have temperature sensor)
rbuf[4] = prms->m_hdst; // m_stat
rbuf[5] = 3; // g_stat
rbuf[6] = prms->m_avec.u.x; // G_Sensor x
rbuf[7] = prms->m_avec.u.y; // G_Sensor y
rbuf[8] = prms->m_avec.u.z; // G_Sensor z
rbuf[9] = prms->m_hvec.u.x; // M_x
rbuf[10] = prms->m_hvec.u.y; // M_y
rbuf[11] = prms->m_hvec.u.z; // M_z
D_WHEN_REPEAT(100, "yaw = %d, pitch = %d, roll = %d; M_x = %d, M_y = %d, M_z = %d.", rbuf[0], rbuf[1], rbuf[2], rbuf[9], rbuf[10], rbuf[11] );
/* .! : 将计算得到的结果回写到驱动. */
ioctl(g_file, ECS_IOCTL_SET_YPR, &rbuf); // 之后, 驱动会将该数据上报 sensor HAL.
}
/* 否则, ... */
else {
Disp_MeasurementResult(prms);
}
}
/**
* Acquire acceleration data from acceleration sensor and convert it to Android coordinate system.
* .! : 目前设计为 尽量不阻塞的形式.
* @param[out] fData
* A acceleration data array.
* The coordinate system of the acquired data follows the definition of Android.
* @return
* If this function succeeds, the return value is #AKD_SUCCESS.
* Otherwise the return value is #AKD_FAIL.
*
*/
int16_t Acc_GetAccelerationData(float fData[3])
{
int16_t result = AKD_SUCCESS;
struct itimerspec ts; /* 将用于定义超时时间. */
struct sensor_axis accData = {0, 0, 0};
/* 若尚未使能 acc, 则... */
if ( !sHasEnabledAcc )
{
/* 使能 acc. */
if ( 0 > ioctl(sAccFd, GSENSOR_IOCTL_START) )
{
E("failed to START acc device; error is '%s'.", strerror(errno));
result = AKD_FAIL;
goto EXIT;
}
/* 设置采样率. */
int sample_rate = MMA8452_RATE_12P5; // .! : 暂时先使用 12.5 Hz.
if ( 0 > ioctl(sAccFd, GSENSOR_IOCTL_APP_SET_RATE, &sample_rate) )
{
E("failed to set sample rete of acc device; error is '%s'.", strerror(errno));
result = AKD_FAIL;
goto EXIT;
}
/* 置位标识. */
sHasEnabledAcc = TRUE;
}
/* 获取 acc sensor 数据. */ // .! : 尽量不阻塞.
if ( 0 > ioctl(sAccFd, GSENSOR_IOCTL_GETDATA, &accData) )
{
E("failed to GET acc data, error is '%s.'", strerror(errno));
result = AKD_FAIL;
goto EXIT;
}
/* 重置 sDisableAccTimer. */
// D("to reset 'sDisableAccTimer'.");
memset(&ts, 0, sizeof(ts) );
ts.it_value.tv_sec = ENABLE_TIME_OUT;
if ( timer_settime(sDisableAccTimer, 0, &ts, NULL) < 0 )
{
E("failed start 'sDisableAccTimer'; error is '%s'.", strerror(errno));
result = AKD_FAIL;
goto EXIT;
}
/* 转化为 Android 定义的格式, 待返回. */ // .! : 和 HAL 的 MmaSensor.cpp 中一致, 使用 默认横屏坐标定义 g sensor 数据.
fData[0] = ( (accData.x) * ACCELERATION_RATIO_ANDROID_TO_HW);
fData[1] = ( (accData.y) * ACCELERATION_RATIO_ANDROID_TO_HW);
fData[2] = ( (accData.z) * ACCELERATION_RATIO_ANDROID_TO_HW);
D_WHEN_REPEAT(100, "got acc sensor data : x = %f, y = %f, z = %f.", fData[0], fData[1], fData[2] );
EXIT:
return result;
}
/*!
SmartCompass main calculation routine. This function will be processed
when INT pin event is occurred.
@retval AKRET_
@param[in] bData An array of register values which holds TMPS, H1X, H1Y
and H1Z value respectively.
@param[in,out] prms A pointer to a #AK8975PRMS structure.
@param[in] curForm 当前设备的 formation 状态标识.
@param[in] hDecimator
@param[in] cntSuspend
// @param[in] pre_form The index of hardware position which represents the
// index when this function is called at the last time.
*/
int16 MeasuringEventProcess(
const int16 bData[],
AK8975PRMS* prms,
const int16 curForm,
const int16 hDecimator,
const int16 cntSuspend
)
{
int16vec have;
int16 dor, derr, hofl;
int16 isOF;
int16 aksc_ret;
int16 hdSucc;
int16 preThe;
dor = 0;
derr = 0;
hofl = 0;
//DBGPRINT(DBG_LEVEL2, "%s:prms->m_form1111=%d\n",
// __FUNCTION__,prms->m_form );
// Decompose one block data into each Magnetic sensor's data
aksc_ret = AKSC_Decomp8975(
bData, // 输入 raw 的 mag 数据.
prms->m_hnave, // 指定 参与平均计算的 mag 数据(向量) 的个数.
&prms->m_asa, // 输入 sensor 的 敏感度调整参数, 该参数的源头是 AK8975 的 fuse rom.
prms->m_hdata, // 输入/输出 : raw mag 数据的 AKSC 形态. 数组实例的元素个数是 AKSC_HDATA_SIZE.
&prms->m_hn, // 返回的 mag 数据的个数. 对 AK8975 将是 1.
&have, // 返回平均计算之后得到的 mag 数据.
&dor, // 返回是否发生 "data overrun".
&derr, // 返回是否发生 "data error".
&hofl // 返回是否发生 "data overflow".
);
if (aksc_ret == 0) {
DBGPRINT(DBG_LEVEL1,
"AKSC_Decomp8975 failed. asa(dec)=%d,%d,%d hn=%d\n",
prms->m_asa.u.x, prms->m_asa.u.y, prms->m_asa.u.z, prms->m_hn);
return AKRET_PROC_FAIL;
}
// Check the formation change
if(prms->m_form != curForm){
// Set counter
prms->m_cntSuspend = cntSuspend;
prms->m_form = curForm;
return AKRET_FORMATION_CHANGED;
}
if(derr == 1){
return AKRET_DATA_READERROR;
}
if(prms->m_cntSuspend > 0){
prms->m_cntSuspend--;
}
else {
// Detect a fluctuation of magnetic field.
isOF = AKSC_HFlucCheck(&(prms->m_hflucv), &(prms->m_hdata[0]));
if(hofl == 1){
// Set a HDOE level as "HDST_UNSOLVED"
AKSC_SetHDOELevel(
&prms->m_hdoev,
&prms->m_ho,
AKSC_HDST_UNSOLVED,
1
);
prms->m_hdst = AKSC_HDST_UNSOLVED;
return AKRET_DATA_OVERFLOW;
}
else if(isOF == 1){
// Set a HDOE level as "HDST_UNSOLVED"
AKSC_SetHDOELevel(
&prms->m_hdoev,
&prms->m_ho,
AKSC_HDST_UNSOLVED,
1
);
prms->m_hdst = AKSC_HDST_UNSOLVED;
return AKRET_HFLUC_OCCURRED;
}
else {
prms->m_callcnt--;
if(prms->m_callcnt <= 0){
//Calculate Magnetic sensor's offset by DOE .Q : DOE ?
hdSucc = AKSC_HDOEProcessS3(
prms->m_licenser,
prms->m_licensee,
prms->m_key,
&prms->m_hdoev,
prms->m_hdata,
prms->m_hn,
&prms->m_ho,
&prms->m_hdst
);
if(hdSucc > 0){
prms->HSUC_HO[prms->m_form] = prms->m_ho;
prms->HSUC_HDST[prms->m_form] = prms->m_hdst;
prms->HFLUCV_HREF[prms->m_form] = prms->m_hflucv.href;
}
prms->m_callcnt = hDecimator;
}
}
}
// Subtract offset and normalize magnetic field vector.
aksc_ret = AKSC_VNorm(
&have,
&prms->m_ho,
&prms->m_hs,
AKSC_HSENSE_TARGET,
&prms->m_hvec
);
if (aksc_ret == 0) {
DBGPRINT(DBG_LEVEL1, "AKSC_VNorm failed.\n");
return AKRET_PROC_FAIL;
}
preThe = prms->m_theta;
D_WHEN_REPEAT(100, "befor calling AKSC_DirectionS3() : \n"
"\t prms->m_form = %d", prms->m_form);
prms->m_ds3Ret = AKSC_DirectionS3(
prms->m_licenser,
prms->m_licensee,
prms->m_key,
&prms->m_hvec, /* 输入的 mag 数据矢量. */
&prms->m_avec, /* 输入的 acc 数据矢量. */
&prms->m_dvec,
&prms->m_hlayout[prms->m_form],
&prms->m_alayout[prms->m_form],
&prms->m_theta,
&prms->m_delta,
&prms->m_hr,
&prms->m_hrhoriz,
&prms->m_ar,
&prms->m_phi180,
&prms->m_phi90,
&prms->m_eta180,
&prms->m_eta90,
&prms->m_mat
);
prms->m_theta = AKSC_ThetaFilter(
prms->m_theta,
preThe,
THETAFILTER_SCALE
);
if(prms->m_ds3Ret != 3) {
/*
DBGPRINT(DBG_LEVEL2, "AKSC_Direction6D failed (0x%02x). \n",
prms->m_ds3Ret);
*/
DBGPRINT(DBG_LEVEL2, "AKSC_Direction3S failed (0x%x). \n",
(unsigned short)(prms->m_ds3Ret) );
DBGPRINT(DBG_LEVEL2, "hvec=%d,%d,%d avec=%d,%d,%d dvec=%d,%d,%d\n",
prms->m_hvec.u.x, prms->m_hvec.u.y, prms->m_hvec.u.z,
prms->m_avec.u.x, prms->m_avec.u.y, prms->m_avec.u.z,
prms->m_dvec.u.x, prms->m_dvec.u.y, prms->m_dvec.u.z);
}
return AKRET_PROC_SUCCEED;
}
/*!
This is the main routine of measurement.
@param[in,out] prms A pointer to a #AK8975PRMS structure.
*/
void MeasureSNGLoop(AK8975PRMS* prms)
{
BYTE i2cData[AKSC_BDATA_SIZE];
int16 i;
int16 bData[AKSC_BDATA_SIZE]; // Measuring block data
int16 ret;
int32 ch;
int32 doze;
int32_t delay;
AKMD_INTERVAL interval;
struct timespec tsstart, tsend;
if (openKey() < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
if (openFormation() < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Get initial interval
GetValidInterval(CSPEC_INTERVAL_SNG, &interval);
// Initialize
if(InitAK8975_Measure(prms) != AKD_SUCCESS){
return;
}
while(TRUE){
// Get start time
if (clock_gettime(CLOCK_REALTIME, &tsstart) < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Set to SNG measurement pattern (Set CNTL register)
if (AKD_SetMode(AK8975_MODE_SNG_MEASURE) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// .! : 获取 M snesor 的原始数据. 这里可能阻塞.
// Get measurement data from AK8975
// ST1 + (HXL + HXH) + (HYL + HYH) + (HZL + HZH) + ST2
// = 1 + (1 + 1) + (1 + 1) + (1 + 1) + 1 = 8 bytes
if (AKD_GetMagneticData(i2cData) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// Copy to local variable
// DBGPRINT(DBG_LEVEL3, "%s: bData(Hex)=", __FUNCTION__);
for(i=0; i<AKSC_BDATA_SIZE; i++){
bData[i] = i2cData[i];
// DBGPRINT(DBG_LEVEL3, "%02x,", bData[i]);
}
// DBGPRINT(DBG_LEVEL3, "\n");
D_WHEN_REPEAT(100,
"raw mag x : %d, raw mag y : %d, raw mag z : %d.",
(signed short)(bData[1] + (bData[2] << 8) ),
(signed short)(bData[3] + (bData[4] << 8) ),
(signed short)(bData[5] + (bData[6] << 8) ) );
// .! :
// Get acceelration sensor's measurement data.
if (GetAccVec(prms) != AKRET_PROC_SUCCEED) {
return;
}
/*
DBGPRINT(DBG_LEVEL3,
"%s: acc(Hex)=%02x,%02x,%02x\n", __FUNCTION__,
prms->m_avec.u.x, prms->m_avec.u.y, prms->m_avec.u.z);
*/
ret = MeasuringEventProcess(
bData,
prms,
getFormation(),
interval.decimator,
CSPEC_CNTSUSPEND_SNG
);
// Check the return value
if(ret == AKRET_PROC_SUCCEED){
if(prms->m_cntSuspend > 0){
// Show message
DBGPRINT(DBG_LEVEL2,
"Suspend cycle count = %d\n", prms->m_cntSuspend);
}
else if (prms->m_callcnt <= 1){
// Check interval
if (AKD_GetDelay(&delay) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
} else {
GetValidInterval(delay, &interval);
}
}
// Display(or dispatch) the result.
Disp_MeasurementResultHook(prms);
}
else if(ret == AKRET_FORMATION_CHANGED){
// Switch formation.
SwitchFormation(prms);
}
else if(ret == AKRET_DATA_READERROR){
DBGPRINT(DBG_LEVEL2,
"Data read error occurred.\n\n");
}
else if(ret == AKRET_DATA_OVERFLOW){
DBGPRINT(DBG_LEVEL2,
"Data overflow occurred.\n\n");
}
else if(ret == AKRET_HFLUC_OCCURRED){
DBGPRINT(DBG_LEVEL2,
"AKSC_HFlucCheck did not return 1.\n\n");
}
else{
// Does not reach here
LOGE("MeasuringEventProcess has failed.\n");
break;
}
// Check user order
ch = checkKey();
if (ch == AKKEY_STOP_MEASURE) {
break;
}
else if(ch < 0){
LOGD("Bad key code.\n");
break;
}
// Get end time
if (clock_gettime(CLOCK_REALTIME, &tsend) < 0) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
return;
}
// calculate wait time
doze = interval.interval - ((tsend.tv_sec - tsstart.tv_sec)*1000000 +
(tsend.tv_nsec - tsstart.tv_nsec)/1000);
if (doze < 0) {
doze = 0;
}
// Adjust sampling frequency
// DBGPRINT(DBG_LEVEL3, "Sleep %d usec.\n", doze);
usleep(doze);
}
// Set to PowerDown mode
if (AKD_SetMode(AK8975_MODE_POWERDOWN) != AKD_SUCCESS) {
DBGPRINT(DBG_LEVEL1,
"%s:%d Error.\n", __FUNCTION__, __LINE__);
}
closeFormation();
closeKey();
}
