Raharja Agie / Mbed 2 deprecated Mini-X

ahrs.h@0:d463d5c04541, 2011-08-16 (annotated)

Committer:
agiembed
Date:
Tue Aug 16 05:32:33 2011 +0000
Revision:
0:d463d5c04541

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


agiembed

0:d463d5c04541


     1

#define Ahrs_num 42





agiembed

0:d463d5c04541


     2

char checkhead[2], imu_cnt = 0, data_flag = 0, gcs_cnt = 0;





agiembed

0:d463d5c04541


     3

unsigned char data_que[Ahrs_num];





agiembed

0:d463d5c04541


     4







agiembed

0:d463d5c04541


     5







agiembed

0:d463d5c04541


     6

void IMU_Update(){





agiembed

0:d463d5c04541


     7

    fcc.roll = IMU.roll;





agiembed

0:d463d5c04541


     8

    fcc.pitch = IMU.pitch;





agiembed

0:d463d5c04541


     9

    fcc.yaw = IMU.yaw;





agiembed

0:d463d5c04541


    10

    fcc.g_roll = IMU.g_roll;





agiembed

0:d463d5c04541


    11

    fcc.g_pitch = IMU.g_pitch;





agiembed

0:d463d5c04541


    12

    fcc.g_yaw = IMU.g_yaw;





agiembed

0:d463d5c04541


    13

    fcc.acc_x = IMU.acc_x;





agiembed

0:d463d5c04541


    14

    fcc.acc_y = IMU.acc_y;





agiembed

0:d463d5c04541


    15

    fcc.acc_z = IMU.acc_z;// + (cos((fcc.roll*3.14)/180) * cos((fcc.pitch*3.14)/180));





agiembed

0:d463d5c04541


    16

    fcc.bat = (unsigned short)(bat * 12000);





agiembed

0:d463d5c04541


    17

    fcc.US = (unsigned short)(alti.read_u16() * 0.06);





agiembed

0:d463d5c04541


    18

    fcc.IR1 = (unsigned short)(IRb.read_u16() * 0.01) - 220;





agiembed

0:d463d5c04541


    19

    fcc.IR2 = (unsigned short)(IRl.read_u16() * 0.01) - 220;





agiembed

0:d463d5c04541


    20

    fcc.IR3 = (unsigned short)(IRf.read_u16() * 0.01) - 220;





agiembed

0:d463d5c04541


    21

    fcc.IR4 = (unsigned short)(IRr.read_u16() * 0.01) - 220;





agiembed

0:d463d5c04541


    22

    





agiembed

0:d463d5c04541


    23

    if(fcc.roll>180 || fcc.roll<-180) fcc.roll = last.roll;





agiembed

0:d463d5c04541


    24

    if(fcc.pitch>90 || fcc.pitch<-90) fcc.pitch = last.pitch;





agiembed

0:d463d5c04541


    25

    if(fcc.yaw>360 || fcc.yaw<0) fcc.yaw = last.yaw;





agiembed

0:d463d5c04541


    26

    if(fcc.g_roll>300 || fcc.g_roll<-300) fcc.g_roll = last.g_roll;





agiembed

0:d463d5c04541


    27

    if(fcc.g_pitch>300 || fcc.g_pitch<-300) fcc.g_pitch = last.g_pitch;





agiembed

0:d463d5c04541


    28

    if(fcc.g_yaw>300 || fcc.g_yaw<-300) fcc.g_yaw = last.g_yaw;





agiembed

0:d463d5c04541


    29

    if(fcc.acc_x>4 || fcc.acc_x<-4) fcc.acc_x = last.acc_x;





agiembed

0:d463d5c04541


    30

    if(fcc.acc_y>4 || fcc.acc_y<-4) fcc.acc_y = last.acc_y;





agiembed

0:d463d5c04541


    31

    if(fcc.acc_z>4 || fcc.acc_z<-4) fcc.acc_z = last.acc_z;





agiembed

0:d463d5c04541


    32

    





agiembed

0:d463d5c04541


    33

    last.roll = IMU.roll;





agiembed

0:d463d5c04541


    34

    last.pitch = IMU.pitch;





agiembed

0:d463d5c04541


    35

    last.yaw = IMU.yaw;





agiembed

0:d463d5c04541


    36

    last.g_roll = IMU.g_roll;





agiembed

0:d463d5c04541


    37

    last.g_pitch = IMU.g_pitch;





agiembed

0:d463d5c04541


    38

    last.g_yaw = IMU.g_yaw;





agiembed

0:d463d5c04541


    39

    last.acc_x = IMU.acc_x;





agiembed

0:d463d5c04541


    40

    last.acc_y = IMU.acc_y;





agiembed

0:d463d5c04541


    41

    last.acc_z = IMU.acc_z;





agiembed

0:d463d5c04541


    42

    





agiembed

0:d463d5c04541


    43

    if(gcs_cnt == 3){





agiembed

0:d463d5c04541


    44

        //IMUtoGCS();





agiembed

0:d463d5c04541


    45

        gcs_cnt = 0;





agiembed

0:d463d5c04541


    46

    }





agiembed

0:d463d5c04541


    47

    else toBHG();





agiembed

0:d463d5c04541


    48

    gcs_cnt++;    





agiembed

0:d463d5c04541


    49







agiembed

0:d463d5c04541


    50

}                 





