添加模拟灯光

yy1717

2021-01-19 87156fa3adfa2e3232a6f6e612584aa8a4ebaea1

 lib/src/main/cpp/rtk_platform/platform.cpp

@@ -25,6 +25,7 @@
#include "../test_items2/stop_car.h"
#include "../test_items2/operate_gear.h"
#include "../test_items2/drive_straight.h"
#include "../rtk_module/virtual_rtk.h"

#define PARSE_BUFF_SIZE         4096

@@ -79,13 +80,12 @@

static bool mAyDevice = false;

struct platformSocket exceptSocket, currSocket;
struct platformSocket exceptSocket;
static sem_t sem_status_changed;

static bool requestPlatformSendRtk = false;
static int platform_tcp_fd = 0;
static CTcpPort *ctp = NULL;

static pthread_mutex_t platform_tx_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t events_mutex = PTHREAD_MUTEX_INITIALIZER;

static struct gpsBrief gbf;
@@ -101,10 +101,12 @@

static void ReqRtkPlatformConfigTimeout(union sigval sig);

static void TcpEventCallback(int stat, void *p, void *context);
static void TcpDataCallback(void *buffer, int length, void *p, void *context);

static void ConnectPlatform(const char *domain_name, int port);
static void ConnectPlatformLater(union sigval sig);

static void *PlatformDataListenThread(void *p);
static void *StatusListenThread(void *p);
static void PlatformChangeEntry(uint32_t events, const uint8_t *data, int length);

@@ -194,14 +196,10 @@
    memset(&rbf, 0, sizeof(rbf));
    memset(&defaultMcuRom, 0, sizeof(defaultMcuRom));

    pthread_mutex_init(&platform_tx_mutex, NULL);
    pthread_mutex_init(&events_mutex, NULL);

    eventQueue = NULL;

    platform_tcp_fd = -1;

    memset(&currSocket, 0, sizeof(currSocket));
    memset(&platformStatus, 0, sizeof(platformStatus));

    sem_init(&sem_status_changed, 0, 0);
@@ -247,14 +245,16 @@
        platformStatus.registed = 0;
    }

    pthread_mutex_lock(&platform_tx_mutex);
    if (platform_tcp_fd > 0 && (exceptSocket.port != currSocket.port || strcmp(currSocket.domain_name, exceptSocket.domain_name))) {
        DisconnectTCP(platform_tcp_fd);
        platform_tcp_fd = -1;
    } else if (platform_tcp_fd <= 0) {
        ConnectPlatform(exceptSocket.domain_name, exceptSocket.port);
    }
    pthread_mutex_unlock(&platform_tx_mutex);
    ConnectPlatform(exceptSocket.domain_name, exceptSocket.port);

//    pthread_mutex_lock(&platform_tx_mutex);
//    if (platform_tcp_fd > 0 && (exceptSocket.port != currSocket.port || strcmp(currSocket.domain_name, exceptSocket.domain_name))) {
//        DisconnectTCP(platform_tcp_fd);
//        platform_tcp_fd = -1;
//    } else if (platform_tcp_fd <= 0) {
//        ConnectPlatform(exceptSocket.domain_name, exceptSocket.port);
//    }
//    pthread_mutex_unlock(&platform_tx_mutex);
}

void PlatformStatusChanged(uint32_t event, const uint8_t *data, int length)
@@ -270,17 +270,43 @@
        return;

    DEBUG("连接RTK平台 %s: %d", domain_name, port);
    // TODO
    struct platformSocketInfo *ptr = (struct platformSocketInfo *)malloc(sizeof(struct platformSocketInfo));

    strcpy(ptr->domain_name, domain_name);
    ptr->port = port;
    if (ctp == NULL) {
        ctp = new CTcpPort();

    pthread_t platform_pid;
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
    pthread_create(&platform_pid, &attr, PlatformDataListenThread, ptr);
        ctp->set_event_callback(TcpEventCallback, NULL);
        ctp->set_data_callback(TcpDataCallback, NULL);
    }

    if (!ctp->IsOpen()) {
        ctp->OpenTcpPort(domain_name, port);
    } else if ((ctp->m_nPort != port || strcmp(ctp->m_sIp.c_str(), domain_name))) {
        ctp->CloseTcpPort();
    }
}

static void TcpEventCallback(int stat, void *p, void *context)
{
    CTcpPort *pTcpPort = (CTcpPort *)p;
    uint8_t buffer[256];

    strcpy((char *)buffer, pTcpPort->m_sIp.c_str());

    buffer[pTcpPort->m_sIp.length() + 1] = HI_UINT16(pTcpPort->m_nPort);
    buffer[pTcpPort->m_sIp.length() + 2] = LO_UINT16(pTcpPort->m_nPort);

    if (stat == 0) {
        PlatformStatusChanged(PLATFORM_CONNECT_EVT, buffer, pTcpPort->m_sIp.length() + 3);
    } else {
        PlatformStatusChanged(PLATFORM_DISCONNECT_EVT, buffer, pTcpPort->m_sIp.length() + 3);
    }
}

