File content as of revision 3:a96f397dde4a:

#include "mbed.h"
#include "LSM303D.h"
#include <math.h>

DigitalOut myled(LED1);
Serial gps(PA_9, PA_10,38400);
Serial pc(PA_2, PA_3,38400);
CAN can(PA_11, PA_12); //pin21,22 rd,td
LSM303D lsm(PB_7,PB_6);
DigitalIn fpin(PB_1);

CANMessage msg;

float aData[3],mData[3];
float m_x,m_y,m_z,a_x,a_y,a_z,x,y,z;
int i=0;
float max_x = -60.0;
float max_y = -60.0;
float max_z = -60.0;
float min_x = 60.0;
float min_y = 60.0;
float min_z = 60.0;

float cal_x = -0.35207172;
float cal_y = 0.26154809;
float cal_z = 1.11167238;

float yaw = 0;
float pitch = 0;

union Float2Byte{
    float _float;
    char _byte[4];
};
typedef union Float2Byte Float2Byte;

union Int2Byte{
    unsigned short _int;
    char _byte[2];
};
typedef union Int2Byte Int2Byte;

float _DMS2DEG(float raw_data){
    int d=(int)(raw_data/100);
    float m=(raw_data-(float)d*100);
    return (float)d+m/60;
}

void send(float senddata,int id){
    //pc.printf("Master send()\n\r");
    
    /*ID: 0x01*/
    Float2Byte sendFloat;
    sendFloat._float = senddata;
    //ここに送りたい値を入れる．
    
    char serialData[4];
    for(int i=0;i<4;++i){
        serialData[i] = sendFloat._byte[i];
        //pc.printf("send_char: %d\n\r", serialData[i]);
    }
    //pc.printf("sendFloat: %f\n\r", sendFloat._float);
    if(can.write(CANMessage(id, serialData, 4))){
        //pc.printf("Send.\n\r");
    } 
    
    
    myled = !myled;
}

void send2(unsigned short senddata1,unsigned short senddata2,unsigned short senddata3,int id){
    //pc.printf("Master send()\n\r");
    
    /*ID: 0x01*/
    Int2Byte sendInt;
    sendInt._int = senddata1;
    //ここに送りたい値を入れる．
    
    char serialData[6];
    
    serialData[0] = sendInt._byte[0];
    serialData[1] = sendInt._byte[1];
        //pc.printf("send_char: %d\n\r", serialData[i]);
    sendInt._int = senddata2;
    serialData[2] = sendInt._byte[0];
    serialData[3] = sendInt._byte[1];

    sendInt._int = senddata3;
    serialData[4] = sendInt._byte[0];
    serialData[5] = sendInt._byte[1];
    //pc.printf("sendFloat: %f\n\r", sendFloat._float);
    if(can.write(CANMessage(id, serialData, 6))){
        //pc.printf("Send.\n\r");
    } 
    
    
    myled = !myled;
}

void receive(){
    Float2Byte getFloat;
    Int2Byte getInt;
    
    if(can.read(msg)){
        /*ID: 0x03*/
        
        if(msg.id == 0x03){
            //pc.printf("ID: 0x01\n\r");
            for(int i=0;i<4;++i){
                getFloat._byte[i] = msg.data[i];
                //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
            }
            pc.printf("IMU:%.2f\r\n", getFloat._float);
            myled = !myled;
        }
        
        /*ID: 0x02*/
        if(msg.id == 0x06){
            //pc.printf("ID: 0x02\n\r");
            getInt._byte[0] = msg.data[0];
            getInt._byte[1] = msg.data[1];
            pc.printf("HIKARI:%.2f", (float)getInt._int /10.0f);
            
            getInt._byte[0] = msg.data[2];
            getInt._byte[1] = msg.data[3];
            pc.printf(",%.2f", (float)getInt._int /10.0f);
            
            getInt._byte[0] = msg.data[4];
            getInt._byte[1] = msg.data[5];
            pc.printf(",%.2f\r\n", (float)getInt._int /10.0f);
        }
        
        if(msg.id == 0x05){
            //pc.printf("ID: 0x05\n\r");
            for(int i=0;i<6;++i){
                msg.data[i];
                pc.printf("%d,",msg.data[i]);
            }
            pc.printf("\n\r");
            myled = !myled;
        }
    }
    
}

int main() {
    char gps_data[256];
    int cnt_gps=0;
    int c=0;
    float world_time=0.0;
    wait(1.0);
    pc.printf("Start%d\r\n",sizeof(c));
    can.attach(receive, CAN::RxIrq);
    while(1){
        if(gps.readable()){
            gps_data[cnt_gps] = gps.getc();
            if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
                cnt_gps = 0;
                memset(gps_data,'\0',256);
            }else if(gps_data[cnt_gps] == '\r'){
                float world_time, lon_east, lat_north;
                int rlock, sat_num;
                char lat,lon;
                if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
                    if(rlock==1 || rlock==2){
                        lat_north = _DMS2DEG(lat_north);
                        lon_east = _DMS2DEG(lon_east);
                        //pc.printf("%s\r\n",gps_data);
                        pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
                        //pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max);
                        send(world_time,0x01);
                        
                    }else{
                        pc.printf("%s\r\n",gps_data);
                        if(sscanf(gps_data,"GPGGA,%f",&world_time)>=1){
                            send(world_time,0x01);
                        }else{
                            send(0.0,0x01);
                        }
                        
                        //pc.printf("NoGPSSignal\r\n");
                    }
                }else{
                    pc.printf("No_Satellite_signal\r\n");
                    send(0.0,0x01);
                }
            }else{
                cnt_gps++;
            }
        }
        if(pc.readable()){
            /*pc.printf("kita!\r\n");
            while(1){
                c = pc.getc(); 
                pc.printf("%c",c);
            }
            */
        }
        if(fpin == 0 && c == 0){
            lsm.read(aData,mData);
            m_x = mData[0]/10.0f - cal_x;
            m_y = mData[1]/10.0f - cal_y;
            m_z = mData[2]/10.0f - cal_z;
        
            a_x = aData[0] / 100.0f;
            a_y = aData[1] / 100.0f;
            a_z = aData[2] / 100.0f; 
        
            x = atan2(a_x, a_y);
            x = x*180.0f/3.1415f;
            y = atan2(0-a_z, (float)sqrt(a_x*a_x + a_y*a_y));
            y = y * 180.0f / 3.14159265f;
            z = atan2((a_x*m_x - a_y*m_y)*sqrt(a_x*a_x + a_y*a_y + a_z*a_z) , (a_x*a_x + a_y*a_y)*m_z + a_z*(a_x*m_y + a_y*m_x)) ;
            //z = atan2((a_x*a_x + a_y*a_y)*m_z + a_z*(a_x*m_y + a_y*m_x) , (a_x*m_x - a_y*m_y)*sqrt(a_x*a_x + a_y*a_y + a_z*a_z)) ;
            z = z * 180.0f / 3.14159265f;
            
            if(x < 0)x+360.0;
            if(y < 0)y+360.0;
            if(z < 0)z+360.0;
            //pc.printf("%.1f,%.1f,%.1f\r\n",x,y,z);
            send2((unsigned short)(x*10.0f),(unsigned short)(y*10.0f),(unsigned short)(z*10.0f),0x02);
            //send(world_time+(float)i*0.5,0x01);
            pc.printf("%d,%d,%d\r\n",(unsigned short)(x*10.0f),(unsigned short)(y*10.0f),(unsigned short)(z*10.0f));
            i++;
            wait(0.5);
            if(i==5) c=1;
            
        }
    }
}