agiembed

0:d463d5c04541


    51







agiembed

0:d463d5c04541


    52

void ahrs_parsing(){





agiembed

0:d463d5c04541


    53

    int i = 0;





agiembed

0:d463d5c04541


    54

    





agiembed

0:d463d5c04541


    55

    data.roll = (char *) &IMU.roll;





agiembed

0:d463d5c04541


    56

    for(i=0; i<4; i++) data.roll[i] = data_que[i+28];





agiembed

0:d463d5c04541


    57

    





agiembed

0:d463d5c04541


    58

    data.pitch = (char *) &IMU.pitch;





agiembed

0:d463d5c04541


    59

    for(i=0; i<4; i++) data.pitch[i] = data_que[i+32];





agiembed

0:d463d5c04541


    60

    





agiembed

0:d463d5c04541


    61

    data.yaw = (char *) &IMU.yaw;





agiembed

0:d463d5c04541


    62

    for(i=0; i<4; i++) data.yaw[i] = data_que[i+36];    





agiembed

0:d463d5c04541


    63

    





agiembed

0:d463d5c04541


    64

    data.g_roll = (char *) &IMU.g_roll;





agiembed

0:d463d5c04541


    65

    for(i=0; i<4; i++) data.g_roll[i] = data_que[i+4];





agiembed

0:d463d5c04541


    66

    





agiembed

0:d463d5c04541


    67

    data.g_pitch = (char *) &IMU.g_pitch;





agiembed

0:d463d5c04541


    68

    for(i=0; i<4; i++) data.g_pitch[i] = data_que[i+8];





agiembed

0:d463d5c04541


    69

    





agiembed

0:d463d5c04541


    70

    data.g_yaw = (char *) &IMU.g_yaw;





agiembed

0:d463d5c04541


    71

    for(i=0; i<4; i++) data.g_yaw[i] = data_que[i+12];        





agiembed

0:d463d5c04541


    72

            





agiembed

0:d463d5c04541


    73

    data.acc_x = (char *) &IMU.acc_x;





agiembed

0:d463d5c04541


    74

    for(i=0; i<4; i++) data.acc_x[i] = data_que[i+16];





agiembed

0:d463d5c04541


    75

    





agiembed

0:d463d5c04541


    76

    data.acc_y = (char *) &IMU.acc_y;





agiembed

0:d463d5c04541


    77

    for(i=0; i<4; i++) data.acc_y[i] = data_que[i+20];





agiembed

0:d463d5c04541


    78

    





agiembed

0:d463d5c04541


    79

    data.acc_z = (char *) &IMU.acc_z;





agiembed

0:d463d5c04541


    80

    for(i=0; i<4; i++) data.acc_z[i] = data_que[i+24]; 





agiembed

0:d463d5c04541


    81

    





agiembed

0:d463d5c04541


    82

    IMU_Update();





agiembed

0:d463d5c04541


    83

}





agiembed

0:d463d5c04541


    84







agiembed

0:d463d5c04541


    85

void Checksum(){





agiembed

0:d463d5c04541


    86

    char checksum = 0;





agiembed

0:d463d5c04541


    87

    for(char i=1; i<40; i++) checksum = checksum ^ data_que[i];





agiembed

0:d463d5c04541


    88

    if(checksum == data_que[40]) ahrs_parsing();





agiembed

0:d463d5c04541


    89

}    





agiembed

0:d463d5c04541


    90







agiembed

0:d463d5c04541


    91

void ahrs_rec(){      





agiembed

0:d463d5c04541


    92

    unsigned char data;





agiembed

0:d463d5c04541


    93

    data = ahrs.getc();





agiembed

0:d463d5c04541


    94

    checkhead[0] = checkhead[1];





agiembed

0:d463d5c04541


    95

    checkhead[1] = data;





agiembed

0:d463d5c04541


    96

    if(checkhead[0] == 0xFF && checkhead[1] == 0xFF) {





agiembed

0:d463d5c04541


    97

        imu_cnt = 0;





agiembed

0:d463d5c04541


    98

        data_flag = 1;





agiembed

0:d463d5c04541


    99

    }





agiembed

0:d463d5c04541


   100

    if(data_flag == 1){





agiembed

0:d463d5c04541


   101

        data_que[imu_cnt] = data;





agiembed

0:d463d5c04541


   102

        imu_cnt++;





agiembed

0:d463d5c04541


   103

    }





agiembed

0:d463d5c04541


   104

    if(imu_cnt == 41){





agiembed

0:d463d5c04541


   105

        Checksum();





agiembed

0:d463d5c04541


   106

        data_flag = 2;





agiembed

0:d463d5c04541


   107

    }    





agiembed

0:d463d5c04541


   108

}





agiembed

0:d463d5c04541


   109







agiembed

0:d463d5c04541


   110







agiembed

0:d463d5c04541


   111







agiembed

0:d463d5c04541


   112







agiembed

0:d463d5c04541


   113

    





agiembed

0:d463d5c04541


   114

    





agiembed

0:d463d5c04541


   115

    





agiembed

0:d463d5c04541


   116

    





agiembed

0:d463d5c04541


   117

        





agiembed

0:d463d5c04541


   118