static void TcpDataCallback(void *buffer, int length, void *p, void *context)
{
    if (length > 0) {
        Parse(DATA_ACCESS_PLATFORM, (uint8_t *)buffer, length);
    }
}

static void ConnectPlatformLater(union sigval sig) {
@@ -306,8 +332,8 @@
{
    switch (events) {
        case PLATFORM_CONNECT_EVT: {
            DEBUG("平台连接成功 %s:%d", currSocket.domain_name, currSocket.port);
            MA_RtkPlatformConnect(1, currSocket.domain_name, currSocket.port);
            DEBUG("平台连接成功 %s:%d", data, BUILD_UINT16(data[strlen((char *)data) + 2], data[strlen((char *)data) + 1]));
            MA_RtkPlatformConnect(1, (char *)data, BUILD_UINT16(data[strlen((char *)data) + 2], data[strlen((char *)data) + 1]));

            platformStatus.connected = 1;
            if (!platformStatus.registed || platformStatus.platformKeyLength == 0) {
@@ -321,8 +347,8 @@
        }

        case PLATFORM_DISCONNECT_EVT: {
            DEBUG("平台断开 %s:%d", currSocket.domain_name, currSocket.port);
            MA_RtkPlatformConnect(0, currSocket.domain_name, currSocket.port);
            DEBUG("平台断开 %s:%d", data, BUILD_UINT16(data[strlen((char *)data) + 2], data[strlen((char *)data) + 1]));
            MA_RtkPlatformConnect(0, (char *)data, BUILD_UINT16(data[strlen((char *)data) + 2], data[strlen((char *)data) + 1]));

            AppTimer_delete(ConnectPlatformLater);
            AppTimer_add(ConnectPlatformLater, D_SEC(2));
@@ -660,7 +686,11 @@
            break;
        }
        case BLUETOOTH_DATA_EVT: {
            ParseMcu(data, length);
            if (Virtual2IsConnected()) {

            } else {
                ParseMcu(data, length);
            }
            break;
        }
        case SENSOR_CHANGE_EVT: {
@@ -672,76 +702,13 @@
    }
}

static void *PlatformDataListenThread(void *p) {
    struct platformSocket *ptr = (struct platformSocket *)p;

    uint8_t RxBuf[PARSE_BUFF_SIZE];

    int fds_ret;
    struct timeval tv;
    fd_set rdfds;
    fd_set exfds;

    int fd = -1;
    int RxBufLen = 0;

    fd = ConnectTCP(ptr->domain_name, ptr->port);

    pthread_mutex_lock(&platform_tx_mutex);
    platform_tcp_fd = fd;

    currSocket = *ptr;

    pthread_mutex_unlock(&platform_tx_mutex);

    if (fd > 0) {
        PlatformStatusChanged(PLATFORM_CONNECT_EVT, NULL, 0);
    }

    while (fd > 0) {
        tv.tv_sec = 5;
        tv.tv_usec = 0;
        FD_ZERO(&rdfds); //clean
        FD_SET(fd, &rdfds); //set

        fds_ret = select(fd + 1, &rdfds, NULL, NULL, &tv);

        if (fds_ret < 0) {
            break;
        } else if(fds_ret == 0) {
            //Occur failure(such as line disconnect)
        } else if(FD_ISSET(fd, &rdfds)) {
            RxBufLen = ReadTCP(fd, RxBuf, sizeof(RxBuf));

            if (RxBufLen < 0) {
                break;
            } else if (RxBufLen > 0) {
                Parse(DATA_ACCESS_PLATFORM, RxBuf, RxBufLen);
            }
        }
    }

    pthread_mutex_lock(&platform_tx_mutex);
    if (platform_tcp_fd > 0) {
        DisconnectTCP(platform_tcp_fd);
        platform_tcp_fd = -1;
    }
    pthread_mutex_unlock(&platform_tx_mutex);
    free(ptr);

    PlatformStatusChanged(PLATFORM_DISCONNECT_EVT, NULL, 0);
    pthread_exit(NULL);
}

int WritePlatform(const uint8_t * buf, uint32_t len)
{
    int ret = -1;

    pthread_mutex_lock(&platform_tx_mutex);
    if (platform_tcp_fd > 0) {
        ret = WriteTCP(platform_tcp_fd, buf, len);
    if (ctp != NULL && ctp->IsOpen()) {
        ret = ctp->WriteTCP(buf, len);
    }
    pthread_mutex_unlock(&platform_tx_mutex);

    return ret;
}