GSensor的角度计算

   目前负责关于GSensor的模式识别，这里贴出自己的研究成果，供自己备忘。
ACCEL_DIRECTION CGSensorModule::CalDirection(smb380acc_t& a_xyz,const unsigned char nAccelRange, double* pdbAngle)
{
    ACCEL_DIRECTION adNew = AD_UNKNOWN;
    double dbAngle = 0;
    // expressions: tan(angle) = a_xyz.x / a_xyz.y
    // angle is start from AD_LAYRIGHT

    // notice:    1. a_xyz.x and a_xyz.y should not be small together, so we need to give a liminal value
    //            2. the a_xyz.z should be enough small to ensure the G-Sensor is vertical,
    //                the degree is 45, means abs(a_xyz.z) should not larger than nAccelRange*sin(45)
    if((abs(a_xyz.x) < nAccelRange/10) 
        && (abs(a_xyz.y) < nAccelRange/10)
        || abs(a_xyz.z) > (nAccelRange*0.707))
    {
        dbAngle = nAccelRange;
        return AD_UNKNOWN;
    }

    float fTanValue = float(a_xyz.y)/float(a_xyz.x);
    dbAngle = atan(fTanValue);
    // AD_LAYRIGHT or AD_LAYLEFT
    if(fTanValue>-1 && fTanValue<1)
    {
        if(a_xyz.x>0)
        {
            adNew = AD_LAYRIGHT;
        }
        else
        {
            dbAngle += PI;
            adNew = AD_LAYLEFT;
        }
    }
    else
    {
        if(a_xyz.y>0)
        {
            if (dbAngle<0)
                dbAngle += PI;
            adNew = AD_LAYUP;
        }
        else
        {
            if (dbAngle>0)
                dbAngle -= PI;
            adNew = AD_LAYDOWN;
        }
    }

    if(dbAngle<0)
        dbAngle+=(PI*2);

    if(pdbAngle != NULL)
        *pdbAngle = dbAngle;
    return adNew;
}

BOOL CGSensorModule::GetDirection(ACCEL_DIRECTION& adNew, double* pdbAngle)
{
    CHECK_GSENSOR_HANDLE();
    smb380acc_t a_xyz;
    unsigned char nRange;
    if(!GetAccelXYZT(a_xyz))
    {
        return FALSE;
    }
    if(!GetRange(nRange))
    {
        return FALSE;
    }
    adNew = CalDirection(a_xyz,MAX_ACCELRANGE/(2<<nRange),pdbAngle);
    return TRUE;
}

posted on 2008-05-26 11:37 追梦时代 阅读(193) 评论(0)  编辑 收藏 引用


标题  
姓名  
主页
验证码 *
内容(提交失败后,可以通过“恢复上次提交”恢复刚刚提交的内容)  
  登录  使用高级评论  新用户注册  返回页首  恢复上次提交      
[使用Ctrl+Enter键可以直接提交]
相关链接:
网站导航:
<2008年5月>
日一二三四五六
27282930123
45678910
11121314151617
18192021222324
25262728293031
1234567

导航

统计

常用链接

留言簿(3)

随笔档案

文章档案

搜索

积分与排名

最新随笔

最新评论

阅读排行榜

评论排行榜