version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
LOCALIZE.cpp
- Committer:
- 12104404
- Date:
- 2016-03-01
- Revision:
- 0:96d6eb224379
- Child:
- 1:8966a48ce8d5
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; }