Vehicle-cpp/zxf/JMros.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>

#include "pxTools.h"
#include "pxEpoll.h"
#include "JMcan.h"
#include "JMros.h"

extern int epollfd;
extern POSTDATABUF_S postData;

static int serverfd=0;
static int clientfd=0;
static PXEPOLL_S *clientEpoll=NULL;
static u8 allzero[8]={0};

void serverReceive(PXEPOLL_S *epoll)
{
	char buf[1024]={0};
	ssize_t s = read(epoll->clientfd,buf,sizeof(buf));
	if(s>0)
	{
		//正常接收
        if(s==sizeof(ROSRECVDATA_S))
        {
            // postData.wheelGet=((ROSRECVDATA_S*)buf)->whellGet;
            // CAN_setSendBuf(AUTOCONTROL,((ROSRECVDATA_S*)buf)->data);
            // CAN_setCan306(((ROSRECVDATA_S*)buf)->can306);
            // send(epoll->clientfd, (void*)&postData, sizeof(POSTDATABUF_S), 0);
            memcpy(postData.rosdata,buf+sizeof(u8),sizeof(ROSRECVDATA_S)-sizeof(u16)-sizeof(u8));
        }
        else
        {
            //数据不正常
            // CAN_setCan306(allzero);
            pxLog(ERROR,"[ROS] recv wrong pack,len:%d",s);
        }
	}
	else
	{
        pxLog(INFO,"[ROS] a client closed");
        epollDel(epoll);
        clientfd=0;
        clientEpoll=NULL;
        postData.status.ros=0;
	}
}

void serverReceiveFailed(PXEPOLL_S *epoll)
{
    pxLog(INFO,"[ROS] a client closed");
    if(clientEpoll)
    {
        epollDel(clientEpoll);
        clientfd=0;
        clientEpoll=NULL;
        postData.status.ros=0;
    }
}

void server_accpet(PXEPOLL_S *epoll)
{
    struct sockaddr_in client;
	socklen_t len = sizeof(client);
	
	int new_fd = accept(serverfd,(struct sockaddr*)&client,&len);
	if(new_fd < 0)
	{
		pxLog(INFO,"[ROS] A new client connect failed,code:%d",new_fd);
		return;
	}

    if(clientfd!=0)
    {
        epollDel(clientEpoll);
        clientEpoll=NULL;
    }
    clientEpoll=epollAdd(epollfd,new_fd,serverReceive,serverReceiveFailed,NULL);
	if(clientEpoll==NULL)
    {
		pxLog(ERROR,"[ROS] A new client Receive failed");
    }
	pxLog(INFO,"[ROS] A new client connect:%d",new_fd);
    postData.status.ros=1;
    clientfd=new_fd;
}

void ROS_listen(PXTIMER_S *p)
{
    pxLog(INIT,"[ROS] communication Init");
    postData.status.ros=0;
    struct sockaddr_in my_addr;
    if ((serverfd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
    {
        pxLog(ERROR,"[ROS] socket error:%d,retry in 3 sec",serverfd);
	    timerAdd(epollfd,0,3000,1,ROS_listen,NULL);
        return;
    }
    int mw_optval = 1;
    setsockopt(serverfd, SOL_SOCKET, SO_REUSEADDR, (char *)&mw_optval,sizeof(mw_optval));

    my_addr.sin_family = AF_INET;
    my_addr.sin_port = htons(29812);
    my_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    bzero(&(my_addr.sin_zero),8);
    if (bind(serverfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr))== -1)
    {
        pxLog(ERROR,"[ROS] bind error,retry in 3 sec");
	    timerAdd(epollfd,0,3000,1,ROS_listen,NULL);
        return;
    }
    if (listen(serverfd, 20) == -1)
    { 
        pxLog(ERROR,"[ROS] listen error,retry in 3 sec");
	    timerAdd(epollfd,0,3000,1,ROS_listen,NULL);
        return;
    }
    pxLog(INIT,"[ROS] communication Init success");
    PXEPOLL_S *serverEpoll=epollAdd(epollfd,serverfd,server_accpet,NULL,NULL);
	if(serverEpoll==NULL)
	{
    	pxLog(ERROR,"[ROS] server_accpet failed");
        return;
	}
}