File content as of revision 0:96d6eb224379:

#include "LOCALIZE.h"

LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset):
    _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu_reset(imu_reset), _imu(x_imu_i2c, imu_reset)
{

}

int LOCALIZE::init (void)
{
    if(_imu.chip_ready()==0)
        return ERROR_IMU;
    _imu.set_mounting_position(MT_P0);
    _imu.change_fusion_mode(MODE_IMU);
    return 0;
}

void LOCALIZE::get_angle(LOCALIZE_xya *xya)
{
    _imu.get_Euler_Angles(&euler);
    _imu.get_gravity(&gravity);
    int angle=0;
    unsigned int quad=0;
    if(gravity.y>=0)
        quad&=~1;
    else
        quad|=1;
    if(gravity.x>=0)
        quad&=~(1<<1);
    else
        quad|=(1<<1);
    quad&=3;
    switch(quad) {
        case 0:
            angle=euler.p;
            break;
        case 1:
            angle=180-euler.p;
            break;
        case 2:
            angle=360+euler.p;
            break;
        case 3:
            angle=180+abs(euler.p);
            break;
        default:
            angle=-1;
            break;
    }
    xya->a=angle;
}