Read IMU data at Serial port (p28, p27), data rate <100Hz @115200bps Read 5 channels PPM signal at p11 using InterruptIn Send data via RF Modem at serial port (p13, p14) in 36Hz @115200 Drive i2C motor speed controller at I2C port (p9, p10) in every 10mS (control loop delay time)
ahrs.h@0:7ccd56e1c3b1, 2010-08-11 (annotated)
- Committer:
- agiembed
- Date:
- Wed Aug 11 01:23:42 2010 +0000
- Revision:
- 0:7ccd56e1c3b1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
agiembed | 0:7ccd56e1c3b1 | 1 | #define Ahrs_num 40 |
agiembed | 0:7ccd56e1c3b1 | 2 | #define start 0 |
agiembed | 0:7ccd56e1c3b1 | 3 | #define head1 0xFF |
agiembed | 0:7ccd56e1c3b1 | 4 | #define head2 0xFF |
agiembed | 0:7ccd56e1c3b1 | 5 | #define check1 1 |
agiembed | 0:7ccd56e1c3b1 | 6 | #define check2 2 |
agiembed | 0:7ccd56e1c3b1 | 7 | |
agiembed | 0:7ccd56e1c3b1 | 8 | unsigned char buf[1000], head[2], q[41], ck[1], data_idx = 0; |
agiembed | 0:7ccd56e1c3b1 | 9 | char AHRS[Ahrs_num], check_ahrs = start, checkdata = 0, checksum; |
agiembed | 0:7ccd56e1c3b1 | 10 | |
agiembed | 0:7ccd56e1c3b1 | 11 | |
agiembed | 0:7ccd56e1c3b1 | 12 | void IMU_Update(){ |
agiembed | 0:7ccd56e1c3b1 | 13 | fcc.roll = IMU.roll; |
agiembed | 0:7ccd56e1c3b1 | 14 | fcc.pitch = IMU.pitch; |
agiembed | 0:7ccd56e1c3b1 | 15 | fcc.yaw = IMU.yaw; |
agiembed | 0:7ccd56e1c3b1 | 16 | fcc.g_roll = IMU.g_roll; |
agiembed | 0:7ccd56e1c3b1 | 17 | fcc.g_pitch = IMU.g_pitch; |
agiembed | 0:7ccd56e1c3b1 | 18 | fcc.g_yaw = IMU.g_yaw; |
agiembed | 0:7ccd56e1c3b1 | 19 | fcc.acc_x = IMU.acc_x; |
agiembed | 0:7ccd56e1c3b1 | 20 | fcc.acc_y = IMU.acc_y; |
agiembed | 0:7ccd56e1c3b1 | 21 | fcc.acc_z = IMU.acc_z; |
agiembed | 0:7ccd56e1c3b1 | 22 | } |
agiembed | 0:7ccd56e1c3b1 | 23 | |
agiembed | 0:7ccd56e1c3b1 | 24 | |
agiembed | 0:7ccd56e1c3b1 | 25 | void Get_AHRS(char *data){ |
agiembed | 0:7ccd56e1c3b1 | 26 | int i; |
agiembed | 0:7ccd56e1c3b1 | 27 | char *dTemp = (char *) &IMU; |
agiembed | 0:7ccd56e1c3b1 | 28 | |
agiembed | 0:7ccd56e1c3b1 | 29 | for(i=0; i<Ahrs_num; i++) *(dTemp+i) = *(data+i); |
agiembed | 0:7ccd56e1c3b1 | 30 | IMU_Update(); |
agiembed | 0:7ccd56e1c3b1 | 31 | } |
agiembed | 0:7ccd56e1c3b1 | 32 | |
agiembed | 0:7ccd56e1c3b1 | 33 | void ahrs_data(unsigned char data){ |
agiembed | 0:7ccd56e1c3b1 | 34 | |
agiembed | 0:7ccd56e1c3b1 | 35 | if(check_ahrs == start){ |
agiembed | 0:7ccd56e1c3b1 | 36 | if(data == head1) check_ahrs = check1; |
agiembed | 0:7ccd56e1c3b1 | 37 | else check_ahrs = start; |
agiembed | 0:7ccd56e1c3b1 | 38 | } |
agiembed | 0:7ccd56e1c3b1 | 39 | |
agiembed | 0:7ccd56e1c3b1 | 40 | else if(check_ahrs == check1){ |
agiembed | 0:7ccd56e1c3b1 | 41 | if(data == head2) check_ahrs = check2; |
agiembed | 0:7ccd56e1c3b1 | 42 | else check_ahrs = start; |
agiembed | 0:7ccd56e1c3b1 | 43 | } |
agiembed | 0:7ccd56e1c3b1 | 44 | |
agiembed | 0:7ccd56e1c3b1 | 45 | else if(check_ahrs == check2){ |
agiembed | 0:7ccd56e1c3b1 | 46 | AHRS[data_idx] = data; |
agiembed | 0:7ccd56e1c3b1 | 47 | if(data_idx<(Ahrs_num-1)){ |
agiembed | 0:7ccd56e1c3b1 | 48 | //checkdata ^= AHRS[data_idx]; |
agiembed | 0:7ccd56e1c3b1 | 49 | data_idx++; |
agiembed | 0:7ccd56e1c3b1 | 50 | if(data_idx == 3) data_idx = 4; |
agiembed | 0:7ccd56e1c3b1 | 51 | } |
agiembed | 0:7ccd56e1c3b1 | 52 | else{ |
agiembed | 0:7ccd56e1c3b1 | 53 | checksum = data; |
agiembed | 0:7ccd56e1c3b1 | 54 | //if(checksum == checkdata) |
agiembed | 0:7ccd56e1c3b1 | 55 | Get_AHRS(AHRS); |
agiembed | 0:7ccd56e1c3b1 | 56 | data_idx = 0; |
agiembed | 0:7ccd56e1c3b1 | 57 | check_ahrs = start; |
agiembed | 0:7ccd56e1c3b1 | 58 | } |
agiembed | 0:7ccd56e1c3b1 | 59 | } |
agiembed | 0:7ccd56e1c3b1 | 60 | |
agiembed | 0:7ccd56e1c3b1 | 61 | else{ |
agiembed | 0:7ccd56e1c3b1 | 62 | data_idx = 0; |
agiembed | 0:7ccd56e1c3b1 | 63 | check_ahrs = start; |
agiembed | 0:7ccd56e1c3b1 | 64 | } |
agiembed | 0:7ccd56e1c3b1 | 65 | } |
agiembed | 0:7ccd56e1c3b1 | 66 | |
agiembed | 0:7ccd56e1c3b1 | 67 | void ahrs_rec(){ |
agiembed | 0:7ccd56e1c3b1 | 68 | unsigned char data; |
agiembed | 0:7ccd56e1c3b1 | 69 | data = ahrs.getc(); |
agiembed | 0:7ccd56e1c3b1 | 70 | ahrs_data(data); |
agiembed | 0:7ccd56e1c3b1 | 71 | //pc.putc(data); |
agiembed | 0:7ccd56e1c3b1 | 72 | } |