Vehicle-cpp/zxf/JMcan.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <time.h>
#include <poll.h>
#include <errno.h>
#include <fcntl.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>

#include <linux/can/error.h>

#include "JMmqtt.h"
#include "JMremote.h"
#include "JMcan.h"

extern u8 carGear;

static struct can_frame canPack={0,8,0,0,0,{0}};
static int canSocket=0;

u32 canRecvPackIDs[CANRECVPACKCOUNT]={0x1f0,0x1f1,0x1f2,0x1f4,0x1f5,0x1f6,0x2f2,0x3f0,0x3f2,0x4f0,0x4f2,0x4f4,0x5f4,0x6f4,0x70a,0x18FEE500,0x18FEEE00,0x18FEEF00,0x0CF00300,0x0CF00400,0x0CFF2403,0x0CFF2303};
u32 canPackIDs[CANPACKCOUNT]={0x186,0x286,0x386,0x486};

u8 defaultCan[CANBUFSIZE]={
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xff,
    0x00,0x00,0x01,0x01,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
};

u8 canSendBuf[CANBUFSIZE]={0};
u8 *manualSendBuf=defaultCan;
u8 *sendBuf=NULL;
POSTDATABUF_S postData={0};

u8 can306[8]={0};

int CAN_Open(char *name)
{
    struct ifreq ifr;
	struct sockaddr_can addr;
    pxLog(INFO,"[CAN] CAN ready to open %s",name);

    if(canSocket>0)
    {
        close(canSocket);
        canSocket=0;
    }
	/* open socket */
	if ((canSocket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) 
    {
    	pxLog(ERROR,"[CAN] CAN create failed:%d,exit.",canSocket);
        return -1;
	}
    
	strncpy(ifr.ifr_name, name, IFNAMSIZ - 1);
	ifr.ifr_name[IFNAMSIZ - 1] = '\0';
	ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name);
	if (!ifr.ifr_ifindex) {
    	pxLog(ERROR,"[CAN] CAN if_nametoindex failed,exit.");
	    close(canSocket);
        canSocket=0;
        return -1;
	}

	memset(&addr, 0, sizeof(addr));
	addr.can_family = AF_CAN;
	addr.can_ifindex = ifr.ifr_ifindex;
    
	struct can_filter *rfilter= malloc(sizeof(struct can_filter) * (CANRECVPACKCOUNT+1));
    if (!rfilter) {
        pxLog(ERROR,"[CAN] CAN set filter failed,exit.");
	    close(canSocket);
        canSocket=0;
        return -1;
    }
    for(int x=0;x<CANRECVPACKCOUNT;x++)
    {
        rfilter[x].can_id=canRecvPackIDs[x];
        rfilter[x].can_mask=0xfffffff;
    }
    rfilter[CANRECVPACKCOUNT].can_id=0x303;
    rfilter[CANRECVPACKCOUNT].can_mask=0xfff;
	setsockopt(canSocket, SOL_CAN_RAW, CAN_RAW_FILTER, rfilter, sizeof(struct can_filter) * (CANRECVPACKCOUNT+1));
    
    // 设置 Socket 套接字为非阻塞模式
    int flags = fcntl(canSocket, F_GETFL, 0);
    fcntl(canSocket, F_SETFL, flags |O_NONBLOCK);

	if (bind(canSocket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
    	pxLog(ERROR,"[CAN] CAN bind failed,exit.");
	    close(canSocket);
        canSocket=0;
        return -1;
	}
	return 0;
}

void CAN_setCan306(u8 *data)
{
    memcpy(can306,data,8);
}

void CAN_setSendBuf(int sender,u8 *data)
{
    if(sender==postData.status.system && data)
        memcpy(canSendBuf,data,CANBUFSIZE);
    else if(CLEARCANSENDBUF==sender)
        memcpy(canSendBuf,defaultCan,CANBUFSIZE);
}

int memcmpset(void* data,void *src,void *dst,int len)
{
    if(memcmp(data,src,len))
    {
        memcpy(src,data,len);
        memcpy(dst,data,len);
        return 1;
    }
    return 0;
}

void canDataLog()
{
    time_t now;
	struct tm *dateNow;
    static u64 timeOrig=0;
    static int dateLast=0;
    static FILE *logfp = NULL;
    static u8 logBuf[CANBUFSIZE+2]={0};

	time(&now);
	dateNow = localtime(&now);
    if(dateLast!=dateNow->tm_mday || logfp==NULL)
    {
        dateLast=dateNow->tm_mday;
        if(logfp)
        {
            timeOrig=get_timeStamp();
            fwrite(&timeOrig,8,1,logfp);
            fclose(logfp);
        }
        char fileName[64]={0};
        sprintf(fileName,DATALOGPATH"%02d-%02d-%02d",dateNow->tm_year+1900,dateNow->tm_mon+1,dateNow->tm_mday);

        logfp = fopen(fileName, "a+");
        if(logfp==NULL)
            return;
        fcntl(fileno(logfp), F_SETFD, FD_CLOEXEC);
        timeOrig=get_timeStamp();
        static u8 startSign[4]={0xfc,0xfd,0xfe,0xff};
        fwrite(startSign,4,1,logfp);
        fwrite(&timeOrig,8,1,logfp);

        memcpy(logBuf,&postData.status,1);
        u8 wheel=(s8)(*(double*)(&postData.wheelSet));
        logBuf[1]=wheel;
        memcpy(logBuf+2,sendBuf,CANBUFSIZE);
        fwrite(logBuf,CANBUFSIZE+2,1,logfp);
        fflush(logfp);
        return;
    }
    
    u8 logTmp[CANBUFSIZE+7]={0};
    int writeSize=5;
    if(memcmpset(&postData.status,logBuf,logTmp+writeSize,1))
    {
        logTmp[4]|=1;
        writeSize++;
    }
    u8 wheel=(u8)postData.wheelSet;
    if(memcmpset(&wheel,logBuf+1,logTmp+writeSize,1))
    {
        logTmp[4]|=2;
        writeSize++;
    }
    for(int x=0;x<CANPACKCOUNT;x++)
    {
        if(memcmpset(sendBuf+x*CAN_MAX_DLEN,logBuf+2+x*CAN_MAX_DLEN,logTmp+writeSize,CAN_MAX_DLEN))
        {
            logTmp[4]|=4<<x;
            writeSize+=CAN_MAX_DLEN;
        }
    }
    if(writeSize!=5)
    {
        u32 dt=get_timeStamp()-timeOrig;
        memcpy(logTmp,&dt,4);
        fwrite(logTmp,writeSize,1,logfp);
        fflush(logfp);
    }
}

int CAN_dataSend()
{
    static int logCount=0;
    if(logCount++ >= 10)
    {
        logCount=0;
        canDataLog();
    }
    for(int x=0;x<CANPACKCOUNT;x++)
    {
        canPack.can_id=canPackIDs[x];
        memcpy(canPack.data,sendBuf+x*CAN_MAX_DLEN,CAN_MAX_DLEN);
        if(write(canSocket,&canPack,sizeof(canPack))<0)
        {
            if(errno!=EAGAIN)
                return -1;
            else
            {
                usleep(100);
                if(write(canSocket,&canPack,sizeof(canPack))<0)
                    return -1;
            }
        }
    }
    //send 306
    // {
    //      canPack.can_id=0x306;
    //      memcpy(canPack.data,can306,CAN_MAX_DLEN);
    //     if(write(canSocket,&canPack,sizeof(canPack))<0)
    //     {
    //         if(errno!=EAGAIN)
    //             return -1;
    //         else
    //         {
    //             usleep(100);
    //             if(write(canSocket,&canPack,sizeof(canPack))<0)
    //                 return -1;
    //         }
    //     }
    // }
    return 0;
}

static int lastMode=0;

int CanSettoCanCantrol()
{
    return lastMode;
}

int CAN_dataRecv()
{
    int recvCount=0;
    int res=0;
    struct can_frame frame;
    while(1)
    {
        res = read(canSocket, &frame, sizeof(frame));
        if(res!=sizeof(frame))
        {
            if(res==-1 && errno==EAGAIN)
                return recvCount;
            else 
                return -1;
        }
        // if(frame.can_id==0x0CFF2303)
        // {
        //     carGear=frame.data[0];
        //     continue;
        // }
        frame.can_id=frame.can_id&0x1fffffff;
        if(frame.can_id==0x0CFF2303)
            carGear=frame.data[0];
        if(frame.can_id==0x4f2)   
        {
            //   pxLog(DEBUG,"[CAN] 0x4f2:1[%02x] 8[%02x]",frame.data[0],frame.data[7]);
            if((frame.data[7] & 0x02)!=0)
            {
                if(lastMode)
                {
                    //断开遥操
                    lastMode=0;
                    
                    pxLog(EVENT,"[CAN] mode set to manul");
                    MQTT_disConnectRemoteControl();
                }
            }
            else
                lastMode=1;
        }
        for(int x=0;x<CANRECVPACKCOUNT;x++)
        {
            if(frame.can_id==canRecvPackIDs[x])
            {
                recvCount++;
                memcpy(postData.data+x*CAN_MAX_DLEN,frame.data,CAN_MAX_DLEN);
                break;
            }
        }
    }
}

void CAN_Run(void *p)
{
    pxLog(INIT,"[CAN] Can thread run");
    mkdir(DATALOGPATH,S_IRWXU|S_IRGRP|S_IXGRP|S_IROTH);
    char *canName=(char*)p;
    int recvCount=0,recvEmptyCount=0;
    if(canName==NULL)
    {
        pxLog(ERROR,"[CAN] Can name is NULL");
        exit(-1);
    }
    while(1)
    {
        postData.status.can=0;
        while(1)
        {
            if(CAN_Open(canName)==0)
                break;
            pxLog(WARNING,"[CAN] open %s failed,retry in 60 sec",canName);
            sleep(60);
        }
        postData.status.can=1;
        recvEmptyCount=0;
        while(1)
        {
            if((postData.status.system==AUTOCONTROL && postData.status.ros)
                || (postData.status.system==REMOTECONTROL && postData.status.udp))
                sendBuf=canSendBuf;
            else
                sendBuf=manualSendBuf;
            // if(postData.status.system!=AUTOCONTROL && lastMode)
            if(postData.status.system==REMOTECONTROL && lastMode)
            {
                if(CAN_dataSend()!=0)
                {
                    sleep(1);
                    break;
                }
            }
            static int recvTime=0;
            if(recvTime++>=10)
            {
                recvTime=0;
                recvCount=CAN_dataRecv();
                if(recvCount==-1)
                {
                    sleep(1);
                    break;
                }
                if(recvCount==0)
                {
                    if(recvEmptyCount++>3)
                    {
                        //can接收超时
                        postData.status.canTimeout=1;
                    }
                }
                else
                {
                    recvEmptyCount=0;
                    postData.status.canTimeout=0;
                }
            }
            usleep(9000);
        }
    }
}