Raharja Agie
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:bd282c11d296, 2010-08-12 (annotated)
- Committer:
- agiembed
- Date:
- Thu Aug 12 01:08:40 2010 +0000
- Revision:
- 0:bd282c11d296
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| agiembed | 0:bd282c11d296 | 1 | #define Ahrs_num 40 |
| agiembed | 0:bd282c11d296 | 2 | #define start 0 |
| agiembed | 0:bd282c11d296 | 3 | #define head1 0xFF |
| agiembed | 0:bd282c11d296 | 4 | #define head2 0xFF |
| agiembed | 0:bd282c11d296 | 5 | #define check1 1 |
| agiembed | 0:bd282c11d296 | 6 | #define check2 2 |
| agiembed | 0:bd282c11d296 | 7 | |
| agiembed | 0:bd282c11d296 | 8 | unsigned char buf[1000], head[2], q[41], ck[1], data_idx = 0; |
| agiembed | 0:bd282c11d296 | 9 | char AHRS[Ahrs_num], check_ahrs = start, checkdata = 0, checksum; |
| agiembed | 0:bd282c11d296 | 10 | |
| agiembed | 0:bd282c11d296 | 11 | |
| agiembed | 0:bd282c11d296 | 12 | void IMU_Update(){ |
| agiembed | 0:bd282c11d296 | 13 | fcc.roll = IMU.roll; |
| agiembed | 0:bd282c11d296 | 14 | fcc.pitch = IMU.pitch; |
| agiembed | 0:bd282c11d296 | 15 | fcc.yaw = IMU.yaw; |
| agiembed | 0:bd282c11d296 | 16 | fcc.g_roll = IMU.g_roll; |
| agiembed | 0:bd282c11d296 | 17 | fcc.g_pitch = IMU.g_pitch; |
| agiembed | 0:bd282c11d296 | 18 | fcc.g_yaw = IMU.g_yaw; |
| agiembed | 0:bd282c11d296 | 19 | fcc.acc_x = IMU.acc_x; |
| agiembed | 0:bd282c11d296 | 20 | fcc.acc_y = IMU.acc_y; |
| agiembed | 0:bd282c11d296 | 21 | fcc.acc_z = IMU.acc_z; |
| agiembed | 0:bd282c11d296 | 22 | } |
| agiembed | 0:bd282c11d296 | 23 | |
| agiembed | 0:bd282c11d296 | 24 | |
| agiembed | 0:bd282c11d296 | 25 | void Get_AHRS(char *data){ |
| agiembed | 0:bd282c11d296 | 26 | int i; |
| agiembed | 0:bd282c11d296 | 27 | char *dTemp = (char *) &IMU; |
| agiembed | 0:bd282c11d296 | 28 | |
| agiembed | 0:bd282c11d296 | 29 | for(i=0; i<Ahrs_num; i++) *(dTemp+i) = *(data+i); |
| agiembed | 0:bd282c11d296 | 30 | IMU_Update(); |
| agiembed | 0:bd282c11d296 | 31 | } |
| agiembed | 0:bd282c11d296 | 32 | |
| agiembed | 0:bd282c11d296 | 33 | void ahrs_data(unsigned char data){ |
| agiembed | 0:bd282c11d296 | 34 | |
| agiembed | 0:bd282c11d296 | 35 | if(check_ahrs == start){ |
| agiembed | 0:bd282c11d296 | 36 | if(data == head1) check_ahrs = check1; |
| agiembed | 0:bd282c11d296 | 37 | else check_ahrs = start; |
| agiembed | 0:bd282c11d296 | 38 | } |
| agiembed | 0:bd282c11d296 | 39 | |
| agiembed | 0:bd282c11d296 | 40 | else if(check_ahrs == check1){ |
| agiembed | 0:bd282c11d296 | 41 | if(data == head2) check_ahrs = check2; |
| agiembed | 0:bd282c11d296 | 42 | else check_ahrs = start; |
| agiembed | 0:bd282c11d296 | 43 | } |
| agiembed | 0:bd282c11d296 | 44 | |
| agiembed | 0:bd282c11d296 | 45 | else if(check_ahrs == check2){ |
| agiembed | 0:bd282c11d296 | 46 | AHRS[data_idx] = data; |
| agiembed | 0:bd282c11d296 | 47 | if(data_idx<(Ahrs_num-1)){ |
| agiembed | 0:bd282c11d296 | 48 | //checkdata ^= AHRS[data_idx]; |
| agiembed | 0:bd282c11d296 | 49 | data_idx++; |
| agiembed | 0:bd282c11d296 | 50 | if(data_idx == 3) data_idx = 4; |
| agiembed | 0:bd282c11d296 | 51 | } |
| agiembed | 0:bd282c11d296 | 52 | else{ |
| agiembed | 0:bd282c11d296 | 53 | checksum = data; |
| agiembed | 0:bd282c11d296 | 54 | //if(checksum == checkdata) |
| agiembed | 0:bd282c11d296 | 55 | Get_AHRS(AHRS); |
| agiembed | 0:bd282c11d296 | 56 | data_idx = 0; |
| agiembed | 0:bd282c11d296 | 57 | check_ahrs = start; |
| agiembed | 0:bd282c11d296 | 58 | } |
| agiembed | 0:bd282c11d296 | 59 | } |
| agiembed | 0:bd282c11d296 | 60 | |
| agiembed | 0:bd282c11d296 | 61 | else{ |
| agiembed | 0:bd282c11d296 | 62 | data_idx = 0; |
| agiembed | 0:bd282c11d296 | 63 | check_ahrs = start; |
| agiembed | 0:bd282c11d296 | 64 | } |
| agiembed | 0:bd282c11d296 | 65 | } |
| agiembed | 0:bd282c11d296 | 66 | |
| agiembed | 0:bd282c11d296 | 67 | void ahrs_rec(){ |
| agiembed | 0:bd282c11d296 | 68 | unsigned char data; |
| agiembed | 0:bd282c11d296 | 69 | data = ahrs.getc(); |
| agiembed | 0:bd282c11d296 | 70 | ahrs_data(data); |
| agiembed | 0:bd282c11d296 | 71 | //pc.putc(data); |
| agiembed | 0:bd282c11d296 | 72 | } |