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)

Dependencies:   mbed

Committer:
agiembed
Date:
Wed Aug 11 01:23:42 2010 +0000
Revision:
0:7ccd56e1c3b1

        

Who changed what in which revision?

UserRevisionLine numberNew 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 }