坡道坐标修正、发往主进程的消息线程独立

yy1717

2020-03-31 79f2500657c85c84aceed46ec00f9a703eae395b

 lib/src/main/cpp/master/comm_if.cpp

@@ -20,6 +20,8 @@
#include <string>
#include <iostream>
#include <vector>
#include <list>
#include <semaphore.h>

#define DEBUG(fmt, args...)     LOGD("<comm_if> <%s>: " fmt, __func__, ##args)

@@ -67,14 +69,80 @@

static int OnOff = 0;//0xFFFF;

struct msg_2_main_t {
    int cmd;
    string value;
};

static list<struct msg_2_main_t> MessageBuffer;

static sem_t sem_msg_income;
static pthread_mutex_t msg_mutex = PTHREAD_MUTEX_INITIALIZER;

static void SendMsgToMainProcIndep(int cmd, const char *value);
static void *SendMsgToMainProcThread(void *p);

static void SendMsgToMainProcIndep(int cmd, const char *value)
{
    struct msg_2_main_t msg;

    msg.cmd = cmd;

    if (value != NULL) {
        msg.value = value;
    } else {
        msg.value.clear();
    }

    pthread_mutex_lock(&msg_mutex);
    MessageBuffer.push_front(msg);
    pthread_mutex_unlock(&msg_mutex);

    sem_post(&sem_msg_income);
}

static void *SendMsgToMainProcThread(void *p) {
    while (true) {
        sem_wait(&sem_msg_income);

        if (MessageBuffer.size() > 0) {
            struct msg_2_main_t msg;

            pthread_mutex_lock(&msg_mutex);
            msg = MessageBuffer.back();
            MessageBuffer.pop_back();
            pthread_mutex_unlock(&msg_mutex);

            if (msg.value.length() > 0)
                SendMsgToMainProc(msg.cmd, msg.value.c_str());
            else
                SendMsgToMainProc(msg.cmd, NULL);
        }
    }
}

void MA_Init(void)
{
    sem_init(&sem_msg_income, 0, 0);
    MessageBuffer.clear();

    pthread_mutex_init(&msg_mutex, NULL);

    pthread_t pid;
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
    pthread_create(&pid, &attr, SendMsgToMainProcThread, NULL);
}

void MA_NdkStart(void)
{
    SendMsgToMainProc(ID_SM_NDK_START, NULL);
    SendMsgToMainProcIndep(ID_SM_NDK_START, NULL);
}

void MA_ReqRtkPlatformConfig(void)
{
    SendMsgToMainProc(ID_SM_REQ_RTK_PLAT_CFG, NULL);
    SendMsgToMainProcIndep(ID_SM_REQ_RTK_PLAT_CFG, NULL);
}

void MA_RtkPlatformConnect(int conn, const char *ip, int port)
@@ -92,7 +160,7 @@

    writer.EndObject();

    SendMsgToMainProc(ID_SM_RTK_PLAT_CONN, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_RTK_PLAT_CONN, sb.GetString());
}

void MA_RtkPlatformRegister(int reg, const uint8_t *pwd, int length)
@@ -114,7 +182,7 @@

    writer.EndObject();

    SendMsgToMainProc(ID_SM_RTK_PLAT_REG, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_RTK_PLAT_REG, sb.GetString());
}

void MA_RtkPlatformLogin(int login)
@@ -129,17 +197,17 @@

    writer.EndObject();

    SendMsgToMainProc(ID_SM_RTK_PLAT_LOGIN, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_RTK_PLAT_LOGIN, sb.GetString());
}

void MA_ReadMap(void)
{
    SendMsgToMainProc(ID_SM_READ_MAP, NULL);
    SendMsgToMainProcIndep(ID_SM_READ_MAP, NULL);
}

void MA_ReadCar(void)
{
    SendMsgToMainProc(ID_SM_READ_CAR, NULL);
    SendMsgToMainProcIndep(ID_SM_READ_CAR, NULL);
}

