Localization
Dependencies: BNO055_fusion mbed
LOCALIZE.cpp@3:c58eb4be51de, 2016-03-03 (annotated)
- Committer:
- 12104404
- Date:
- Thu Mar 03 01:00:32 2016 +0000
- Revision:
- 3:c58eb4be51de
- Parent:
- 2:634c1adf89b2
- Child:
- 4:d70375cfa533
cleaned up localization;
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 | 1:8966a48ce8d5 | 4 | _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _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 | 1:8966a48ce8d5 | 15 | char data[]= {0,0,0}; |
12104404 | 1:8966a48ce8d5 | 16 | if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0) |
12104404 | 1:8966a48ce8d5 | 17 | return ERROR_RX_P; |
12104404 | 2:634c1adf89b2 | 18 | if(_x_imu_i2c.read(R_N_ADDR<<1, data, 3)!=0) |
12104404 | 2:634c1adf89b2 | 19 | return ERROR_RX_N; |
12104404 | 1:8966a48ce8d5 | 20 | if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0) |
12104404 | 1:8966a48ce8d5 | 21 | return ERROR_RY_P; |
12104404 | 2:634c1adf89b2 | 22 | if(_y_i2c.read(R_N_ADDR<<1, data, 3)!=0) |
12104404 | 2:634c1adf89b2 | 23 | return ERROR_RY_N; |
12104404 | 0:96d6eb224379 | 24 | return 0; |
12104404 | 0:96d6eb224379 | 25 | } |
12104404 | 0:96d6eb224379 | 26 | |
12104404 | 0:96d6eb224379 | 27 | void LOCALIZE::get_angle(LOCALIZE_xya *xya) |
12104404 | 0:96d6eb224379 | 28 | { |
12104404 | 1:8966a48ce8d5 | 29 | _imu.get_Euler_Angles(&_euler); |
12104404 | 1:8966a48ce8d5 | 30 | _imu.get_gravity(&_gravity); |
12104404 | 1:8966a48ce8d5 | 31 | int _angle=-1; |
12104404 | 0:96d6eb224379 | 32 | unsigned int quad=0; |
12104404 | 1:8966a48ce8d5 | 33 | if(_gravity.y>=0) |
12104404 | 0:96d6eb224379 | 34 | quad&=~1; |
12104404 | 0:96d6eb224379 | 35 | else |
12104404 | 0:96d6eb224379 | 36 | quad|=1; |
12104404 | 1:8966a48ce8d5 | 37 | if(_gravity.x>=0) |
12104404 | 0:96d6eb224379 | 38 | quad&=~(1<<1); |
12104404 | 0:96d6eb224379 | 39 | else |
12104404 | 0:96d6eb224379 | 40 | quad|=(1<<1); |
12104404 | 0:96d6eb224379 | 41 | quad&=3; |
12104404 | 0:96d6eb224379 | 42 | switch(quad) { |
12104404 | 0:96d6eb224379 | 43 | case 0: |
12104404 | 1:8966a48ce8d5 | 44 | _angle=_euler.p; |
12104404 | 0:96d6eb224379 | 45 | break; |
12104404 | 0:96d6eb224379 | 46 | case 1: |
12104404 | 1:8966a48ce8d5 | 47 | _angle=180-_euler.p; |
12104404 | 0:96d6eb224379 | 48 | break; |
12104404 | 0:96d6eb224379 | 49 | case 2: |
12104404 | 1:8966a48ce8d5 | 50 | _angle=360+_euler.p; |
12104404 | 0:96d6eb224379 | 51 | break; |
12104404 | 0:96d6eb224379 | 52 | case 3: |
12104404 | 1:8966a48ce8d5 | 53 | _angle=180+abs(_euler.p); |
12104404 | 0:96d6eb224379 | 54 | break; |
12104404 | 0:96d6eb224379 | 55 | default: |
12104404 | 1:8966a48ce8d5 | 56 | _angle=-1; |
12104404 | 0:96d6eb224379 | 57 | break; |
12104404 | 0:96d6eb224379 | 58 | } |
12104404 | 1:8966a48ce8d5 | 59 | _xya.a=_angle; |
12104404 | 1:8966a48ce8d5 | 60 | xya->a=_xya.a; |
12104404 | 0:96d6eb224379 | 61 | } |
12104404 | 1:8966a48ce8d5 | 62 | |
12104404 | 1:8966a48ce8d5 | 63 | void LOCALIZE::get_xy(LOCALIZE_xya *xya) |
12104404 | 1:8966a48ce8d5 | 64 | { |
12104404 | 3:c58eb4be51de | 65 | int _rx, _ry; |
12104404 | 3:c58eb4be51de | 66 | get_raw_xy(); |
12104404 | 1:8966a48ce8d5 | 67 | get_angle(xya); |
12104404 | 1:8966a48ce8d5 | 68 | if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) { |
12104404 | 2:634c1adf89b2 | 69 | _rx=_rx_n; |
12104404 | 1:8966a48ce8d5 | 70 | _ry=_ry_p; |
12104404 | 1:8966a48ce8d5 | 71 | } else if(abs(_xya.a-90)<R_ERROR) { |
12104404 | 1:8966a48ce8d5 | 72 | _rx=_ry_p; |
12104404 | 1:8966a48ce8d5 | 73 | _ry=FRAME_H-RX_OFF-_rx_p; |
12104404 | 1:8966a48ce8d5 | 74 | } else if(abs(_xya.a-180)<R_ERROR) { |
12104404 | 1:8966a48ce8d5 | 75 | _rx=FRAME_W-RX_OFF-_rx_p; |
12104404 | 1:8966a48ce8d5 | 76 | _ry=FRAME_H-RY_OFF-_ry_p; |
12104404 | 1:8966a48ce8d5 | 77 | } else if(abs(_xya.a-270)<R_ERROR) { |
12104404 | 1:8966a48ce8d5 | 78 | _rx=FRAME_W-RY_OFF-_ry_p; |
12104404 | 1:8966a48ce8d5 | 79 | _ry=_rx_p; |
12104404 | 1:8966a48ce8d5 | 80 | } else { |
12104404 | 1:8966a48ce8d5 | 81 | _rx=_rx_p; |
12104404 | 1:8966a48ce8d5 | 82 | _ry=_ry_p; |
12104404 | 1:8966a48ce8d5 | 83 | } |
12104404 | 1:8966a48ce8d5 | 84 | _xya.x=_rx; |
12104404 | 1:8966a48ce8d5 | 85 | _xya.y=_ry; |
12104404 | 1:8966a48ce8d5 | 86 | xya->x=_xya.x; |
12104404 | 1:8966a48ce8d5 | 87 | xya->y=_xya.y; |
12104404 | 1:8966a48ce8d5 | 88 | } |
12104404 | 1:8966a48ce8d5 | 89 | |
12104404 | 3:c58eb4be51de | 90 | void LOCALIZE::get_raw_xy(void) |
12104404 | 3:c58eb4be51de | 91 | { |
12104404 | 3:c58eb4be51de | 92 | char data[]= {0,0,0}; |
12104404 | 3:c58eb4be51de | 93 | _x_imu_i2c.read(R_P_ADDR<<1, data, 3); |
12104404 | 3:c58eb4be51de | 94 | wait(0.1); |
12104404 | 3:c58eb4be51de | 95 | _rx_p=data[1]; |
12104404 | 3:c58eb4be51de | 96 | _x_imu_i2c.read(R_N_ADDR<<1, data, 3); |
12104404 | 3:c58eb4be51de | 97 | wait(0.1); |
12104404 | 3:c58eb4be51de | 98 | _rx_n=data[1]; |
12104404 | 3:c58eb4be51de | 99 | _y_i2c.read(R_P_ADDR<<1, data, 3); |
12104404 | 3:c58eb4be51de | 100 | wait(0.1); |
12104404 | 3:c58eb4be51de | 101 | _ry_p=data[1]; |
12104404 | 3:c58eb4be51de | 102 | _y_i2c.read(R_N_ADDR<<1, data, 3); |
12104404 | 3:c58eb4be51de | 103 | wait(0.1); |
12104404 | 3:c58eb4be51de | 104 | _ry_n=data[1]; |
12104404 | 3:c58eb4be51de | 105 | } |