Raharja Agie / Mbed 2 deprecated Controller

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

ahrs.h

Committer:
agiembed
Date:
2010-08-12
Revision:
0:bd282c11d296

File content as of revision 0:bd282c11d296:

#define Ahrs_num 40
#define start 0
#define head1 0xFF
#define head2 0xFF
#define check1 1
#define check2 2

unsigned char buf[1000], head[2], q[41], ck[1], data_idx = 0;
char AHRS[Ahrs_num], check_ahrs = start, checkdata = 0, checksum;


void IMU_Update(){
    fcc.roll = IMU.roll;
    fcc.pitch = IMU.pitch;
    fcc.yaw = IMU.yaw;
    fcc.g_roll = IMU.g_roll;
    fcc.g_pitch = IMU.g_pitch;
    fcc.g_yaw = IMU.g_yaw;
    fcc.acc_x = IMU.acc_x;
    fcc.acc_y = IMU.acc_y;
    fcc.acc_z = IMU.acc_z;
} 
        

void Get_AHRS(char *data){
    int i;
    char *dTemp = (char *) &IMU;
    
    for(i=0; i<Ahrs_num; i++) *(dTemp+i) = *(data+i);
    IMU_Update();
}   

void ahrs_data(unsigned char data){
    
    if(check_ahrs == start){
        if(data == head1) check_ahrs = check1;
        else check_ahrs = start;
        }
    
    else if(check_ahrs == check1){
        if(data == head2) check_ahrs = check2;
        else check_ahrs = start;
        }
    
    else if(check_ahrs == check2){
        AHRS[data_idx] = data;
        if(data_idx<(Ahrs_num-1)){                  
             //checkdata ^= AHRS[data_idx];
             data_idx++;
             if(data_idx == 3) data_idx = 4;
        }
        else{
            checksum = data;
            //if(checksum == checkdata)
            Get_AHRS(AHRS);                        
            data_idx = 0;
            check_ahrs = start;
        }                  
    }
    
    else{
        data_idx = 0;
        check_ahrs = start;
    }
}  

void ahrs_rec(){      
    unsigned char data;
    data = ahrs.getc();
    ahrs_data(data);
    //pc.putc(data);
}