DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: LOCALIZE.cpp
- Revision:
- 0:96d6eb224379
- Child:
- 1:8966a48ce8d5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LOCALIZE.cpp Tue Mar 01 18:20:45 2016 +0000 @@ -0,0 +1,51 @@ +#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; +}