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

yy1717

2020-03-31 79f2500657c85c84aceed46ec00f9a703eae395b

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

 Documents/Ä£Äâ¼Ý¿¼APPÄÚ²¿Í¨Ñ¶.docx

Binary files differ

 lib/src/main/cpp/driver_test.cpp

@@ -429,7 +429,7 @@

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                    int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                    int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
                    int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight)
{
    DEBUG("加入车辆信息 pointNum %d", pointNum);

@@ -474,7 +474,9 @@
        }
    }

    CarModel->antPitch = -0.6;//antPitch;/////////////////////////////////////
    CarModel->antPitch = antPitch;
    CarModel->antHeight = antHeight;
    CarModel->groundHeight = groundHeight;

    CarModel->pointNum = pointNum;
    CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
@@ -730,7 +732,6 @@

static void ExecuteExam(const struct RtkTime* rtkTime)
{
//    DEBUG("ExecuteExam 111111111");
    {
        static const char *NAME[] = {"OBD_SPEED",
                                     "ENGINE_RPM",
@@ -754,7 +755,7 @@
                                     "CAR_STATUS_END"};

        static const char *VALUE[] = {
                "0",
                "INACTIVE",
                "HAZARD_LIGHTS",
                "LEFT_TURN_LIGHT",
                "RIGHT_TURN_LIGHT",
@@ -807,7 +808,6 @@
        }

    }
//DEBUG("-------------- 1111111111");

    if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
        if (engineRuning) {
@@ -864,13 +864,10 @@
            }
        }
    }

//    DEBUG("ExecuteExam 1111111111 ----------------------");
}

static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
//    DEBUG("ExecuteExam 2222222");
    if (move != 0) {
        if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
            DEBUG("不系安全带");
@@ -965,7 +962,6 @@
            CurrExamMapIndex = -1;
        }
    }
//    DEBUG("ExecuteExam 2222222 -----------------------");
}

static void EngineStartHold(union sigval sig) {
@@ -1226,10 +1222,10 @@
            Line carAxial;

            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
            MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
            MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]);

            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
                if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1)
                    return i;
            }
        }
@@ -1360,22 +1356,23 @@
 */
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
{
    carModel->basePoint = main_ant;


    // ä¿¯ä»°è§’修正
//    DEBUG("俯仰角 %f", pitch);
    carModel->yaw = azimuth;
    carModel->pitch = pitch;
    carModel->tm = *rtkTime;

    pitch = pitch - carModel->antPitch;
//    DEBUG("yaw = %f ä¿®æ­£ä¿¯ä»°è§’ %f", azimuth, pitch);

//    DEBUG("xz俯仰角 %f", pitch);
    // ä¸»å¤©çº¿æŠ•å½±ä¿®æ­£
    carModel->basePoint.X = main_ant.X + fabs(carModel->antHeight - carModel->groundHeight) * sin(toRadians(pitch)) * sin(toRadians(azimuth));
    carModel->basePoint.Y = main_ant.Y + fabs(carModel->antHeight - carModel->groundHeight) * sin(toRadians(pitch)) * cos(toRadians(azimuth));

    for (int i = 0; i < carModel->pointNum; ++i) {
        double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
        double qry =
                carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) /
                carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) *
                cos(toRadians(pitch));

        double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
@@ -1391,11 +1388,14 @@
        carModel->carXY[i].X =
                projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
                projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                main_ant.X;
                carModel->basePoint.X;
        carModel->carXY[i].Y =
                projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
                main_ant.Y;
                carModel->basePoint.Y;

//        DEBUG("<%d>. æ ‡è· %f æ ‡è§’ %f X = %f Y = %f", i, carModel->carDesc[i].distance, carModel->carDesc[i].angle,
//              carModel->carXY[i].X, carModel->carXY[i].Y);
    }
}


 lib/src/main/cpp/driver_test.h

@@ -52,6 +52,8 @@
    int *body;
    int pointNum;
    double antPitch;                // å‰åŽå¤©çº¿å› å®‰è£…位置之固有pitch角
    double antHeight;               // è½¦é¡¶å¤©çº¿é«˜ç¨‹
    double groundHeight;            // å¤§åœ°é«˜ç¨‹
    double yaw;
    double pitch;
    struct car_desc_ *carDesc;      // ç»çŽ°åœºæµ‹é‡ç‚¹è®¡ç®—后的值
@@ -134,7 +136,7 @@

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                        int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch);
                        int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight);

void StartDriverExam(int start, int type);
void StartMapExam(int map_id, int exam);

 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);
}

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

@@ -76,6 +76,7 @@
void MA_MainProcMsgEntry(int cmd, const char *value);
void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length);

void MA_Init(void);
void MA_NdkStart(void);
void MA_ReqRtkPlatformConfig(void);
void MA_RtkPlatformConnect(int conn, const char *ip, int port);

 lib/src/main/cpp/native-lib.cpp

@@ -26,7 +26,7 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};

const char *VIRTUAL_RTK_IP = "192.168.3.52";
const char *VIRTUAL_RTK_IP = "192.168.16.100";
const int VIRTUAL_RTK_PORT = 9001;

static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;
@@ -262,7 +262,7 @@
    ConfigMCU();
    DriverTestInit();
    ConfigRTKModule();

    MA_Init();
    InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);
    AppTimer_add(SendBootIndicate, 500);


 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -200,7 +200,7 @@
        }*/

        if (RxBufLen > 0) {
#if 1
#if 0
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));

 lib/src/main/cpp/test_items2/dummy_light.cpp

@@ -59,7 +59,7 @@
void DummyLightTTSDone(int id)
{
    // ç­‰è¯­éŸ³æ’­æŠ¥å®Œæ¯•åŽè®¡æ—¶
    if (id == examTtsSeq) {
    if (id == examTtsSeq && testing) {
        DEBUG("DummyLightTTSDone %d", id);
        for (int i = 0; i < contentNum; ++i) {
            if (content[i].itemStatus == TTS_DOING) {
@@ -74,6 +74,7 @@

void TerminateDummyLightExam(void)
{
    testing = false;
    AppTimer_delete(DummyLightCheckActive);
    AppTimer_delete(ExamDummyLight);
}