test program for using sensor R1307
r1307.cpp@3:5655017c6806, 2017-12-01 (annotated)
- Committer:
- WAT34
- Date:
- Fri Dec 01 18:18:17 2017 +0900
- Revision:
- 3:5655017c6806
- Parent:
- 1:59efa65938c7
- Child:
- 4:b6583f28f7b7
checksum added
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 1:59efa65938c7 | 1 | #include "r1307.h" |
WAT34 | 1:59efa65938c7 | 2 | |
WAT34 | 1:59efa65938c7 | 3 | |
WAT34 | 1:59efa65938c7 | 4 | R1307::R1307(PinName tx_,PinName rx_): serial(tx_,rx_) |
WAT34 | 1:59efa65938c7 | 5 | { |
WAT34 | 3:5655017c6806 | 6 | //ticker.attach(this,&R1307::update,0.01); |
WAT34 | 1:59efa65938c7 | 7 | serial.baud(115200); |
WAT34 | 1:59efa65938c7 | 8 | } |
WAT34 | 1:59efa65938c7 | 9 | |
WAT34 | 1:59efa65938c7 | 10 | |
WAT34 | 1:59efa65938c7 | 11 | void R1307::update() |
WAT34 | 1:59efa65938c7 | 12 | { |
WAT34 | 1:59efa65938c7 | 13 | uint8_t index; |
WAT34 | 1:59efa65938c7 | 14 | int16_t angle; |
WAT34 | 1:59efa65938c7 | 15 | int16_t rate; |
WAT34 | 1:59efa65938c7 | 16 | int16_t x_acc; |
WAT34 | 1:59efa65938c7 | 17 | int16_t y_acc; |
WAT34 | 1:59efa65938c7 | 18 | int16_t z_acc; |
WAT34 | 3:5655017c6806 | 19 | uint8_t check_sum,recievedData[13]; |
WAT34 | 1:59efa65938c7 | 20 | //Verify packet heading information |
WAT34 | 1:59efa65938c7 | 21 | while(serial.getc() != 0xAA){ |
WAT34 | 1:59efa65938c7 | 22 | if(!serial.readable()) |
WAT34 | 1:59efa65938c7 | 23 | return; |
WAT34 | 1:59efa65938c7 | 24 | } |
WAT34 | 1:59efa65938c7 | 25 | if(serial.getc() != 0x00) |
WAT34 | 1:59efa65938c7 | 26 | return; |
WAT34 | 1:59efa65938c7 | 27 | //Assemble data |
WAT34 | 3:5655017c6806 | 28 | for(int i = 0;i < 13;i++) |
WAT34 | 3:5655017c6806 | 29 | { |
WAT34 | 3:5655017c6806 | 30 | recievedData[i] = serial.getc(); |
WAT34 | 3:5655017c6806 | 31 | } |
WAT34 | 3:5655017c6806 | 32 | for(int i = 0;i < 12;i++) |
WAT34 | 3:5655017c6806 | 33 | { |
WAT34 | 3:5655017c6806 | 34 | check_sum += recievedData[i]; |
WAT34 | 3:5655017c6806 | 35 | } |
WAT34 | 3:5655017c6806 | 36 | if (recievedData[12] != check_sum) return; |
WAT34 | 1:59efa65938c7 | 37 | |
WAT34 | 3:5655017c6806 | 38 | index = recievedData[0]; |
WAT34 | 3:5655017c6806 | 39 | rate = (recievedData[1] & 0xFF) | ((recievedData[2] << 8) & 0xFF00); |
WAT34 | 3:5655017c6806 | 40 | angle = (recievedData[3] & 0xFF) | ((recievedData[4]<< 8) & 0XFF00); |
WAT34 | 3:5655017c6806 | 41 | x_acc = (recievedData[5] & 0xFF) | ((recievedData[6]<< 8) & 0xFF00); |
WAT34 | 3:5655017c6806 | 42 | y_acc = (recievedData[7] & 0xFF) | ((recievedData[8] << 8) & 0XFF00); |
WAT34 | 3:5655017c6806 | 43 | z_acc = (recievedData[9]& 0xFF) | ((recievedData[10] << 8) & 0xFF00); |
WAT34 | 1:59efa65938c7 | 44 | //Scale and store data |
WAT34 | 1:59efa65938c7 | 45 | gRate = rate / 100.0; |
WAT34 | 1:59efa65938c7 | 46 | gAngle = angle / 100.0; |
WAT34 | 1:59efa65938c7 | 47 | gX_acc = x_acc; |
WAT34 | 1:59efa65938c7 | 48 | gY_acc = y_acc; |
WAT34 | 1:59efa65938c7 | 49 | gZ_acc = z_acc; |
WAT34 | 1:59efa65938c7 | 50 | return; |
WAT34 | 1:59efa65938c7 | 51 | } |