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;
}

--------------------next---------------------