void MA_ReadSensor(void)
@@ -161,7 +229,7 @@

    writer.EndObject();

    SendMsgToMainProc(ID_SM_EXAM_STATUS, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_EXAM_STATUS, sb.GetString());
}

void MA_SendCardBrief(const struct cardBrief *brief)
@@ -176,7 +244,7 @@
    writer.String(brief->card);
    writer.EndObject();

    SendMsgToMainProc(ID_SM_PUT_CARD, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_PUT_CARD, sb.GetString());
}

void MA_SendMcuBrief(const struct mcuBrief *brief)
@@ -201,7 +269,7 @@

    writer.EndObject();

    SendMsgToMainProc(ID_SM_MCU_BRIEF, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_MCU_BRIEF, sb.GetString());
}

void MA_SendGpsBrief(const struct gpsBrief *brief)
@@ -231,7 +299,7 @@

    writer.EndObject();

    SendMsgToMainProc(ID_SM_GPS_BRIEF, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_GPS_BRIEF, sb.GetString());
}

void MA_SendRtkBrief(const struct rtkBrief *brief)
@@ -281,7 +349,7 @@

    writer.EndObject();

    SendMsgToMainProc(ID_SM_RTK_BRIEF, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_RTK_BRIEF, sb.GetString());
}

void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
@@ -306,7 +374,7 @@

    writer.EndArray();

    SendMsgToMainProc(ID_SM_EXAM_BRIEF, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_EXAM_BRIEF, sb.GetString());
}

void MA_SendCarPosition(const struct carBrief *brief)
@@ -383,7 +451,7 @@
    writer.EndArray();
    writer.EndObject();

    SendMsgToMainProc(ID_SM_CAR, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_CAR, sb.GetString());
}

void MA_SendRtcmInd(int length)
@@ -398,7 +466,7 @@
    writer.Int(length);
    writer.EndObject();

    SendMsgToMainProc(ID_SM_RTCM_IND, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_RTCM_IND, sb.GetString());
}

void MA_SendDebugInfo(const char *str, ...)
@@ -422,7 +490,7 @@
    writer.String(buffer);
    writer.EndObject();

    SendMsgToMainProc(ID_SM_DEBUG_INFO, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_DEBUG_INFO, sb.GetString());
}

void MA_MainProcMsgEntry(int cmd, const char *value)
@@ -744,6 +812,8 @@
                int *body = NULL;
                int pointNum = 0;
                double antPitch = 0;
                double antHeight = 0;
                double groundHeight = 0;

                double (*point)[2] = NULL;

@@ -842,8 +912,18 @@
                    antPitch = s.GetDouble();
                }

                if (doc.HasMember("ant_height")) {
                    const Value& s = doc["ant_height"];
                    antHeight = s.GetDouble();
                }

                if (doc.HasMember("ground_height")) {
                    const Value& s = doc["ground_height"];
                    groundHeight = s.GetDouble();
                }

                SetCarMeasurePoint(basePoint, axial, left_front_tire, right_front_tire,
                        left_rear_tire, right_rear_tire, body, bodyNum, point, pointNum, antPitch);
                        left_rear_tire, right_rear_tire, body, bodyNum, point, pointNum, antPitch, antHeight, groundHeight);

                if (body != NULL) delete []body;
                if (point != NULL) delete []point;
@@ -1011,10 +1091,10 @@
    writer.Int(enter);
    writer.EndObject();

    SendMsgToMainProc(ID_SM_ENTER_MAP, sb.GetString());
    SendMsgToMainProcIndep(ID_SM_ENTER_MAP, sb.GetString());
}

void MA_ExamLight(void)
{
    SendMsgToMainProc(ID_SM_LIGHT_EXAM_REQ, NULL);
    SendMsgToMainProcIndep(ID_SM_LIGHT_EXAM_REQ, NULL);
}