GPS受信機から得たデータをCANに流すプログラム STM32F303K8

Dependencies:   mbed LSM303D

main.cpp

Committer:
sashida_h
Date:
2020-07-22
Revision:
0:c0aa7d5cd549
Child:
1:03c3b1db6179

File content as of revision 0:c0aa7d5cd549:

#include "mbed.h"

DigitalOut myled(LED1);
Serial gps(PA_9, PA_10,38400);
Serial pc(PA_2, PA_3,115200);
CAN can(PA_11, PA_12); //pin21,22 rd,td

CANMessage msg;

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

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 receive(){
    Float2Byte getFloat;
    
    if(can.read(msg)){
        /*ID: 0x01*/
        if(msg.id == 0x01){
            //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("%.2f\r\n", getFloat._float);
            myled = !myled;
        }
        /*ID: 0x02*/
        if(msg.id == 0x02){
            //pc.printf("ID: 0x02\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("%.3f\n\r", getFloat._float);
            myled = !myled;
        }
    }
    
}

int main() {
    char gps_data[256];
    int cnt_gps=0;
    char c;
    pc.printf("Start\r\n");
    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);
                        //pc.printf("NoGPSSignal\r\n");
                    }
                }else{
                    pc.printf("No_Satellite_signal\r\n");
                }
            }else{
                cnt_gps++;
            }
        }
        if(pc.readable()){
            pc.printf("kita!\r\n");
            while(1){
                c = pc.getc(); 
                pc.printf("%c",c);
            }
        }
    }
}