Localization
Dependencies: BNO055_fusion mbed
LOCALIZE.cpp@0:96d6eb224379, 2016-03-01 (annotated)
- Committer:
- 12104404
- Date:
- Tue Mar 01 18:20:45 2016 +0000
- Revision:
- 0:96d6eb224379
- Child:
- 1:8966a48ce8d5
ANGLE;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
12104404 | 0:96d6eb224379 | 1 | #include "LOCALIZE.h" |
12104404 | 0:96d6eb224379 | 2 | |
12104404 | 0:96d6eb224379 | 3 | LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset): |
12104404 | 0:96d6eb224379 | 4 | _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu_reset(imu_reset), _imu(x_imu_i2c, imu_reset) |
12104404 | 0:96d6eb224379 | 5 | { |
12104404 | 0:96d6eb224379 | 6 | |
12104404 | 0:96d6eb224379 | 7 | } |
12104404 | 0:96d6eb224379 | 8 | |
12104404 | 0:96d6eb224379 | 9 | int LOCALIZE::init (void) |
12104404 | 0:96d6eb224379 | 10 | { |
12104404 | 0:96d6eb224379 | 11 | if(_imu.chip_ready()==0) |
12104404 | 0:96d6eb224379 | 12 | return ERROR_IMU; |
12104404 | 0:96d6eb224379 | 13 | _imu.set_mounting_position(MT_P0); |
12104404 | 0:96d6eb224379 | 14 | _imu.change_fusion_mode(MODE_IMU); |
12104404 | 0:96d6eb224379 | 15 | return 0; |
12104404 | 0:96d6eb224379 | 16 | } |
12104404 | 0:96d6eb224379 | 17 | |
12104404 | 0:96d6eb224379 | 18 | void LOCALIZE::get_angle(LOCALIZE_xya *xya) |
12104404 | 0:96d6eb224379 | 19 | { |
12104404 | 0:96d6eb224379 | 20 | _imu.get_Euler_Angles(&euler); |
12104404 | 0:96d6eb224379 | 21 | _imu.get_gravity(&gravity); |
12104404 | 0:96d6eb224379 | 22 | int angle=0; |
12104404 | 0:96d6eb224379 | 23 | unsigned int quad=0; |
12104404 | 0:96d6eb224379 | 24 | if(gravity.y>=0) |
12104404 | 0:96d6eb224379 | 25 | quad&=~1; |
12104404 | 0:96d6eb224379 | 26 | else |
12104404 | 0:96d6eb224379 | 27 | quad|=1; |
12104404 | 0:96d6eb224379 | 28 | if(gravity.x>=0) |
12104404 | 0:96d6eb224379 | 29 | quad&=~(1<<1); |
12104404 | 0:96d6eb224379 | 30 | else |
12104404 | 0:96d6eb224379 | 31 | quad|=(1<<1); |
12104404 | 0:96d6eb224379 | 32 | quad&=3; |
12104404 | 0:96d6eb224379 | 33 | switch(quad) { |
12104404 | 0:96d6eb224379 | 34 | case 0: |
12104404 | 0:96d6eb224379 | 35 | angle=euler.p; |
12104404 | 0:96d6eb224379 | 36 | break; |
12104404 | 0:96d6eb224379 | 37 | case 1: |
12104404 | 0:96d6eb224379 | 38 | angle=180-euler.p; |
12104404 | 0:96d6eb224379 | 39 | break; |
12104404 | 0:96d6eb224379 | 40 | case 2: |
12104404 | 0:96d6eb224379 | 41 | angle=360+euler.p; |
12104404 | 0:96d6eb224379 | 42 | break; |
12104404 | 0:96d6eb224379 | 43 | case 3: |
12104404 | 0:96d6eb224379 | 44 | angle=180+abs(euler.p); |
12104404 | 0:96d6eb224379 | 45 | break; |
12104404 | 0:96d6eb224379 | 46 | default: |
12104404 | 0:96d6eb224379 | 47 | angle=-1; |
12104404 | 0:96d6eb224379 | 48 | break; |
12104404 | 0:96d6eb224379 | 49 | } |
12104404 | 0:96d6eb224379 | 50 | xya->a=angle; |
12104404 | 0:96d6eb224379 | 51 | } |