智慧驾培首次提交

yy1717

2022-12-08 f7a18ec4494b9c5c9ef3fd440bbf68ffc6425e18

智慧驾培首次提交

 lib/src/main/cpp/CMakeLists.txt

@@ -5,6 +5,9 @@

cmake_minimum_required(VERSION 3.4.1)

set(CMAKE_BUILD_TYPE "Release")
add_definitions("-O3")

# Creates and names a library, sets it as either STATIC
# or SHARED, and provides the relative paths to its source code.
# You can define multiple libraries, and CMake builds them for you.
@@ -21,12 +24,19 @@
        common/serial_port.cpp
        common/net.cpp
        common/apptimer.cpp
        common/crc32.cpp
        common/md5.cpp
        common/string_util.cpp
        common/xtea.cpp
        rtk_platform/parse_net.cpp
        rtk_platform/platform.cpp
        rtk_module/parse_gps.cpp
        test_common/Geometry.cpp
        driver_test.cpp
        mcu/mcu_if.cpp
        mcu/ahp.cpp
        mcu/ada.cpp
        mcu/dfu.cpp

        test_common/car_sensor.cpp
        test_common/odo_graph.cpp

 lib/src/main/cpp/common/apptimer.cpp

@@ -1,188 +1,262 @@
//POSIX.1b Timer
#include <jni.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>
#include <sys/time.h>
#include <pthread.h>
#include <errno.h>
#include <time.h>
#include "../jni_log.h"
//
// Created by YY on 2021/3/16.
//

#include "apptimer.h"
#include "../jni_log.h"

#define MAX_TIMER    32
#include <cstdint>
#include <functional>
#include <chrono>
#include <mutex>
#include <condition_variable>
#include <list>
#include <thread>
#include <map>
#include <string>
#include <sstream>
#include <iomanip>

static struct {
    timer_t timerId;
#define DEBUG(fmt, args...)     LOGD("<apptimer> <%s>: " fmt, __func__, ##args)

    void (*func)(union sigval sig);
std::mutex mtx;

    int value;
    uint8_t *user_data;
} AppTimer[MAX_TIMER];
std::map<void(*)(apptimer_var_t), CTimer *> TimerList;

static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
static void RemoveTimerList(const CTimer *p);

static int createTimer(timer_t *timerId, void (*func)(union sigval sig),
                       int value,
                       uint8_t *usr_data, int usr_data_length,
                       uint8_t **usr_data_ptr) {
    struct sigevent sev;
    pthread_attr_t attr;
CTimer::CTimer() {
    flag = 0;
    var.var_ptr = nullptr;
    var.var1 = var.var2 = var.var_length = 0;
    pthread = nullptr;
}

    // Register printMsg to SIGALRM
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
CTimer::~CTimer() {
    if (pthread != nullptr) {
        delete pthread;
    }
    if (var.var_ptr != nullptr) {
        delete var.var_ptr;
    }
}

    memset(&sev, 0, sizeof(sev));
bool CTimer::_updateStatus(CTimer *timer) {
    if (timer->flag == 0) {
        return false;
    } else {
        return true;    //cancel
    }
}

    sev.sigev_notify = SIGEV_THREAD;
    sev.sigev_notify_function = func;
    sev.sigev_notify_attributes = &attr;
void CTimer::_do_task_func(uint32_t msec, CTimer *timer, std::function<void(apptimer_var_t)> callback) {
    std::unique_lock<std::mutex> lk(timer->cv_mtx);

    if (usr_data != NULL && usr_data_length != 0) {
        if ((sev.sigev_value.sival_ptr = malloc(usr_data_length)) != NULL) {
            *usr_data_ptr = (uint8_t *) sev.sigev_value.sival_ptr;
            memcpy(sev.sigev_value.sival_ptr, usr_data, usr_data_length);            //Copy usr data
            sev.sigev_value.sival_int = usr_data_length;
        } else {
            return -1;
    auto now = std::chrono::steady_clock::now();

    if (timer->cv.wait_until(lk, now + std::chrono::milliseconds(msec), std::bind(_updateStatus, timer)) == false) {
        RemoveTimerList(timer);             // 从列表中删除索引

        if (timer->flag == 0) {
            callback(timer->var);
        }
    } else {
        sev.sigev_value.sival_int = value;
//        LOGD("Cancel %d", static_cast<int>(timer->flag));
        RemoveTimerList(timer);
    }

    /* create timer */
    if (timer_create(CLOCK_REALTIME, &sev, timerId) == -1) {
        return -1;
    //delete timer;
    lk.unlock();
    std::thread(clear, timer).detach();
}

void CTimer::copy(int value1, int value2, const void *data, int length)
{
    this->var.var1 = value1;
    this->var.var2 = value2;
    this->var.var_length = length;
    if (length > 0 && data != nullptr) {
        this->var.var_ptr = new uint8_t[length];
        memcpy(this->var.var_ptr, data, length);
    } else {
        this->var.var_ptr = nullptr;
    }
//    LOGD("timer_create\n");
    return 0;
}

static int setTimer(timer_t timerId, int timeMSec) {
    struct itimerspec its;

    /* Start the timer */
    its.it_value.tv_sec = timeMSec / 1000;
    its.it_value.tv_nsec = (timeMSec % 1000) * 1000000;

    its.it_interval.tv_sec = 0;
    its.it_interval.tv_nsec = 0;

    if (timer_settime(timerId, 0, &its, NULL) == -1) {
        return -1;
void CTimer::clear(CTimer *timer)
{
    if (timer != nullptr) {
        delete timer;
    }
//    LOGD("timer_settime\n");
    return 0;
}

void AppTimer_Init(void) {
    memset(AppTimer, 0, sizeof(AppTimer));
    pthread_mutex_init(&mutex, NULL);
bool CTimer::start(uint32_t msec, std::function<void(apptimer_var_t)> callback) {
    if (pthread == nullptr) {
        try {
            pthread = new std::thread(_do_task_func, msec, this, callback);
            pthread->detach();
        } catch (std::exception &ex) {
            DEBUG("%s", ex.what());
            return false;
        }
//        this->callbackThread = std::thread(_do_task_func, msec, this, callback);
//        this->callbackThread.detach();
    }
    return true;
}

void AppTimer_add(void (*func)(union sigval), int timeMS) {
    int i;
//    LOGD("AppTimer_add\n");
    pthread_mutex_lock(&mutex);
    for (i = 0; i < MAX_TIMER; i++) {
        if (AppTimer[i].func == NULL) {
            if (createTimer(&(AppTimer[i].timerId), func, 0, NULL, 0, &(AppTimer[i].user_data)) == 0) {
                AppTimer[i].func = func;
void CTimer::stop() {
    std::unique_lock<std::mutex> lk(this->cv_mtx);
    this->flag = 1;
    this->cv.notify_one();
}

                if (setTimer(AppTimer[i].timerId, timeMS) !=
                    0) {            //Set timer fail, delele it
                    timer_delete(AppTimer[i].timerId);
                    if (AppTimer[i].user_data != NULL) {
                        free(AppTimer[i].user_data);
                        AppTimer[i].user_data = NULL;
                    }
                    AppTimer[i].func = NULL;
                }
            }
void AppTimer_init(void)
{
    TimerList.clear();
}

void AppTimer_add(void(*cb)(apptimer_var_t), uint32_t msec)
{
    CTimer *cTimer = new CTimer();
    cTimer->copy(0, 0, nullptr, 0);

    std::lock_guard<std::mutex> lock(mtx);
    TimerList.insert(std::pair<void (*)(apptimer_var_t), CTimer *>(cb, cTimer));

    if (cTimer->start(msec, cb) == false) {
        auto it = TimerList.find(cb);
        if (it != TimerList.end()) {
            CTimer *ptr = it->second;
            TimerList.erase(it);
        }
        delete cTimer;
    }
}

void AppTimer_add(void(*cb)(apptimer_var_t), uint32_t msec, int value1, int value2)
{
    CTimer *cTimer = new CTimer();
    cTimer->copy(value1, value2, nullptr, 0);

    std::lock_guard<std::mutex> lock(mtx);
    TimerList.insert(std::pair<void (*)(apptimer_var_t), CTimer *>(cb, cTimer));

    if (cTimer->start(msec, cb) == false) {
        LOGD("建立失败");
        auto it = TimerList.find(cb);
        if (it != TimerList.end()) {
            CTimer *ptr = it->second;
            TimerList.erase(it);
        }
        delete cTimer;
    }
}

void AppTimer_add(void(*cb)(apptimer_var_t), uint32_t msec, void *data, int lenght)
{
    CTimer *cTimer = new CTimer();
    cTimer->copy(0, 0, data, lenght);

    std::lock_guard<std::mutex> lock(mtx);
    TimerList.insert(std::pair<void (*)(apptimer_var_t), CTimer *>(cb, cTimer));

    if (cTimer->start(msec, cb) == false) {
        auto it = TimerList.find(cb);
        if (it != TimerList.end()) {
            CTimer *ptr = it->second;
            TimerList.erase(it);
        }
        delete cTimer;
    }
}

void AppTimer_delete(void(*cb)(apptimer_var_t))
{
    std::lock_guard<std::mutex> lock(mtx);

    auto it = TimerList.find(cb);
    if (it != TimerList.end()) {
        CTimer *ptr = it->second;
        TimerList.erase(it);
        ptr->stop();
    }
}

static void RemoveTimerList(const CTimer *p)
{
    std::lock_guard<std::mutex> lock(mtx);

    for (auto it = TimerList.begin(); it != TimerList.end(); ++it) {
        CTimer *ptr = it->second;
        if (ptr == p) {
            TimerList.erase(it);
            break;
        }
    }
    pthread_mutex_unlock(&mutex);
}

void AppTimer_add(void (*func)(union sigval), int timeMS, uint8_t *data, int length) {
    int i;
//    LOGD("AppTimer_add\n");
    pthread_mutex_lock(&mutex);
    for (i = 0; i < MAX_TIMER; i++) {
        if (AppTimer[i].func == NULL) {
            if (createTimer(&(AppTimer[i].timerId), func, 0, data, length, &(AppTimer[i].user_data)) == 0) {
                AppTimer[i].func = func;

                if (setTimer(AppTimer[i].timerId, timeMS) !=
                    0) {            //Set timer fail, delele it
                    timer_delete(AppTimer[i].timerId);
                    if (AppTimer[i].user_data != NULL) {
                        free(AppTimer[i].user_data);
                        AppTimer[i].user_data = NULL;
                    }
                    AppTimer[i].func = NULL;
                }
            }
            break;
        }
    }
    pthread_mutex_unlock(&mutex);
}

void AppTimer_add(void (*func)(union sigval), int timeMS, int value) {
    int i;
//    LOGD("AppTimer_add\n");
    pthread_mutex_lock(&mutex);
    for (i = 0; i < MAX_TIMER; i++) {
        if (AppTimer[i].func == NULL) {
            if (createTimer(&(AppTimer[i].timerId), func, value, NULL, 0, &(AppTimer[i].user_data)) == 0) {
                AppTimer[i].func = func;

                if (setTimer(AppTimer[i].timerId, timeMS) !=
                    0) {            //Set timer fail, delele it
                    timer_delete(AppTimer[i].timerId);
                    if (AppTimer[i].user_data != NULL) {
                        free(AppTimer[i].user_data);
                        AppTimer[i].user_data = NULL;
                    }
                    AppTimer[i].func = NULL;
                }
            }
            break;
        }
    }
    pthread_mutex_unlock(&mutex);
}

void AppTimer_delete(void (*func)(union sigval)) {
    int i;
//    LOGD("AppTimer_delete\n");
    pthread_mutex_lock(&mutex);
    for (i = 0; i < MAX_TIMER; i++) {
        if (AppTimer[i].func == func) {
            timer_delete(AppTimer[i].timerId);
            if (AppTimer[i].user_data != NULL) {
                free(AppTimer[i].user_data);
                AppTimer[i].user_data = NULL;
            }
            AppTimer[i].func = NULL;
        }
    }
    pthread_mutex_unlock(&mutex);
}

uint32_t AppTimer_GetTickCount(void)
{
    struct timespec ts;
    std::chrono::milliseconds as = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now().time_since_epoch());

    clock_gettime(CLOCK_MONOTONIC, &ts);
    return as.count();
}

    return (ts.tv_sec * 1000 + ts.tv_nsec / 1000000);
uint32_t AppTimer_GetGmtTickCount(void)
{
    std::chrono::seconds as = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch());

    return as.count();
}

std::string FormatTime(const char *fmt)
{
    auto now = std::chrono::system_clock::now();
    auto timet = std::chrono::system_clock::to_time_t(now);
    auto localTime = *std::localtime(&timet);

    std::stringstream ss;
    std::string str;
    ss << std::put_time(&localTime, "%Y-%m-%d %H:%M:%S");
    str = ss.str();
    return str;
}

std::time_t MktimeString(const char *str, const char *fmt)
{
    struct std::tm dt;
    std::istringstream ss(str);

    ss >> std::get_time(&dt, fmt);

    std::stringstream out;
    out << std::put_time(&dt, "%Y-%m-%d %H:%M:%S");
//    DEBUG("%s, %ld", out.str().c_str(), std::mktime(&dt));

    time_t utc = std::mktime(&dt);

    return utc;
}

time_t TimeStamp(int year, int mon, int day, int hour, int min, int sec)
{
    // YYHHDDhhmmss(GMT+8)
    struct tm test_tm;

    struct timeval tv;
    struct timezone tz;

    gettimeofday(&tv,&tz);

    memset(&test_tm, 0, sizeof(test_tm));

    test_tm.tm_year = year - 1900;
    test_tm.tm_mon = mon - 1;
    test_tm.tm_mday = day;
    test_tm.tm_hour = hour;
    test_tm.tm_min = min;
    test_tm.tm_sec = sec;

    return mktime(&test_tm) - tz.tz_minuteswest*60;
}

 lib/src/main/cpp/common/apptimer.h

@@ -1,23 +1,69 @@
#ifndef _APPTIMER_H_
#define _APPTIMER_H_
//
// Created by YY on 2021/3/16.
//

#include <stdint.h>
#include <signal.h>
#ifndef NDKADV_APPTIMER_H
#define NDKADV_APPTIMER_H

#include <cstdint>
#include <functional>
#include <thread>
#include <chrono>
#include <atomic>
#include <mutex>
#include <condition_variable>
#include <string>
#include <ctime>

#define D_SEC(n)        ((n)*1000UL)
#define D_MIN(n)        ((n)*1000UL*60UL)
#define D_HOUR(n)        ((n)*1000UL*60UL*60UL)

extern void AppTimer_Init(void);
typedef struct {
    int var1;
    int var2;
    void * var_ptr;
    int var_length;
} apptimer_var_t;

extern void AppTimer_add(void (*func)(union sigval), int timeMS);
class CTimer {
public:
    CTimer();
    virtual ~CTimer();
    static bool _updateStatus(CTimer *timer);
    static void _do_task_func(uint32_t msec, CTimer *timer, std::function<void(apptimer_var_t)> callback);
    bool start(uint32_t msec, std::function<void(apptimer_var_t)> callback);
    void stop();
    void copy(int value1, int value2, const void *data, int length);
    static void clear(CTimer *timer);
private:
    std::mutex cv_mtx;
    std::condition_variable cv;
//    std::thread callbackThread;
    std::thread *pthread;
    apptimer_var_t var;
public:
    std::atomic_int flag;
};

extern void AppTimer_add(void (*func)(union sigval), int timeMS, uint8_t *data, int length);
void AppTimer_init(void);

extern void AppTimer_add(void (*func)(union sigval), int timeMS, int value);
void AppTimer_add(void(*cb)(apptimer_var_t), uint32_t msec);

extern void AppTimer_delete(void (*func)(union sigval));
void AppTimer_add(void(*cb)(apptimer_var_t), uint32_t msec, int value1, int value2 = 0);

extern uint32_t AppTimer_GetTickCount(void);
void AppTimer_add(void(*cb)(apptimer_var_t), uint32_t msec, void *data, int lenght);

#endif
void AppTimer_delete(void (*cb) (apptimer_var_t));

uint32_t AppTimer_GetTickCount(void);

uint32_t AppTimer_GetGmtTickCount(void);

std::string FormatTime(const char *fmt);

std::time_t MktimeString(const char *str, const char *fmt);

time_t TimeStamp(int year, int mon, int day, int hour, int min, int sec);

#endif //NDKADV_APPTIMER_H

 lib/src/main/cpp/common/net.cpp

@@ -12,7 +12,7 @@
#include <sys/select.h>
#include <arpa/inet.h>
#include <regex>
#include <pthread.h>
#include <thread>
#include <semaphore.h>
#include <string>

@@ -70,12 +70,7 @@
    m_sIp = ip;
    m_nPort = port;

    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, TcpConnectThread, this);

    std::thread(TcpConnectThread, this).detach();
    return true;
}

@@ -317,7 +312,7 @@
    return( -1 );
}

void *CTcpPort::TcpConnectThread(void *p)
void CTcpPort::TcpConnectThread(void *p)
{
    CTcpPort *pCTcpPort = (CTcpPort *)p;

@@ -325,7 +320,7 @@

    if (pCTcpPort->is_domain_name(pCTcpPort->m_sIp.c_str())) {
        if (pCTcpPort->GetHostIP(pCTcpPort->m_sIp.c_str(), net_addr) != 0) {
            goto CONNECT_END;
            return;
        }
    } else {
        strcpy(net_addr, pCTcpPort->m_sIp.c_str());
@@ -338,22 +333,15 @@
            pCTcpPort->event_func(0, pCTcpPort, pCTcpPort->event_func_context);
        }

        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, TcpListenThread, p);
        std::thread(TcpListenThread, p).detach();
    } else {
        if (pCTcpPort->event_func != NULL) {
            pCTcpPort->event_func(-1, pCTcpPort, pCTcpPort->event_func_context);
        }
    }

CONNECT_END:
    pthread_exit(NULL);
}

void *CTcpPort::TcpListenThread(void *p)
void CTcpPort::TcpListenThread(void *p)
{
    int fds_ret;
    struct timeval tv;
@@ -403,8 +391,6 @@
    if (pCTcpPort->event_func) {
        pCTcpPort->event_func(-1, pCTcpPort, pCTcpPort->event_func_context);
    }

    pthread_exit(NULL);
}

void CTcpPort::set_event_callback(void (*callback)(int, void *, void *), void *context)

 lib/src/main/cpp/common/net.h

@@ -45,8 +45,8 @@
    void(*receive_data_func)(void *, int, void *, void *);    //数据回调函数指针
    void *receive_data_func_context;    //数据回调函数上下文

    static void *TcpConnectThread(void *p);
    static void *TcpListenThread(void *p);
    static void TcpConnectThread(void *p);
    static void TcpListenThread(void *p);
};

//int WriteTCP(int fd, const uint8_t * buf, uint32_t len);

 lib/src/main/cpp/common/serial_port.cpp

@@ -4,7 +4,7 @@

#include <jni.h>
#include <string>

#include <mutex>
#include <sys/stat.h>
#include <stdbool.h>
#include <stdint.h>
@@ -15,16 +15,22 @@
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <pthread.h>
#include "../jni_log.h"
#include "serial_port.h"

using namespace std;

static volatile int serial_port_fd[2] = {0, 0};
static pthread_mutex_t mutex[2] = {PTHREAD_MUTEX_INITIALIZER, PTHREAD_MUTEX_INITIALIZER};
SerialPort::SerialPort(struct serial_config cfg)
{
    this->cfg = cfg;
}

int setRTS(int fd, int level)
SerialPort::~SerialPort()
{
    UninitSerialPort();
}

int SerialPort::setRTS(int level)
{
    int status;

@@ -47,7 +53,7 @@
/*********************************************************************
 * PUBLIC FUNCTIONS
 */
static int SetSerialPort(int fd, int speed, int databits, char parity, int stopbits, int flowctrl) {
int SerialPort::SetSerialPort(int fd, int speed, int databits, char parity, int stopbits, int flowctrl) {
    int status = 0;
    struct termios opt;
    int speed_arr[] = {B921600, B576000, B500000, B460800, B230400, B115200, B38400, B19200,
@@ -142,7 +148,7 @@
    return status;
}

static int OpenSerialPort(const char *name) {
int SerialPort::OpenSerialPort(const char *name) {
    int uart_fd;

//    uart_fd = open(name, O_RDWR /*| O_NONBLOCK/*| O_NOCTTY | O_NDELAY*/);
@@ -151,14 +157,13 @@
    return uart_fd;
}

static void CloseSerialPort(int fd) {
void SerialPort::CloseSerialPort(int fd) {
    close(fd);
}

int WriteSerialPort(int id, const void *buf, int len) {
int SerialPort::WriteSerialPort(const void *buf, int len) {
    int ret = -1;
    int fds_ret;
    int fd = GetSerialPort(id);

    struct timeval tv;
    fd_set wrfds;
@@ -173,7 +178,7 @@
    FD_ZERO(&wrfds); //clean
    FD_SET(fd, &wrfds); //set

    pthread_mutex_lock(&mutex[id]);
    lock_guard<std::mutex> lock(mtx);

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

@@ -194,113 +199,66 @@
        LOGE("Serial Port error 2\n");
    }

    pthread_mutex_unlock(&mutex[id]);

    return ret;
}

int GetSerialPort(int id) {
    return serial_port_fd[id];
}

int InitSerialPort(int id, int baud, int dataBits, char parity, int stopBits, int flowctrl)
int SerialPort::InitSerialPort(void)
{
    char name[32];
//    char name[32];
//
//    if (id == UART_0) {
//        strcpy(name, "/dev/ttyCH341USB5");
//    } else if (id == UART_1) {
//        strcpy(name, "/dev/ttyCH341USB6");
//    } else {
//        return -1;
//    }

    if (id == UART_0) {
        strcpy(name, "/dev/ttyHSL0");
    } else if (id == UART_1) {
        strcpy(name, "/dev/ttyHSL1");
    } else {
    fd = OpenSerialPort(cfg.name);
    if (fd <= 0) {
        return -1;
    }

    UninitSerialPort(id);

    serial_port_fd[id] = OpenSerialPort(name);
    if (serial_port_fd[id] <= 0) {
        return -1;
    }

    if (SetSerialPort(serial_port_fd[id], baud, dataBits, parity, stopBits, flowctrl) != 0) {
    if (SetSerialPort(fd, cfg.baud, cfg.data_bit, cfg.verify_bit, cfg.stop_bit, cfg.flow_ctrl) != 0) {
        return -2;
    }
    pthread_mutex_init(&mutex[id], NULL);

    return 0;
}

void UninitSerialPort(int id)
void SerialPort::UninitSerialPort(void)
{
    if (serial_port_fd[id] > 0) {
        CloseSerialPort(serial_port_fd[id]);
        pthread_mutex_destroy(&mutex[id]);
        serial_port_fd[id] = 0;
    if (fd > 0) {
        CloseSerialPort(fd);
        fd = 0;
    }
}

int ReadSerialPort(int id, uint8_t *out, uint16_t length)
int SerialPort::ReadSerialPort(uint8_t *out, uint16_t length)
{
    if (serial_port_fd[id] <= 0) return 0;
    return read(serial_port_fd[id], out, length);
}
    if (fd <= 0) return 0;

//extern "C"
//JNIEXPORT jint JNICALL
//Java_com_example_yy_jnicallback_MyService_InitSerialPort(
//        JNIEnv *env,
//        jobject /* this */,
//        jstring name,
//        jint baud,
//        jint dataBits,
//        jbyte parity,
//        jint stopBits) {
//
//    const char *s = env->GetStringUTFChars(name, 0);
//    char item_value[128];
//    strcpy(item_value, s);
//    env->ReleaseStringUTFChars(name, s);
//    LOGD("serial port = %s", item_value);
//
//    if (serial_port_fd > 0) {
//        CloseSerialPort(serial_port_fd);
//        pthread_mutex_destroy(&mutex);
//        serial_port_fd = 0;
//    }
//
//    serial_port_fd = OpenSerialPort(item_value);
//    if (serial_port_fd <= 0) {
//        return -1;
//    }
//
//    if (SetSerialPort(serial_port_fd, baud, dataBits, parity, stopBits) != 0) {
//        return -2;
//    }
//    pthread_mutex_init(&mutex, NULL);
//    return 0;
//}
//
//extern "C"
//JNIEXPORT void JNICALL
//Java_com_example_yy_jnicallback_MyService_MonitSerialPort(
//        JNIEnv *env,
//        jobject /* this */) {
//    if (serial_port_fd > 0) {
//        uint8_t UartRxBuf[4096];
//
///*        uint8_t pkt[] = {0x7E, 0x80, 0x02, 0x00, 0x00, 0x26, 0x00, 0x00, 0x01, 0x38, 0x20, 0x20,
//                         0x55, 0x45, 0x04, 0x4E,
//                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x02, 0x61, 0x63,
//                         0x0C, 0x06, 0xEA, 0x04,
//                         0xFE, 0x00, 0x00, 0x01, 0x63, 0x00, 0xB4, 0x13, 0x03, 0x05, 0x18, 0x18,
//                         0x52, 0x01, 0x04, 0x00,
//                         0x00, 0x00, 0x00, 0x05, 0x02, 0x00, 0x00, 0x4D, 0x7E};
//        WriteSerialPort(serial_port_fd, pkt, sizeof(pkt));*/
//
//        int length = read(serial_port_fd, UartRxBuf, sizeof(UartRxBuf));
//
//        if (length > 0) {
//            Parse(DATA_ACCESS_MCU, UartRxBuf, length);
//        }
//    }
//}
    struct timeval tv;
    fd_set wrfds;

    tv.tv_sec = 5;
    tv.tv_usec = 0;

    FD_ZERO(&wrfds); //clean
    FD_SET(fd, &wrfds); //set

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

    if (fds_ret < 0) {
        return -1;
    }
    else if(fds_ret == 0) {
        // timeout
        return -2;
    }
    else if(FD_ISSET(fd, &wrfds)) {
        return read(fd, out, length);
    }

    return -3;
}

 lib/src/main/cpp/common/serial_port.h

@@ -6,12 +6,10 @@
#define JNICALLBACK_SERIAL_PORT_H

#include <stdint.h>

#define UART_0            0
#define UART_1            1
#include <mutex>

struct serial_config {
    char name[32];
    char name[64];
    int baud;
    int data_bit;
    char verify_bit;
@@ -19,12 +17,24 @@
    int flow_ctrl;
};

int InitSerialPort(int id, int baud, int dataBits, char parity, int stopBits, int flowctrl);
void UninitSerialPort(int id);
class SerialPort {
private:
    int fd = -1;
    std::mutex mtx;
    struct serial_config cfg;
    int setRTS(int level);
    int SetSerialPort(int fd, int speed, int databits, char parity, int stopbits, int flowctrl);
    int OpenSerialPort(const char *name);
    void CloseSerialPort(int fd);
    void UninitSerialPort(void);
public:
    SerialPort(struct serial_config cfg);
    virtual ~SerialPort();
    int InitSerialPort(void);
    int WriteSerialPort(const void *buf, int len);
    int ReadSerialPort(uint8_t *out, uint16_t length);
};

int GetSerialPort(int id);
int ReadSerialPort(int id, uint8_t *out, uint16_t length);
int WriteSerialPort(int id, const void *buf, int len);
int setRTS(int fd, int level);
typedef int (SerialPort::*pSerialPortClassFun)(const void *buf, int len);

#endif //JNICALLBACK_SERIAL_PORT_H

 lib/src/main/cpp/driver_test.cpp

@@ -8,7 +8,7 @@
#include <cstdlib>
#include <cmath>
#include <semaphore.h>
#include <pthread.h>
#include <mutex>
#include <cstring>
#include <vector>
#include <list>
@@ -158,19 +158,22 @@
static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;

static pthread_mutex_t clock_mutex = PTHREAD_MUTEX_INITIALIZER;
static std::mutex clock_mutex;
static struct RtkTime rtkClock;

static void SetExamParamDefault(void);
static void EngineStartHold(union sigval sig);
static void EngineStartHold(apptimer_var_t val);
static void ExecuteExam(const struct RtkTime* rtkTime);
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
static void ReadDriverExamPrimerTimeout(apptimer_var_t val);
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);

static void PrintObdInfo(struct RtkTime *rtkTime, double speed);


static void ClockGener(apptimer_var_t val);

void DriverTestInit(void)
{
@@ -195,7 +198,7 @@
    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;

    pthread_mutex_init(&clock_mutex, NULL);
//    AppTimer_add(ClockGener, 200);            // App自己产生定时节拍
}

static void SetExamParamDefault(void)
@@ -293,10 +296,8 @@
    examParam.crossing_turn_unknown_tts = DEFAULT_CROSSING_TURN_UNKNOWN_TTS;
}

static void ReadDriverExamPrimerTimeout(union sigval sig)
static void ReadDriverExamPrimerTimeout(apptimer_var_t val)
{
    AppTimer_delete(ReadDriverExamPrimerTimeout);
    AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
    ReadDriverExamPrimer();
}

@@ -669,9 +670,42 @@
    }
}

static void ClockGener(apptimer_var_t val)
{
    rtk_info rtk;

    rtk.qf = 3;
    rtk.heading = 0;
    rtk.pitch = 0;
    rtk.roll = 0;
    rtk.x = 0;
    rtk.y = 0;

    struct timeval    tv;
    struct timezone tz;

    gettimeofday(&tv, &tz);

    struct tm *pTime = localtime(&tv.tv_sec);

    rtk.YY = (pTime->tm_year + 1900) % 100;
    rtk.MM = pTime->tm_mon + 1;
    rtk.DD = pTime->tm_mday;
    rtk.hh = pTime->tm_hour;
    rtk.mm = pTime->tm_min;
    rtk.ss = pTime->tm_sec;
    rtk.dss = tv.tv_usec / 1000;

//    DEBUG("模拟时间 ：%d-%d-%d %d:%d:%d.%d", rtk.YY, 1+pTime->tm_mon, pTime->tm_mday, pTime->tm_hour, pTime->tm_min, pTime->tm_sec, rtk.dss);

    UpdateRTKInfo(&rtk);

    AppTimer_add(ClockGener, 200);
}

void UpdateRTKInfo(const rtk_info *s)
{
    pthread_mutex_lock(&clock_mutex);
    std::unique_lock<std::mutex> lk(clock_mutex);
    rtkClock.YY = s->YY;
    rtkClock.MM = s->MM;
    rtkClock.DD = s->DD;
@@ -679,7 +713,7 @@
    rtkClock.mm = s->mm;
    rtkClock.ss = s->ss;
    rtkClock.mss = s->dss;
    pthread_mutex_unlock(&clock_mutex);
    lk.unlock();

    if (ExamStart) {
        ExecuteExam(&rtkClock);     // 执行无需车辆定位的项目
@@ -969,15 +1003,14 @@
    }
}

static void EngineStartHold(union sigval sig) {
    AppTimer_delete(EngineStartHold);
static void EngineStartHold(apptimer_var_t val) {
    DEBUG("点火超时");
    if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
        struct RtkTime rtkTime;

        pthread_mutex_lock(&clock_mutex);
        std::unique_lock<std::mutex> lk(clock_mutex);
        rtkTime = rtkClock;
        pthread_mutex_unlock(&clock_mutex);
        lk.unlock();

        // 不及时松开启动开关，扣10分
        if (ExamType == TEST_TYPE_AREA) {
@@ -1316,7 +1349,7 @@
{
    // 关机
    if (event == 1) {
        ShutdownInd(timeout);

    }
    // 重启
    if (event == 0) {
@@ -1337,10 +1370,9 @@
    if (res != 0) {
        struct RtkTime rtkTime;

        pthread_mutex_lock(&clock_mutex);
        std::unique_lock<std::mutex> lk(clock_mutex);
        rtkTime = rtkClock;
        pthread_mutex_unlock(&clock_mutex);

        lk.unlock();
        AddExamFault(40101, &rtkTime);
    }
    exam_dummy_light = 2;

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

@@ -21,6 +21,8 @@
#include <iostream>
#include <vector>
#include <list>
#include <thread>
#include <mutex>
#include <semaphore.h>
#include <unistd.h>

@@ -94,10 +96,10 @@
static list<struct msg_2_main_t> MessageBuffer;

static sem_t sem_msg_income;
static pthread_mutex_t msg_mutex = PTHREAD_MUTEX_INITIALIZER;
static std::mutex msg_mutex;

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

static void SendMsgToMainProcIndep(int cmd, const char *value)
{
@@ -111,14 +113,14 @@
        msg.value.clear();
    }

    pthread_mutex_lock(&msg_mutex);
    lock_guard<mutex> lock(msg_mutex);

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

    sem_post(&sem_msg_income);
}

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

@@ -126,9 +128,9 @@
            struct msg_2_main_t msg;
            int success;

            pthread_mutex_lock(&msg_mutex);
            unique_lock<mutex> lock(msg_mutex);
            msg = MessageBuffer.back();
            pthread_mutex_unlock(&msg_mutex);
            lock.unlock();

            if (msg.value.length() > 0)
                success = SendMsgToMainProc(msg.cmd, msg.value.c_str());
@@ -136,9 +138,9 @@
                success = SendMsgToMainProc(msg.cmd, NULL);

            if (success == 0) {
                pthread_mutex_lock(&msg_mutex);
                lock.lock();
                MessageBuffer.pop_back();
                pthread_mutex_unlock(&msg_mutex);
                lock.unlock();
            } else {
                // 延迟重发
                DEBUG("发送失败");
@@ -154,13 +156,7 @@
    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);
    std::thread(SendMsgToMainProcThread).detach();
}

void MA_NdkStart(void)
@@ -2057,7 +2053,7 @@
{
    switch (cmd) {
        case ID_MS_FILE: {
            UploadDfuFile(value, length);
            ////////////////////UploadDfuFile(value, length);
            break;
        }
        default:

 lib/src/main/cpp/mcu/ada.cpp

New file
@@ -0,0 +1,160 @@
//
// Created by Administrator on 2022/11/15.
//

#include <thread>
#include "ada.h"
#include "../common/string_util.h"
#include "../common/serial_port.h"
#include "mcu_if.h"
#include "dfu.h"
#include "../jni_log.h"
#include "../defs.h"

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

static Dfu *dfu = nullptr;
static SerialPort *pCls = nullptr;

static void InquiryAdaInfo(void)
{
    SendMcuCommand(pCls, &SerialPort::WriteSerialPort, ID_ADA_INFO_REQ, nullptr, 0);
}

static void AdaCommandEntry(uint16_t id, const uint8_t *data, int length)
{
    DEBUG("McuCommandEntry 0x%02X", id);
    switch (id) {
        case ID_ADA_INFO_RESP: {
            DEBUG("ID_ADA_INFO_RESP");
            ada_info_t info;

            info.version = StringUtil::BuffertoString(data, 8);
            info.sn = StringUtil::BcdBuffertoString(data + 8, 8);
            break;
        }
        case ID_ADA_DFU_RESP: {
            if (dfu != nullptr) {
                dfu->RemoteAck(data, length);
            }
            break;
        }
        case ID_ADA_OBD: {
//            DEBUG("ID_ADA_OBD %d %d", length, sizeof(car_sensor_t));
            if (length == sizeof(car_sensor_t)) {
                car_sensor_t *ptr = (car_sensor_t *) data;
            }
            break;
        }
        default:break;
    }
}

#define PARSE_BUFF_SIZE         4096

static void UartThread1(void *p) {
    struct serial_config *cfg = (struct serial_config *) p;
    SerialPort serialPort(*cfg);

    int res = serialPort.InitSerialPort();
    DEBUG("Serial %s open %d", cfg->name, res);
    uint8_t RxBuf[PARSE_BUFF_SIZE];
    int RxBufLen = 0;

    ParseUart parse(AdaCommandEntry);

    while (res == 0) {
        int ul = serialPort.ReadSerialPort((uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
        if (ul < 0) {
            continue;
        } else if (ul == 0){
            // usb串口断开
            break;
        }
        RxBufLen += ul;

        /*{
            static char buffd[16384];

            buffd[0] = 0;
            int i = 0;
            for (i = 0; i < ul; i++) {
                if ((i % 32) == 0) {
                    sprintf(buffd + strlen(buffd), "\n");
                }
                sprintf(buffd + strlen(buffd), "%02X ", RxBuf[i]);
                if (strlen(buffd) > 800) {
                    DEBUG("%s <- %s...", "UART", buffd);
                    buffd[0] = 0;
                }
            }
            if (strlen(buffd) > 0)
                DEBUG("%s <- %s", "UART", buffd);
        }*/

        if (RxBufLen > 0) {
            parse.ParseMcu(RxBuf, RxBufLen);
            RxBufLen = 0;
        }
    }
}

static void AdaRun(void)
{
    static struct serial_config serialConfig;

    strcpy(serialConfig.name, "/dev/ttyCH341USB4");
    serialConfig.baud = 115200;
    serialConfig.data_bit = 8;
    serialConfig.verify_bit = 'N';
    serialConfig.stop_bit = 1;
    serialConfig.flow_ctrl = 0;

    while (true) {
        std::thread t(UartThread1, &serialConfig);
        t.join();
        std::this_thread::sleep_for(std::chrono::seconds(3));
    }
}

void InitAda(void)
{
    // TODO
    std::thread(AdaRun).detach();
}

static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {
    uint8_t buffer[1024];
    int x = 0;

    DEBUG("SendDfuFile fileLen %d sentLen %d blockLen %d", fileLen, sentLen, blockLen);

    buffer[x++] = BREAK_UINT32(fileLen, 3);
    buffer[x++] = BREAK_UINT32(fileLen, 2);
    buffer[x++] = BREAK_UINT32(fileLen, 1);
    buffer[x++] = BREAK_UINT32(fileLen, 0);

    buffer[x++] = BREAK_UINT32(sentLen, 3);
    buffer[x++] = BREAK_UINT32(sentLen, 2);
    buffer[x++] = BREAK_UINT32(sentLen, 1);
    buffer[x++] = BREAK_UINT32(sentLen, 0);

    buffer[x++] = HI_UINT16(blockLen);
    buffer[x++] = LO_UINT16(blockLen);

    memcpy(buffer + x, data, blockLen);
    x += blockLen;

    SendMcuCommand(pCls, &SerialPort::WriteSerialPort, ID_ADA_DFU_UPLOAD, buffer, x);
}

void EnterAdaDfu(const uint8_t *file, int length)
{
    if (dfu != nullptr) {
        delete dfu;
    }
    dfu = new Dfu(SendDfuFile, file, length);
    dfu->Run();
    delete dfu;
    dfu = nullptr;
}

 lib/src/main/cpp/mcu/ada.h

New file
@@ -0,0 +1,58 @@
//
// Created by Administrator on 2022/11/15.
//

#ifndef MYAPPLICATION3_ADA_H
#define MYAPPLICATION3_ADA_H

#include <cstdint>
#include <string>

#define ID_ADA_INFO_REQ     0x0001
#define ID_ADA_INFO_RESP     0x8001
#define ID_ADA_DFU_UPLOAD       0x0002
#define ID_ADA_DFU_RESP         0x8002
#define ID_ADA_OBD          0x800A

typedef struct {
    std::string version;
    std::string sn;
}ada_info_t;

#pragma pack(push, 1)
typedef struct {
    uint32_t total_odo;
    uint32_t this_odo;
    uint32_t this_time;
    uint16_t voltage;
    uint16_t speed;
    uint16_t rpm;
    int16_t steering_wheel;
    uint8_t key_status;
    uint8_t gear;
    uint8_t accelerator;
    uint8_t door;
    uint8_t seat_belt;
    uint8_t clutch;
    uint8_t horn;
    uint8_t wiper;
    uint8_t hand_break;
    uint8_t main_break;
    uint8_t left_sign;
    uint8_t right_sign;
    uint8_t clearance_lamp;
    uint8_t dipped_beam_lamp;
    uint8_t main_beam_lamp;
    uint8_t fog_lamp;
    uint8_t assist_break;
    uint8_t surround1;
    uint8_t surround2;
    uint8_t surround3;
    uint8_t surround4;
    uint8_t resever[3];
}car_sensor_t;
#pragma pack(pop)

void InitAda(void);

#endif //MYAPPLICATION3_ADA_H

 lib/src/main/cpp/mcu/ahp.cpp

New file
@@ -0,0 +1,180 @@
//
// Created by YY on 2022/11/14.
//

#include "ahp.h"
#include "../common/string_util.h"
#include "../common/serial_port.h"
#include "mcu_if.h"
#include "dfu.h"
#include "../jni_log.h"
#include "../defs.h"
#include "../rtk_module/parse_gps.h"

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

static Dfu *dfu = nullptr;
static uint8_t RxBuf[4096];
static int RxBufLen = 0;

static SerialPort *pCls = nullptr;

static void AhpCommandEntry(uint16_t id, const uint8_t *data, int length) {
    switch (id) {
        case ID_AHP_INFO_RESP: {
            aph_info_t info;

            info.version = StringUtil::BuffertoString(data, 8);
            info.sn = StringUtil::BcdBuffertoString(data + 8, 8);
            break;
        }
        case ID_AHP_DFU_RESP: {
            if (dfu != nullptr) {
                dfu->RemoteAck(data, length);
            }
            break;
        }

        case ID_GNSS_DATA: {
//            DEBUG("ID_GNSS_DATA %d", length);
            memcpy(RxBuf + RxBufLen, data, length);
            RxBufLen += length;

//            std::string out = StringUtil::BuffertoString(RxBuf, RxBufLen);
//            DEBUG("%s", out.c_str());

            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if (ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
                RxBufLen -= ptr - RxBuf;
            } else if (RxBufLen == sizeof(RxBuf)) {        //填满了，且没有一个\r，都抛弃
                DEBUG("Parse GPS error");
                RxBufLen = 0;
            }

            break;
        }
        default:break;
    }
}

#define PARSE_BUFF_SIZE         4096

static void UartThread(void *p) {
    struct serial_config *cfg = (struct serial_config *) p;

    pCls = new SerialPort(*cfg);

    int res = pCls->InitSerialPort();
    DEBUG("Serial %s open %d", cfg->name, res);

    uint8_t RxBuf[PARSE_BUFF_SIZE];
    int RxBufLen = 0;

    ParseUart parse(AhpCommandEntry);

    while (res == 0) {
        int ul = pCls->ReadSerialPort((uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
        if (ul < 0) {
            continue;
        } else if (ul == 0) {
            // usb串口断开
            break;
        }
        RxBufLen += ul;

        /*{
            static char buffd[16384];

            buffd[0] = 0;
            int i = 0;
            for (i = 0; i < ul; i++) {
                if ((i % 32) == 0) {
                    sprintf(buffd + strlen(buffd), "\n");
                }
                sprintf(buffd + strlen(buffd), "%02X ", RxBuf[i]);
                if (strlen(buffd) > 800) {
                    DEBUG("%s <- %s...", "UART", buffd);
                    buffd[0] = 0;
                }
            }
            if (strlen(buffd) > 0)
                DEBUG("%s <- %s", "UART", buffd);
        }*/

        if (RxBufLen > 0) {
//            DEBUG("RECV LEN %d", RxBufLen);
            parse.ParseMcu(RxBuf, RxBufLen);
            RxBufLen = 0;
        }
    }

    delete pCls;
    pCls = nullptr;
}

static void AhpRun(void)
{
    static struct serial_config serialConfig;

    strcpy(serialConfig.name, "/dev/ttyCH341USB3");
    serialConfig.baud = 115200;
    serialConfig.data_bit = 8;
    serialConfig.verify_bit = 'N';
    serialConfig.stop_bit = 1;
    serialConfig.flow_ctrl = 0;

    while (true) {
        std::thread t(UartThread, &serialConfig);
        t.join();
        std::this_thread::sleep_for(std::chrono::seconds(3));
    }
}

void InitAhp(void)
{
    // TODO
    std::thread(AhpRun).detach();
}

static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {
    uint8_t buffer[1024];
    int x = 0;

    DEBUG("SendDfuFile fileLen %d sentLen %d blockLen %d", fileLen, sentLen, blockLen);

    buffer[x++] = BREAK_UINT32(fileLen, 3);
    buffer[x++] = BREAK_UINT32(fileLen, 2);
    buffer[x++] = BREAK_UINT32(fileLen, 1);
    buffer[x++] = BREAK_UINT32(fileLen, 0);

    buffer[x++] = BREAK_UINT32(sentLen, 3);
    buffer[x++] = BREAK_UINT32(sentLen, 2);
    buffer[x++] = BREAK_UINT32(sentLen, 1);
    buffer[x++] = BREAK_UINT32(sentLen, 0);

    buffer[x++] = HI_UINT16(blockLen);
    buffer[x++] = LO_UINT16(blockLen);

    memcpy(buffer + x, data, blockLen);
    x += blockLen;

    SendMcuCommand(pCls, &SerialPort::WriteSerialPort, ID_AHP_DFU_UPLOAD, buffer, x);
}

void EnterAhpDfu(const uint8_t *file, int length)
{
    if (dfu != nullptr) {
        delete dfu;
    }
    dfu = new Dfu(SendDfuFile, file, length);
    dfu->Run();
    delete dfu;
    dfu = nullptr;
}

void WriteRtkCommand(const char *data, int length)
{
    DEBUG("发送RTK命令 %s", StringUtil::BuffertoString((uint8_t *)data, length).c_str());
    SendMcuCommand(pCls, &SerialPort::WriteSerialPort, ID_CTRL_CMD, reinterpret_cast<const uint8_t *>(data), length);
}

 lib/src/main/cpp/mcu/ahp.h

New file
@@ -0,0 +1,39 @@
//
// Created by YY on 2022/11/14.
//

#ifndef MYAPPLICATION3_AHP_H
#define MYAPPLICATION3_AHP_H

#include <cstdint>
#include <string>

#define ID_AHP_INFO_REQ          0x0001
#define ID_AHP_INFO_RESP          0x8001
#define ID_AHP_DFU_UPLOAD       0x0002
#define ID_AHP_DFU_RESP         0x8002
#define ID_RTK_DATA             0x0008
#define ID_GNSS_DATA             0x8008
#define ID_CTRL_CMD             0x0009

typedef struct {
    std::string version;
    std::string sn;
} aph_info_t;

struct aph_dfu_data {
    uint32_t file_size;
    uint32_t offset;
    uint16_t length;
    uint8_t data[0];
};

struct aph_dfu_resp {
    uint8_t status;
    uint8_t data[0];
};

void InitAhp(void);
void WriteRtkCommand(const char *data, int length);

#endif //MYAPPLICATION3_AHP_H

 lib/src/main/cpp/mcu/dfu.cpp

New file
@@ -0,0 +1,147 @@
//
// Created by Administrator on 2022/11/28.
//

#include <cstring>
#include "dfu.h"
#include "../defs.h"
#include "../jni_log.h"

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

Dfu::Dfu(SendDfuPtr fun, const uint8_t *data, int length) {
    dfuTryCnt = 0;
    dfuFileBitmap = new uint8_t [128];
    memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));

    if (length > 0) {
        dfuFile = new uint8_t[length];
        dfuFileLength = length;
        memcpy(dfuFile, data, length);
    }
    SendDfuFile = fun;
}

Dfu::~Dfu() {
    delete []dfuFileBitmap;
    if (dfuFile != nullptr) {
        delete []dfuFile;
    }
}

int Dfu::GoNextDfu(void)
{
    int dfuFileSent = 0, currDfuBlockLength = 0;

    if (dfuFileLength == 0 || dfuFile == NULL || SendDfuFile == nullptr || dfuCancel)
        return DFU_END;

    int row = 0, col = 0;

    dfuFileSent = dfuFileLength;

    for (row = 0; row < sizeof(dfuFileBitmap); ++row) {
        for (col = 0; col < 8; ++col) {
            if ((dfuFileBitmap[row] & BV(col)) == 0) {
                DEBUG("ROW = %d COL = %d", row, col);
                dfuFileSent = row * DFU_FILE_BLOCK_SIZE * 8 + col * DFU_FILE_BLOCK_SIZE;
                goto GET_FILE_START;
            }
        }
    }

    GET_FILE_START:
    currDfuBlockLength = (dfuFileLength - dfuFileSent > DFU_FILE_BLOCK_SIZE) ? DFU_FILE_BLOCK_SIZE : (dfuFileLength - dfuFileSent);

    if (dfuFileSent >= dfuFileLength || currDfuBlockLength == 0) {
        return DFU_END;
    }

    SendDfuFile(dfuFileLength, dfuFileSent, currDfuBlockLength, dfuFile + dfuFileSent);

    if (dfuFileSent > 0 && dfuFileSent + currDfuBlockLength < dfuFileLength) {
        dfuFileBitmap[row] |= BV(col);
    }

    if (dfuFileSent == 0 || dfuFileSent + currDfuBlockLength == dfuFileLength) {
        dfuTryCnt++;
        if (dfuTryCnt > DFU_MAX_TRY) {
            return DFU_END;
        }
        DEBUG("GoNextDfuLater 3 sec...");
        return DFU_LONG_WAIT;
    }
    return DFU_SHORT_WAIT;
}

void Dfu::RemoteAck(const uint8_t *data, int length)
{
    DEBUG("ID_MC_AHP_DFU_RESP %d len %d", data[0], length);

    if (data[0] == 0) {
        // 第一包传输成功
        dfuFileBitmap[0] |= 0x01;
    } else if (data[0] == 10) {
        // 升级完成
        memset(dfuFileBitmap, 0xFF, sizeof(dfuFileBitmap));
    } else if (data[0] == 11) {
        // 放弃传输
        dfuCancel = true;
    } else if (data[0] == 12) {
        // 全部重传
        memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
    } else if (data[0] == 13) {
        // 部分重传，有后续字段
        DEBUG("BITMAP %02X %02X %02X %02X %02X", data[1], data[2], data[3], data[4], data[5]);

        int total = dfuFileLength / DFU_FILE_BLOCK_SIZE + ((dfuFileLength % DFU_FILE_BLOCK_SIZE)?1:0);
        int a = 0, b = 0;
        for (int i = 1; i < length; ++i) {
            for (int j = 0; j < 8; ++j) {
                if ((data[i] & BV(j))) b++;
                a++;
                if (a == total) {
                    i = length;
                    break;
                }
            }
        }

        DEBUG("BITMAP total %d succ %d", total, b);

        //memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
        memcpy(dfuFileBitmap, data + 1, length - 1);

        if (total % 8) {
            dfuFileBitmap[total/8] &= ~BV((total%8) - 1);
        } else {
            dfuFileBitmap[(total+7)/8 - 1] &= ~BV(7);
        }
//                dfuFileBitmap[total/8] &= ~BV(total%8);
    }

    std::unique_lock<std::mutex> lk(cv_mtx);
    expired = true;
    cv.notify_one();
}

void Dfu::Run(void)
{
    while(true) {
        std::unique_lock<std::mutex> lk(cv_mtx);
        expired = false;
        int status = GoNextDfu();

        if (status == DFU_END) {
            break;
        }

        if (cv.wait_for(lk, std::chrono::milliseconds(status == DFU_LONG_WAIT? 3000: 10), [this] { return expired == true; }) ==
            false) {
            // timeout
        } else {
            // cancel
            dfuTryCnt = 0;
        }
    }
}

 lib/src/main/cpp/mcu/dfu.h

New file
@@ -0,0 +1,40 @@
//
// Created by Administrator on 2022/11/28.
//

#ifndef MYAPPLICATION3_DFU_H
#define MYAPPLICATION3_DFU_H

#include <cstdint>
#include <mutex>
#include <atomic>
#include <condition_variable>

#define DFU_SHORT_WAIT      0
#define DFU_LONG_WAIT       1
#define DFU_END             2

const int DFU_MAX_TRY = 3;
const int DFU_FILE_BLOCK_SIZE = 896;

class Dfu {
public:
    typedef void (*SendDfuPtr)(int fileLen, int sentLen, int blockLen, const uint8_t *data);
    Dfu(SendDfuPtr fun, const uint8_t *data, int length);
    ~Dfu();
    void RemoteAck(const uint8_t *data, int length);
    void Run(void);
    SendDfuPtr SendDfuFile = nullptr;
private:
    uint8_t *dfuFile = nullptr;
    uint8_t *dfuFileBitmap = nullptr;
    int dfuFileLength = 0;
    int dfuTryCnt = 0;
    int GoNextDfu(void);
    bool dfuCancel = false;
    std::atomic_bool expired;
    std::mutex cv_mtx;
    std::condition_variable cv;
};

#endif //MYAPPLICATION3_DFU_H

 lib/src/main/cpp/mcu/mcu_if.cpp

@@ -3,9 +3,11 @@
//

#include <cstring>
#include <pthread.h>
#include <thread>
#include <mutex>
#include <cstdlib>
#include "mcu_if.h"
#include "ahp.h"
#include "../common/apptimer.h"
#include "../utils/crc16.h"
#include "../defs.h"
@@ -15,92 +17,34 @@
#include "../rtk_module/parse_gps.h"
#include "../native-lib.h"
#include "../rtk_module/virtual_rtk.h"
#include "../common/string_util.h"

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

#define MCU_UART            UART_1

enum parse_status_t {
    SYNC_HEAD_ONE,
    SYNC_HEAD_TWO,
    GET_ID_HI,
    GET_ID_LO,
    GET_LENGTH_HI,
    GET_LENGTH_LO,
    GET_PAYLOAD,
    GET_CRC16_HI,
    GET_CRC16_LO
};

#define ID_CM_APP_BOOT          0x0001
#define ID_MC_MCU_BOOT          0x8001
#define ID_CM_DFU_UPLOAD       0x0002
#define ID_MC_DFU_RSP           0x8002
#define ID_CM_RW_INFO           0x0003
#define ID_MC_RW_INFO_RSP       0x8003
#define ID_CM_MCU_DFU_REQ       0x0004
#define ID_MC_MCU_DFU_RSP       0x8004
#define ID_CM_MCU_DFU_DATA       0x0005
#define ID_CM_MCU_DFU_DATA_CMP  0x0006
#define ID_MC_CAR_INFO2  0x8006
#define ID_MC_CAR_INFO           0x8007
#define ID_CM_AHP_INQ          0x0001
#define ID_MC_AHP_RESP          0x8001
#define ID_CM_AHP_DFU_DATA_UPLOAD       0x0002
#define ID_MC_AHP_DFU_RESP           0x8002
#define ID_MC_RTK_DATA          0x8008
#define ID_CM_RTK_DATA            0x0008
#define ID_CM_READ_RFCARD         0x0009
#define ID_MC_RFCARD_RSP        0x8009
#define ID_CM_SHUTDOWN          0x0020
#define ID_MC_GNSS_DATA         0x800A
#define ID_CM_SINAN_CMD_DATA    0x000B

static parse_status_t parse_status;

static struct {
    uint16_t id;
    uint16_t length;
    uint16_t rx_len;
    uint8_t buffer[4096 + 4];
    uint16_t crc16;
}McuPkt;

static uint8_t *dfuFile = NULL;
static uint8_t dfuFileBitmap[128];

static int dfuFileLength = 0;
static int dfuTryCnt = 0;
const int DFU_MAX_TRY = 3;
const int DFU_FILE_BLOCK_SIZE = 896;

static int (*WriteMcu)(int id, const void *buf, int len);

static int WriteBluetooth(int id, const void *buf, int len);

static void *UartThread1(void *p);
static void ParseMcuTimeout(union sigval sig);
static void McuCommandEntry(uint16_t id, const uint8_t *data, int length);
static void ReadCardTimeout(apptimer_var_t val);

static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data);
static void GoNextDfuLater(union sigval sig);
static void GoNextDfu(void);
static void ReadCardTimeout(union sigval sig);

void McuCommModeSel(int mode)
{
    if (mode == 0) {
        WriteMcu = WriteSerialPort;
    } else {
        WriteMcu = WriteBluetooth;
    }
ParseUart::ParseUart(funptr fun) {
    McuPkt.buffer = new uint8_t [4096 + 4];
    this->fun = fun;
    parse_status = SYNC_HEAD_ONE;
}

void ParseMcuInit(void)
{
    dfuFile = NULL;
    dfuFileLength = 0;

    parse_status = SYNC_HEAD_ONE;
    AppTimer_delete(ParseMcuTimeout);

//    SendMcuCommand(ID_CM_APP_BOOT, NULL, 0);
ParseUart::~ParseUart() {
    delete []McuPkt.buffer;
}

static int WriteBluetooth(int id, const void *buf, int len)
@@ -109,70 +53,26 @@
    return len;
}


#define PARSE_BUFF_SIZE         4096

static void *UartThread1(void *p) {
    struct serial_config *cfg = (struct serial_config *) p;

    int res = InitSerialPort(MCU_UART, cfg->baud, cfg->data_bit, cfg->verify_bit, cfg->stop_bit, cfg->flow_ctrl);
    DEBUG("Serial %s open %d", cfg->name, res);
    uint8_t RxBuf[PARSE_BUFF_SIZE];
    int RxBufLen = 0;

    if (res == 0)
        ParseMcuInit();

    while (res == 0) {
        int ul = ReadSerialPort(MCU_UART, (uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
        RxBufLen += ul;

        /*{
            static char buffd[16384];

            buffd[0] = 0;
            int i = 0;
            for (i = 0; i < ul; i++) {
                if ((i % 32) == 0) {
                    sprintf(buffd + strlen(buffd), "\n");
                }
                sprintf(buffd + strlen(buffd), "%02X ", RxBuf[i]);
                if (strlen(buffd) > 800) {
                    DEBUG("%s <- %s...", "UART", buffd);
                    buffd[0] = 0;
                }
            }
            if (strlen(buffd) > 0)
                DEBUG("%s <- %s", "UART", buffd);
        }*/

        if (RxBufLen > 0) {
//            DEBUG("RECV LEN %d", RxBufLen);
            if (Virtual2IsConnected()) {

            } else {
                ParseMcu(RxBuf, RxBufLen);
            }
            RxBufLen = 0;
        }
    }
    if (res == 0) {
        UninitSerialPort(MCU_UART);
    }
    pthread_exit(NULL);
}

void ParseMcu(const uint8_t *data, int length)
void ParseUart::ParseMcu(const uint8_t *data, int length)
{
    int x = 0;
    uint32_t now = AppTimer_GetTickCount();

    if (now < sync_time) {
        sync_time = now;
    }

    if (now - sync_time >= D_SEC(5)) {
        parse_status = SYNC_HEAD_ONE;
    }

    while (x < length) {
        uint8_t c = data[x];
        switch (parse_status) {
            case SYNC_HEAD_ONE:
                if (c == 0x55) {
                    parse_status = SYNC_HEAD_TWO;
                    AppTimer_delete(ParseMcuTimeout);
                    AppTimer_add(ParseMcuTimeout, D_SEC(5));
                    sync_time = now;
                }
                x++;
                break;
@@ -209,7 +109,6 @@
                if (McuPkt.length >= 1500) {
                    DEBUG("Pkt Too large!");
                    parse_status = SYNC_HEAD_ONE;
                    AppTimer_delete(ParseMcuTimeout);
                }

                McuPkt.buffer[0] = HI_UINT16(McuPkt.id);
@@ -241,14 +140,14 @@

                uint16_t crc16 = CRCCCITT(McuPkt.buffer, McuPkt.length + 4, 0, 0);

//                DEBUG("mcuif crc16 but 0x%04X exp 0x%04X", McuPkt.crc16, crc16);
//                DEBUG("mcuif 0x%02X: crc16 but 0x%04X exp 0x%04X", McuPkt.id, McuPkt.crc16, crc16);

                if (McuPkt.crc16 == crc16) {
                    McuCommandEntry(McuPkt.id, McuPkt.buffer + 4, McuPkt.length);
                if (McuPkt.crc16 == crc16 && fun != nullptr) {
                    fun(McuPkt.id, McuPkt.buffer + 4, McuPkt.length);
                    //McuCommandEntry(McuPkt.id, McuPkt.buffer + 4, McuPkt.length);
                }

                parse_status = SYNC_HEAD_ONE;
                AppTimer_delete(ParseMcuTimeout);
                x++;
                break;
            }
@@ -258,8 +157,11 @@
    }
}

void SendMcuCommand(uint16_t id, const uint8_t *data, int length)
void SendMcuCommand(SerialPort *pClass, pSerialPortClassFun fun, uint16_t id, const uint8_t *data, int length)
{
    if (pClass == nullptr)
        return;

    uint8_t buffer[2048];
    int x = 0;

@@ -279,325 +181,34 @@
    buffer[x++] = HI_UINT16(crc16);
    buffer[x++] = LO_UINT16(crc16);

    WriteMcu(MCU_UART, buffer, x);
}

void ConfigMCU(bool ayDevice)
{
    if (ayDevice) {
        McuCommModeSel(0);

        // TODO
        static struct serial_config serialConfig;

        strcpy(serialConfig.name, "/dev/ttyHSL1");
        serialConfig.baud = 115200;
        serialConfig.data_bit = 8;
        serialConfig.verify_bit = 'N';
        serialConfig.stop_bit = 1;
        serialConfig.flow_ctrl = 0;

        pthread_t pid;
        pthread_attr_t attr;
        pthread_attr_init(&attr);
        pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
        pthread_create(&pid, &attr, UartThread1, &serialConfig);
    } else {
        McuCommModeSel(1);
    }
    (pClass->*fun)(buffer, x);
}

void SendRtkToMcu(const uint8_t *data, int length)
{
    SendMcuCommand(ID_CM_RTK_DATA, data, length);
    //SendMcuCommand(ID_CM_RTK_DATA, data, length);
}

static void ParseMcuTimeout(union sigval sig) {
    AppTimer_delete(ParseMcuTimeout);
    parse_status = SYNC_HEAD_ONE;
}

static void sendrtk(union sigval sig) {
static void sendrtk(apptimer_var_t val) {
    uint8_t data[486];

    memset(data, 0x86, sizeof(data));

    SendMcuCommand(ID_CM_RTK_DATA, data, sizeof(data));
    //SendMcuCommand(ID_CM_RTK_DATA, data, sizeof(data));

    SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
    //SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);

    AppTimer_delete(sendrtk);
    AppTimer_add(sendrtk, D_SEC(1));
}

static uint8_t GnssBuf[PARSE_BUFF_SIZE];
static int GnssBufLen = 0;

static void McuCommandEntry(uint16_t id, const uint8_t *data, int length)
{
    static int ii = 0;

    switch (id) {
        case ID_MC_MCU_BOOT:
            DEBUG("MCU BOOT");
            if (length == 8) {
                PlatformStatusChanged(CAN_UPDATE_EVT, data, length);
            }

            break;
        case ID_MC_DFU_RSP:
            DEBUG("ID_MC_DFU_RSP %d len %d", data[0], length);

            if (data[0] == 0) {
                // 第一包传输成功
                dfuFileBitmap[0] |= 0x01;
            } else if (data[0] == 10) {
                // 升级完成
                memset(dfuFileBitmap, 0xFF, sizeof(dfuFileBitmap));
            } else if (data[0] == 11) {
                // 放弃传输
                UploadDfuFileEnd();
            } else if (data[0] == 12) {
                // 全部重传
                memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
            } else if (data[0] == 13) {
                // 部分重传，有后续字段
                DEBUG("BITMAP %02X %02X %02X %02X %02X", data[1], data[2], data[3], data[4], data[5]);

                int total = dfuFileLength / DFU_FILE_BLOCK_SIZE + ((dfuFileLength % DFU_FILE_BLOCK_SIZE)?1:0);
                int a = 0, b = 0;
                for (int i = 1; i < length; ++i) {
                    for (int j = 0; j < 8; ++j) {
                        if ((data[i] & BV(j))) b++;
                        a++;
                        if (a == total) {
                            i = length;
                            break;
                        }
                    }
                }

                DEBUG("BITMAP total %d succ %d", total, b);

                //memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
                memcpy(dfuFileBitmap, data + 1, length - 1);

                if (total % 8) {
                    dfuFileBitmap[total/8] &= ~BV((total%8) - 1);
                } else {
                    dfuFileBitmap[(total+7)/8 - 1] &= ~BV(7);
                }
//                dfuFileBitmap[total/8] &= ~BV(total%8);
            }

            dfuTryCnt = 0;
            AppTimer_delete(GoNextDfuLater);
            GoNextDfu();
            break;
        case ID_MC_RW_INFO_RSP:
            break;
        case ID_MC_MCU_DFU_RSP:
            break;
        case ID_MC_CAR_INFO2:
           /* DEBUG("ID_MC_CAR_INFO2 %d", length);

            {
                static char buffd[16384];

                buffd[0] = 0;
                int i = 0;
                for (i = 0; i < length; i++) {
                    if ((i % 32) == 0) {
                        sprintf(buffd + strlen(buffd), "\n");
                    }
                    sprintf(buffd + strlen(buffd), "%02X ", data[i]);
                    if (strlen(buffd) > 800) {
                        DEBUG("%s <- %s...", "车辆信号" , buffd);
                        buffd[0] = 0;
                    }
                }
                if (strlen(buffd) > 0)
                    DEBUG("%s <- %s", "车辆信号" , buffd);
            }*/

            if (length > 0) {
                PlatformStatusChanged(CAR_SENSOR_UPDATE_EVT, data, length);
            }
            break;
        case ID_MC_CAR_INFO: {
//            DEBUG("ID_MC_CAR_INFO %d", length);

            if (length > 0)
                PlatformStatusChanged(MCU_UPDATE_EVT, data, length);
            break;
        }
        case ID_MC_RTK_DATA:
            DEBUG("ID_MC_RTK_DATA");
            break;
        case ID_MC_RFCARD_RSP:
            DEBUG("ID_MC_RFCARD_RSP");
            AppTimer_delete(ReadCardTimeout);

            if (length > 0)
                PlatformStatusChanged(CARD_UPDATE_EVT, data, length);
            break;
        case ID_MC_GNSS_DATA: {
            length = (length > PARSE_BUFF_SIZE - GnssBufLen) ? (PARSE_BUFF_SIZE - GnssBufLen) : length;

            memcpy(GnssBuf + GnssBufLen, data, length);
            GnssBufLen += length;

            if (GnssBufLen > 0) {
                if (VirtualIsConnected()) {     //PC模拟用时
                    static bool first = false;

                    if (!first) {
                        first = true;

                        uint8_t buff[33] = "emulator";
                        buff[32] = 1;
                        PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
                    }

                    GnssBufLen = 0;
                } else {
                    const uint8_t *ptr = parseGPS(GnssBuf, GnssBuf + GnssBufLen);
                    if (ptr != GnssBuf) {
                        memcpy(GnssBuf, ptr, GnssBufLen - (ptr - GnssBuf));
                        GnssBufLen -= ptr - GnssBuf;
                    } else if (GnssBufLen == PARSE_BUFF_SIZE) {        //填满了，且没有一个\r，都抛弃
                        DEBUG("Parse GPS error");
                        GnssBufLen = 0;
                    }
                }
            }
            break;
        }
        default:
            break;
    }
}

static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {
    uint8_t buffer[1024];
    int x = 0;

    DEBUG("SendDfuFile fileLen %d sentLen %d blockLen %d", fileLen, sentLen, blockLen);

    buffer[x++] = BREAK_UINT32(fileLen, 3);
    buffer[x++] = BREAK_UINT32(fileLen, 2);
    buffer[x++] = BREAK_UINT32(fileLen, 1);
    buffer[x++] = BREAK_UINT32(fileLen, 0);

    buffer[x++] = BREAK_UINT32(sentLen, 3);
    buffer[x++] = BREAK_UINT32(sentLen, 2);
    buffer[x++] = BREAK_UINT32(sentLen, 1);
    buffer[x++] = BREAK_UINT32(sentLen, 0);

    buffer[x++] = HI_UINT16(blockLen);
    buffer[x++] = LO_UINT16(blockLen);

    memcpy(buffer + x, data, blockLen);
    x += blockLen;

    SendMcuCommand(ID_CM_DFU_UPLOAD, buffer, x);
}

static void GoNextDfuLater(union sigval sig) {
    AppTimer_delete(GoNextDfuLater);

    GoNextDfu();
}

static void GoNextDfu(void)
{
    int dfuFileSent = 0, currDfuBlockLength = 0;

    if (dfuFileLength == 0 || dfuFile == NULL)
        return;

    int row = 0, col = 0;

    dfuFileSent = dfuFileLength;

    for (row = 0; row < sizeof(dfuFileBitmap); ++row) {
        for (col = 0; col < 8; ++col) {
            if ((dfuFileBitmap[row] & BV(col)) == 0) {
                DEBUG("ROW = %d COL = %d", row, col);
                dfuFileSent = row * DFU_FILE_BLOCK_SIZE * 8 + col * DFU_FILE_BLOCK_SIZE;
                goto GET_FILE_START;
            }
        }
    }

GET_FILE_START:
    currDfuBlockLength = (dfuFileLength - dfuFileSent > DFU_FILE_BLOCK_SIZE) ? DFU_FILE_BLOCK_SIZE : (dfuFileLength - dfuFileSent);

    if (dfuFileSent >= dfuFileLength || currDfuBlockLength == 0) {
        UploadDfuFileEnd();
        return;
    }

    SendDfuFile(dfuFileLength, dfuFileSent, currDfuBlockLength, dfuFile + dfuFileSent);

    if (dfuFileSent > 0 && dfuFileSent + currDfuBlockLength < dfuFileLength) {
        dfuFileBitmap[row] |= BV(col);
    }

    if (dfuFileSent == 0 || dfuFileSent + currDfuBlockLength == dfuFileLength) {
        dfuTryCnt++;
        if (dfuTryCnt > DFU_MAX_TRY) {
            UploadDfuFileEnd();
            return;
        }
        DEBUG("GoNextDfuLater 3 sec...");
        AppTimer_add(GoNextDfuLater, D_SEC(3));
    } else {
        AppTimer_add(GoNextDfuLater, 10);
    }
}

void UploadDfuFileEnd(void)
{
    DEBUG("UploadDfuFileEnd");

    if (dfuFile != NULL)
        free(dfuFile);
    dfuFileLength = 0;

    AppTimer_delete(GoNextDfuLater);
}

void UploadDfuFile(const uint8_t *file, int length)
{
    DEBUG("UploadDfuFile %ld", length);

    if (length > 0) {
        if (dfuFile != NULL)
            free(dfuFile);

        dfuTryCnt = 0;

        dfuFile = (uint8_t *) malloc(length);
        dfuFileLength = length;
        memcpy(dfuFile, file, length);
        memset(dfuFileBitmap, 0, sizeof(dfuFileBitmap));
        AppTimer_delete(GoNextDfuLater);

        GoNextDfu();
    }
}

static int readCartCnt = 0;

static void ReadCardTimeout(union sigval sig) {
    AppTimer_delete(ReadCardTimeout);

static void ReadCardTimeout(apptimer_var_t val) {
    readCartCnt++;

    if (readCartCnt < 2) {
        AppTimer_add(ReadCardTimeout, D_SEC(3));
        SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
        //SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
    } else {
        uint8_t data[8] = {0};
        PlatformStatusChanged(CARD_UPDATE_EVT, data, sizeof(data));
@@ -610,12 +221,5 @@

    AppTimer_delete(ReadCardTimeout);
    AppTimer_add(ReadCardTimeout, D_SEC(3));
    SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
}

void ShutdownInd(int timeout)
{
    uint8_t v = timeout;

    SendMcuCommand(ID_CM_SHUTDOWN, &v, 1);
    //SendMcuCommand(ID_CM_READ_RFCARD, NULL, 0);
}

 lib/src/main/cpp/mcu/mcu_if.h

@@ -6,17 +6,47 @@
#define RTKDRIVERTEST_MCU_IF_H

#include <cstdint>
#include "../common/apptimer.h"
#include "../common/serial_port.h"

void McuCommModeSel(int mode);
void ConfigMCU(bool ayDevice);
enum parse_status_t {
    SYNC_HEAD_ONE,
    SYNC_HEAD_TWO,
    GET_ID_HI,
    GET_ID_LO,
    GET_LENGTH_HI,
    GET_LENGTH_LO,
    GET_PAYLOAD,
    GET_CRC16_HI,
    GET_CRC16_LO
};

class ParseUart {
public:
    typedef void (*funptr)(uint16_t id, const uint8_t *data, int length);
    ParseUart(funptr fun);
    ~ParseUart();

    struct {
        uint16_t id;
        uint16_t length;
        uint16_t rx_len;
        uint8_t *buffer;
        uint16_t crc16;
    } McuPkt;

    parse_status_t parse_status;
    uint32_t sync_time = 0;
    funptr fun = nullptr;
    void ParseMcu(const uint8_t *data, int length);
};



void SendRtkToMcu(const uint8_t *data, int length);
void ParseMcuInit(void);
void ParseMcu(const uint8_t *data, int length);
void SendMcuCommand(uint16_t id, const uint8_t *data, int length);
void SendMcuCommand(SerialPort *pClass, pSerialPortClassFun fun, uint16_t id, const uint8_t *data, int length);

void UploadDfuFileEnd(void);
void UploadDfuFile(const uint8_t *file, int length);
void ReadCard(void);
void ShutdownInd(int timeout);


#endif //RTKDRIVERTEST_MCU_IF_H

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

@@ -1,6 +1,6 @@
#include <jni.h>
#include <string>
#include <pthread.h>
#include <mutex>
#include <unistd.h>
#include <cstdlib>
#include <map>
@@ -18,22 +18,26 @@
#include "master/comm_if.h"
#include "rtk_module/virtual_rtk.h"
#include "defs.h"
#include "mcu/ada.h"
#include "mcu/ahp.h"

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

#define LIBENC_ARRAY_ELEMS(a)  (sizeof(a) / sizeof(a[0]))

static JavaVM *sg_jvm = NULL;
static jobject sg_obj = NULL;
static JNIEnv *jenv;


const char *RTK_PLATFORM_IP = "47.93.80.84";
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.43.76";
const char *VIRTUAL_RTK_IP = "192.168.16.212";
const int VIRTUAL_RTK_PORT = 9002;

static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;

static int ttsSeq = 1;
static std::mutex tts_mutex;

static std::map<int, void (*)(int)> TTSCallBack;

@@ -206,12 +210,10 @@

static int GetTtsSeq(void)
{
    int seq = 0;
    static int seq = 0;

    pthread_mutex_lock(&tts_mutex);
    seq = ttsSeq++;
    pthread_mutex_unlock(&tts_mutex);

    lock_guard<std::mutex> lock(tts_mutex);
    seq++;
    return seq;
}

@@ -414,35 +416,30 @@
    }
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_startNative(JNIEnv *env, jobject thiz, jboolean ayDevice) {
static void startNative(JNIEnv *env, jobject thiz, jboolean ayDevice) {
    // TODO: implement startNative()
    // 保存全局JVM以便在子线程中使用
    DEBUG("启动Native");
    env->GetJavaVM(&sg_jvm);
    // 不能直接赋值(g_obj = ojb)
    sg_obj = env->NewGlobalRef(thiz);

    srand(time(NULL));

    AppTimer_Init();
    ConfigMCU(ayDevice);
    AppTimer_init();
    ///////////////ConfigMCU(ayDevice);
    InitAda();
    InitAhp();
    DriverTestInit();
    ConfigRTKModule(ayDevice);

    MA_Init();
    InitPlatform(ayDevice, phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);

    InitVirtualDevice(VIRTUAL_RTK_IP, VIRTUAL_RTK_PORT);

    pthread_mutex_init(&tts_mutex, NULL);

    MA_NdkStart();
//    MA_NdkStart();
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_MainProcMsgEntry(JNIEnv *env, jobject thiz, jint cmd,
static void MainProcMsgEntry(JNIEnv *env, jobject thiz, jint cmd,
                                                       jstring value) {
    // TODO: implement MainProcMsgEntry()
    union {
@@ -474,9 +471,7 @@
    }
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_MainProcBinMsgEntry(JNIEnv *env, jobject thiz, jint cmd,
void MainProcBinMsgEntry(JNIEnv *env, jobject thiz, jint cmd,
                                                          jbyteArray data, jint length) {
    // TODO: implement MainProcBinMsgEntry()
    jbyte *c_dat = env->GetByteArrayElements(data, NULL);
@@ -487,11 +482,8 @@
    env->ReleaseByteArrayElements(data, c_dat, NULL);
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_TextSpeakEnd(JNIEnv *env, jobject thiz, jint id) {
void TextSpeakEnd(JNIEnv *env, jobject thiz, jint id) {
    // TODO: implement TextSpeakEnd()

    auto it = TTSCallBack.find(id);

    if (it != TTSCallBack.end()) {
@@ -506,9 +498,7 @@
    }
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_UpgradeMcu(JNIEnv *env, jobject thiz, jstring vercode,
void UpgradeMcu(JNIEnv *env, jobject thiz, jstring vercode,
                                                 jbyteArray rom) {
    // TODO: implement UpgradeMcu()
    if (vercode != NULL && rom != NULL) {
@@ -523,9 +513,7 @@
    }
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_BluetoothDataComeIn(JNIEnv *env, jobject thiz,
void BluetoothDataComeIn(JNIEnv *env, jobject thiz,
                                                          jbyteArray data, jint length) {
    // TODO: implement BluetoothDataComeIn()
    jbyte *c_dat = env->GetByteArrayElements(data, NULL);
@@ -535,9 +523,8 @@
    env->ReleaseByteArrayElements(data, c_dat, NULL);
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_BluetoothStatusChange(JNIEnv *env, jobject thiz,

void BluetoothStatusChange(JNIEnv *env, jobject thiz,
                                                            jint status) {
    // TODO: implement BluetoothStatusChange()
    uint8_t  sta = status;
@@ -545,9 +532,7 @@
    PlatformStatusChanged(BLUETOOTH_STATUS_EVT, &sta, 1);
}

extern "C"
JNIEXPORT void JNICALL
Java_com_anyun_exam_lib_RemoteService_BluetoothConnected(JNIEnv *env, jobject thiz, jstring name,
void BluetoothConnected(JNIEnv *env, jobject thiz, jstring name,
                                                         jstring addr) {
    // TODO: implement BluetoothConnected()
    if (name != NULL && addr != NULL) {
@@ -576,3 +561,59 @@
        PlatformStatusChanged(BLUETOOTH_STATUS_EVT, &sta, 1);
    }
}

static JNINativeMethod methods[] = {
        {"startNative", "(Z)V", reinterpret_cast<void *>(startNative)},
        {"MainProcMsgEntry", "(ILjava/lang/String;)V", reinterpret_cast<void *>(MainProcMsgEntry)},
        {"MainProcBinMsgEntry", "(I[BI)V", reinterpret_cast<void *>(MainProcBinMsgEntry)},
        {"UpgradeMcu", "(Ljava/lang/String;[B)V", reinterpret_cast<void *>(UpgradeMcu)},
        {"TextSpeakEnd", "(I)V", reinterpret_cast<void *>(TextSpeakEnd)},
        {"BluetoothConnected", "(Ljava/lang/String;Ljava/lang/String;)V", reinterpret_cast<void *>(BluetoothConnected)},
        {"BluetoothStatusChange", "(I)V", reinterpret_cast<void *>(BluetoothStatusChange)},
        {"BluetoothDataComeIn", "([BI)V", reinterpret_cast<void *>(BluetoothDataComeIn)}
};

jint JNI_OnLoad(JavaVM *vm, void *reserved)
{
    sg_jvm = vm;

    if (sg_jvm->GetEnv(reinterpret_cast<void **> (&jenv), JNI_VERSION_1_6) != JNI_OK) {
        DEBUG("Env not got");
        return JNI_ERR;
    }

    jclass clz = jenv->FindClass("com/anyun/exam/lib/RemoteService");
    if (clz == NULL) {
        DEBUG("目标类未找到");
        return JNI_ERR;
    }

    if (jenv->RegisterNatives(clz, methods, LIBENC_ARRAY_ELEMS(methods))) {
        DEBUG("methods not registered");
        return JNI_ERR;
    }

    DEBUG("JNI_OnLoad");

    return JNI_VERSION_1_6;
}

void JNI_OnUnload(JavaVM* vm, void* reserved)
{
    JNIEnv* env;

    if (vm->GetEnv(reinterpret_cast<void **> (&env), JNI_VERSION_1_6) != JNI_OK) {
        DEBUG("Env not got");
        return;
    }

    jclass clz = env->FindClass("com/anyun/exam/lib/RemoteService");
    if (clz == NULL) {
        DEBUG("目标类未找到");
        return;
    }

    env->UnregisterNatives(clz);

    DEBUG("JNI_OnUnload");
}

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

@@ -4,10 +4,10 @@

#include <cstring>
#include <cstdio>
#include <pthread.h>
#include <cmath>
#include <cstdlib>
#include <cctype>
#include <thread>
#include "rtk.h"
#include "parse_gps.h"
#include "../common/serial_port.h"
@@ -20,10 +20,9 @@
#include "../native-lib.h"
#include "virtual_rtk.h"
#include "../mcu/mcu_if.h"
#include "../mcu/ahp.h"

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

#define RTK_MODULE_UART         UART_0

#define PARSE_BUFF_SIZE         4096

@@ -41,64 +40,17 @@
static gpsStatus_t gpsStatus;
static char rtkModel[32] = {0};

static int (*WriteRtk)(int id, const void *buf, int len);

static rtk_info CurrRTKInfo;
static bool needSetPjk = false;
static int lostCnt;

static void CheckPjkParam(void);
static void CheckPjkParamTimeout(union sigval sig);
static void CheckPjkParamTimeout(apptimer_var_t val);
static int WriteBluetooth(int id, const void *buf, int len);
static void GetModuleVersion(void);

static void VersionTimeout(union sigval sig);
static void GpsDataTimeout(union sigval sig);

static void *UartThread(void *p);

void RtkCommModeSel(int mode)
{
    if (mode == 0) {
        WriteRtk = WriteSerialPort;
    } else {
        WriteRtk = WriteBluetooth;
    }
}

void ConfigRTKModule(bool ayDevice)
{
    // TODO
    DEBUG("ConfigRTKModule");

    memset(&CurrRTKInfo, 0, sizeof(CurrRTKInfo));
    CurrRTKInfo.hh = -1;

    memset(&gpsStatus, 0, sizeof(gpsStatus));
    gpsStatus.hh = -1;

    if (ayDevice) {
        RtkCommModeSel(0);

        static struct serial_config serialConfig;

        strcpy(serialConfig.name, "/dev/ttyHSL0");
        serialConfig.baud = 115200;
        serialConfig.data_bit = 8;
        serialConfig.verify_bit = 'N';
        serialConfig.stop_bit = 1;
        serialConfig.flow_ctrl = 0;

        pthread_t pid;
        pthread_attr_t attr;
        pthread_attr_init(&attr);
        pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
        pthread_create(&pid, &attr, UartThread, &serialConfig);
    } else {
        RtkCommModeSel(1);
        // 等待蓝牙连接
    }
}
static void VersionTimeout(apptimer_var_t val);
static void GpsDataTimeout(apptimer_var_t val);

// 蓝牙连接后
void ConfigRTKModuleLater(void)
@@ -111,12 +63,12 @@

void FactorySettings(void)
{
    WriteRtk(RTK_MODULE_UART, FACTORY, strlen(FACTORY));
    WriteRtkCommand(FACTORY, strlen(FACTORY));
}

void RebootModule(void)
{
    WriteRtk(RTK_MODULE_UART, REBOOT, strlen(REBOOT));
    WriteRtkCommand(REBOOT, strlen(REBOOT));
}

void handleRTKRebootComp(const struct nmea *s)
@@ -147,8 +99,8 @@

void SetAYFactoryParam(int freq)
{
    WriteRtk(RTK_MODULE_UART, UNLOGALL, strlen(UNLOGALL));
    WriteRtk(RTK_MODULE_UART, IFCOM2, strlen(IFCOM2));
    WriteRtkCommand(UNLOGALL, strlen(UNLOGALL));
    WriteRtkCommand(IFCOM2, strlen(IFCOM2));

    if (freq == 0)
        freq = 5;
@@ -156,13 +108,13 @@
    for (int i = 0; i < sizeof(PJKITEMS)/ sizeof(PJKITEMS[0]); ++i) {
        char cmd[64];
        sprintf(cmd, "log com1 %s ontime %0.1f\r\n", PJKITEMS[i], 1.0/(double)freq);
        WriteRtk(RTK_MODULE_UART, cmd, strlen(cmd));
        WriteRtkCommand(cmd, strlen(cmd));
    }

    for (int i = 0; i <  sizeof(GPSITEMS)/ sizeof(GPSITEMS[0]); ++i) {
        char cmd[64];
        sprintf(cmd, "log com1 %s ontime %0.1f\r\n", GPSITEMS[i], 1.0/(double)freq);
        WriteRtk(RTK_MODULE_UART, cmd, strlen(cmd));
        WriteRtkCommand(cmd, strlen(cmd));
    }

//    WriteSerialPort(RTK_MODULE_UART, AY_PJKPARAM, strlen(AY_PJKPARAM));
@@ -175,7 +127,7 @@

    sprintf(buff, "set pjkpara 6378137 298.257223563 0 %d 0 500000\r\n", centLon);

    WriteRtk(RTK_MODULE_UART, buff, strlen(buff));
    WriteRtkCommand(buff, strlen(buff));

    DEBUG("%s", buff);
}
@@ -185,79 +137,10 @@
    data = gpsStatus;
}

static void *UartThread(void *p) {
    struct serial_config *cfg = (struct serial_config *) p;

    int res = InitSerialPort(RTK_MODULE_UART, cfg->baud, cfg->data_bit, cfg->verify_bit, cfg->stop_bit, cfg->flow_ctrl);
    DEBUG("Serial %s open %d", cfg->name, res);
    uint8_t RxBuf[PARSE_BUFF_SIZE];
    int RxBufLen = 0;

    if (res == 0) {
//        CheckPjkParam();
        GetModuleVersion();
    }

    while (res == 0) {
        int ul = ReadSerialPort(RTK_MODULE_UART, (uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
        RxBufLen += ul;

        /*{
            static char buffd[16384];

            buffd[0] = 0;
            int i = 0;
            for (i = 0; i < ul; i++) {
                if ((i % 32) == 0) {
                    sprintf(buffd + strlen(buffd), "\n");
                }
                sprintf(buffd + strlen(buffd), "%02X ", RxBuf[i]);
                if (strlen(buffd) > 800) {
                    DEBUG("%s <- %s...", "UART", buffd);
                    buffd[0] = 0;
                }
            }
            if (strlen(buffd) > 0)
                DEBUG("%s <- %s", "UART", buffd);
        }*/

        if (RxBufLen > 0) {
#if 1
            if (VirtualIsConnected()) {     //PC模拟用时
                static bool first = false;
                if (!first) {
                    first = true;

                    uint8_t buff[33] = "emulator";
                    buff[32] = 1;
                    PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
                }
                RxBufLen = 0;
            } else {
                const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
                if (ptr != RxBuf) {
                    memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
                    RxBufLen -= ptr - RxBuf;
                } else if (RxBufLen == PARSE_BUFF_SIZE) {        //填满了，且没有一个\r，都抛弃
                    DEBUG("Parse GPS error");
                    RxBufLen = 0;
                }
            }
#else
            RxBufLen = 0;           //PC模拟用时
#endif
        }
    }
    if (res == 0) {
        UninitSerialPort(RTK_MODULE_UART);
    }
    pthread_exit(NULL);
}

void handleUnrecognisedNMEA(const uint8_t *data, uint16_t length) {
    char buff[4096] = {0};
    memcpy(buff, data, MIN(length, 4000));
//    DEBUG("handleUnrecognisedNMEA: %s", buff);
    DEBUG("handleUnrecognisedNMEA: %s", buff);

    if (length >= 100) {
        string cs(buff);
@@ -280,12 +163,20 @@
    }
}

static bool bbs = false;

void handleGPGGA(const struct nmea *s)
{
    static uint32_t qfCnt = 0;

//    DEBUG("handleGPGGA num = %d", s->nmea_num);
    if (s->nmea_num >= 10) {

        if (!bbs) {
            bbs = true;
            RebootModule();
        }

        lostCnt = 0;
        AppTimer_delete(GpsDataTimeout);
        AppTimer_add(GpsDataTimeout, D_SEC(5));
@@ -325,6 +216,7 @@

        // 计算中央子午线
        int qf = str2int(s->nmea_value[5].data, s->nmea_value[5].length);

        if (qf > 0) {
            qfCnt++;
            if (needSetPjk && qfCnt >= 3) {
@@ -430,34 +322,6 @@
    str2float(&CurrRTKInfo.y, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&CurrRTKInfo.x, s->nmea_value[4].data, s->nmea_value[4].length);

    static double sbx = -12;
    static double sby = 27;

//    CurrRTKInfo.x = (-2.8984 - 4.913)/2;
//    CurrRTKInfo.y = (31.6962 + 29.4974)/2;

    sbx += 0.01;
    sby += 0.01;

//    const double by1 = 28.013;
//    const double bx1 = -11.9669;
//
//    const double by2 = 29.3232;
//    const double bx2 = -9.5057;
//
//    static double xx = -10.9669, yy = 28.013;
//
//    CurrRTKInfo.y = yy;
//    CurrRTKInfo.x = xx;
//
//    if (forwardx) {
//        xx += 0.02;
//        yy += 0.02 * (by2 - by1) / (bx2 - bx1);
//    } else {
//        xx -= 0.02;
//        yy -= 0.02 * (by2 - by1) / (bx2 - bx1);
//    }

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
@@ -484,13 +348,6 @@
    str2float(&CurrRTKInfo.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&CurrRTKInfo.roll, s->nmea_value[3].data, s->nmea_value[3].length);

//    CurrRTKInfo.heading = 60;

    static double deg = 0;
//    CurrRTKInfo.heading = deg;
    deg += 2.0;
    if (deg >= 360) deg = 0;

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
@@ -507,15 +364,13 @@

static void CheckPjkParam(void)
{
    WriteRtk(RTK_MODULE_UART, INQ_PJK_PARAM, strlen(INQ_PJK_PARAM));
    WriteRtkCommand(INQ_PJK_PARAM, strlen(INQ_PJK_PARAM));
    DEBUG("获取PJK参数...");
    AppTimer_delete(CheckPjkParamTimeout);
    AppTimer_add(CheckPjkParamTimeout, D_SEC(3));
}

static void CheckPjkParamTimeout(union sigval sig) {
    AppTimer_delete(CheckPjkParamTimeout);

static void CheckPjkParamTimeout(apptimer_var_t val) {
    DEBUG("获取PJK参数超时");

    uint8_t buff[33];
@@ -529,7 +384,7 @@
static int WriteBluetooth(int id, const void *buf, int len)
{
//    SendToBluetooth((uint8_t *)buf, len);
    SendMcuCommand(0x000B, (uint8_t *)buf, len);
//    SendMcuCommand(0x000B, (uint8_t *)buf, len);
    return len;
}

@@ -537,14 +392,13 @@
{
    AppTimer_delete(VersionTimeout);
    AppTimer_add(VersionTimeout, D_SEC(3));
    WriteRtk(RTK_MODULE_UART, CMD_VERSION, strlen(CMD_VERSION));
    WriteRtkCommand(CMD_VERSION, strlen(CMD_VERSION));

    DEBUG("获取版本...");
}

static void VersionTimeout(union sigval sig)
static void VersionTimeout(apptimer_var_t val)
{
    AppTimer_delete(VersionTimeout);
    DEBUG("版本获取超时");
    GetModuleVersion();

@@ -554,10 +408,8 @@
    PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
}

static void GpsDataTimeout(union sigval sig)
static void GpsDataTimeout(apptimer_var_t val)
{
    AppTimer_delete(GpsDataTimeout);

    if (++lostCnt >= 3) {
        DEBUG("RTK模块收不到GPS数据");
        GetModuleVersion();

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

@@ -5,6 +5,10 @@
#ifndef RTKDRIVERTEST_RTK_H
#define RTKDRIVERTEST_RTK_H

#include <string>
#include <sstream>
#include "../utils/num.h"

typedef struct {
    uint16_t gps_status;
    int YY;
@@ -21,6 +25,21 @@
    double altitude;
    double speed;
    double trackTure;

    std::string toString(void) {
        std::stringstream sst;

        sst<<"GPS:"<<"qf="<<gps_status
        <<" "<<2000+YY<<intw(MM, 2)<<intw(DD, 2)<<intw(hh,2)<<intw(mm, 2)<<intw(ss, 2)<<"."<<intw(mss, 2)
        <<" satNum"<<satNum
                <<" latitude="<<round(latitude, 6)
                <<" longitude="<<round(longitude, 6)
                <<" altitude="<<round(altitude, 3)
                <<" speed="<<round(speed, 3)
                <<" trackTure="<<round(trackTure, 3);

        return sst.str();
    }
}gpsStatus_t;

typedef struct {
@@ -37,6 +56,20 @@
    double roll;
    double x;
    double y;

    std::string toString(void) {
        std::stringstream sst;

        sst<<"RTK:"<<"qf="<<qf
           <<" "<<2000+YY<<intw(MM, 2)<<intw(DD, 2)<<intw(hh,2)<<intw(mm, 2)<<intw(ss, 2)<<"."<<intw(dss, 2)
           <<" heading="<<round(heading, 3)
           <<" pitch="<<round(pitch, 3)
           <<" roll="<<round(roll, 3)
           <<" x="<<round(x, 3)
           <<" y="<<round(y, 3);

        return sst.str();
    }
}rtk_info;

void RtkCommModeSel(int mode);

 lib/src/main/cpp/rtk_module/virtual_rtk.cpp

@@ -2,7 +2,6 @@
// Created by YY on 2020/1/20.
//

#include <pthread.h>
#include <cstdlib>
#include <cstring>
#include "virtual_rtk.h"
@@ -34,8 +33,8 @@
static void TcpEventCallback2(int stat, void *p, void *context);
static void TcpDataCallback2(void *buffer, int length, void *p, void *context);

static void ConnectLater(union sigval sig);
static void ConnectLater2(union sigval sig);
static void ConnectLater(apptimer_var_t val);
static void ConnectLater2(apptimer_var_t val);

void InitVirtualDevice(const char *domain_name, int port)
{
@@ -85,18 +84,14 @@
    return temp;
}

static void ConnectLater(union sigval sig) {
    AppTimer_delete(ConnectLater);

static void ConnectLater(apptimer_var_t val) {
    if (ctp != NULL) {
        ctp->OpenTcpPort(VAddr.domain_name, VAddr.port);
    }
}

static void ConnectLater2(union sigval sig)
static void ConnectLater2(apptimer_var_t val)
{
    AppTimer_delete(ConnectLater2);

    if (ctp2 != NULL) {
        ctp2->OpenTcpPort(VAddr.domain_name, VAddr.port + 1);
    }
@@ -179,7 +174,7 @@
        RxBufLen += length;

        if (RxBufLen > 0) {
            ParseMcu(RxBuf, RxBufLen);
            /////////////////ParseMcu(RxBuf, RxBufLen);
            RxBufLen = 0;
        }
    }

 lib/src/main/cpp/rtk_platform/parse_net.cpp

@@ -6,7 +6,7 @@
#include <cstdio>
#include <cstring>
#include <malloc.h>
#include <pthread.h>
#include <mutex>
#include <semaphore.h>
#include "../jni_log.h"
#include "../defs.h"
@@ -125,10 +125,10 @@
}largeMessage[DATA_ACCESS_END];

static uint8_t PhoneNumber[PHONE_NUM_SIZE] = {0};
static pthread_mutex_t seq_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t tx_queue_mutex = PTHREAD_MUTEX_INITIALIZER;
static mutex seq_mutex;
static mutex tx_queue_mutex;

static void ParseTimeout(union sigval sig);
static void ParseTimeout(apptimer_var_t val);
static uint32_t GetResendTimeout(uint8_t access, uint8_t curr_resend_cnt, uint32_t base_time);
static uint16_t GetMessageSeq(uint8_t access);
static void PacketEntry(uint8_t access, const uint8_t *data, uint16_t length);
@@ -148,8 +148,8 @@
static void ResendItemTimeout(message_tx_table_t **head, uint32_t tm);
static uint32_t GetResentTimeoutTxQueue(message_tx_table_t **head);

static void *TxQueueMgrThread(void *p);
static void TriggerResendTxQueue(union sigval sig);
static void TxQueueMgrThread(void);
static void TriggerResendTxQueue(apptimer_var_t val);

/*
7E 80 89 00 40 87 00 00 01 37 20 20 55 68 00 76 00 9A 41 11 00 22 00 77 00 00 00 00 00 00 00 00
@@ -225,11 +225,8 @@
    }
}

static void ParseTimeout(union sigval sig) {
    AppTimer_delete(ParseTimeout);
    packet_parse_status[sig.sival_int] = PKT_SYNC_HEAD;

    DEBUG("ParseTimeout %d", sig.sival_int);
static void ParseTimeout(apptimer_var_t val) {
    packet_parse_status[val.var1] = PKT_SYNC_HEAD;
}

static uint32_t GetResendTimeout(uint8_t access, uint8_t curr_resend_cnt, uint32_t base_time)
@@ -252,10 +249,10 @@
    static uint16_t TxSeq[DATA_ACCESS_END] = {0};
    uint16_t seq;

    pthread_mutex_lock(&seq_mutex);
    lock_guard<std::mutex> lock(seq_mutex);

    seq = TxSeq[access];
    TxSeq[access]++;
    pthread_mutex_unlock(&seq_mutex);

    return seq;
}
@@ -534,7 +531,7 @@

    DEBUG("AddTxQueue id = 0x%X, seq = %d, len = %d", id, seq, length);

    pthread_mutex_lock(&tx_queue_mutex);
    lock_guard<mutex> lock(tx_queue_mutex);

    // If the item is exist, skip it
    if (FindTxQueue(head, id, seq) != NULL) {
@@ -595,7 +592,6 @@
    }

    ATQ_END:
    pthread_mutex_unlock(&tx_queue_mutex);

    uint32_t tim = GetResentTimeoutTxQueue(&message_tx_table_head[DATA_ACCESS_PLATFORM]);

@@ -614,7 +610,7 @@

    message_tx_table_t *ptr = *head;

    pthread_mutex_lock(&tx_queue_mutex);
    lock_guard<mutex> lock(tx_queue_mutex);
    while ( ptr != NULL ) {
        if (ptr->id == id && ptr->seq == seq) {
            // delete
@@ -642,7 +638,6 @@
            ptr = ptr->next;
        }
    }
    pthread_mutex_unlock(&tx_queue_mutex);
}

static void RemoveTxQueue(message_tx_table_t **head, uint16_t id)
@@ -652,7 +647,7 @@

    message_tx_table_t *ptr = *head;

    pthread_mutex_lock(&tx_queue_mutex);
    lock_guard<mutex> lock(tx_queue_mutex);
    while ( ptr != NULL ) {
        if (ptr->id == id) {
            // delete
@@ -681,7 +676,6 @@
            ptr = ptr->next;
        }
    }
    pthread_mutex_unlock(&tx_queue_mutex);
}

static void RemoveAllTxQueue(message_tx_table_t **head)
@@ -690,13 +684,12 @@
        return;
    }

    pthread_mutex_lock(&tx_queue_mutex);
    lock_guard<mutex> lock(tx_queue_mutex);
    for (message_tx_table_t *ptr = *head, *next; ptr != NULL; ptr = next) {
        next = ptr->next;
        free(ptr);
    }
    *head = NULL;
    pthread_mutex_unlock(&tx_queue_mutex);
}

static int SendQueue(message_tx_table_t *item)
@@ -704,11 +697,7 @@
    DEBUG("SendQueue id = 0x%04X, seq = %d, length = %d", item->id, item->seq, item->length);
    if (item != NULL) {
        if (item->access == DATA_ACCESS_MCU) {
            if (WriteSerialPort(GetSerialPort(UART_1), item->data, item->length) != item->length) {
                item->time_out = 100 + AppTimer_GetTickCount();
                LOGE("发往串口出错了");
//                return -1;
            }

        } else if(item->access == DATA_ACCESS_PLATFORM) {
            if (WritePlatform(item->data, item->length) != item->length) {
                item->time_out = D_SEC(3) + AppTimer_GetTickCount();
@@ -769,8 +758,8 @@
                // delete
                DEBUG("Delete item %d", ptr->curr_cnt);
                message_tx_table_t *temp = ptr;
                lock_guard<mutex> lock(tx_queue_mutex);

                pthread_mutex_lock(&tx_queue_mutex);
                if (ptr == *head) {
                    if (ptr->next == NULL) {
                        DEBUG("****************** Delete all 2 ******************");
@@ -787,7 +776,6 @@
                    ptr->prev->next = ptr->next;
                    ptr = ptr->next;
                }
                pthread_mutex_unlock(&tx_queue_mutex);

                free(temp);
            } else {
@@ -807,7 +795,7 @@
    if (head == NULL)
        return resentTime;

    pthread_mutex_lock(&tx_queue_mutex);
    lock_guard<mutex> lock(tx_queue_mutex);
    message_tx_table_t *ptr = *head;

    while ( ptr != NULL ) {
@@ -819,14 +807,13 @@
        }
        ptr = ptr->next;
    }
    pthread_mutex_unlock(&tx_queue_mutex);

    return resentTime;
}

static sem_t sem_tx_mgr;

static void *TxQueueMgrThread(void *p) {
static void TxQueueMgrThread(void) {
    while (true) {
        sem_wait(&sem_tx_mgr);
        uint32_t tim = AppTimer_GetTickCount();
@@ -844,11 +831,9 @@
            AppTimer_add(TriggerResendTxQueue, tim);
        }
    }
    pthread_exit(NULL);
}

static void TriggerResendTxQueue(union sigval sig) {
    AppTimer_delete(TriggerResendTxQueue);
static void TriggerResendTxQueue(apptimer_var_t val) {
    sem_post(&sem_tx_mgr);
}

@@ -864,15 +849,9 @@
    PhoneNumber[6] = 0x00;
    PhoneNumber[7] = 0x02;*/

    pthread_mutex_init(&seq_mutex, NULL);
    pthread_mutex_init(&tx_queue_mutex, NULL);
    sem_init(&sem_tx_mgr, 0, 0);

    pthread_t pid;
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
    pthread_create(&pid, &attr, TxQueueMgrThread, NULL);
    std::thread(TxQueueMgrThread).detach();
}

void SetPlatformTxPhoneNum(const char *phone)

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

@@ -5,7 +5,8 @@
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <pthread.h>
#include <thread>
#include <mutex>
#include <semaphore.h>
#include <cmath>
#include "platform.h"
@@ -88,7 +89,7 @@

static CTcpPort *ctp = NULL;

static pthread_mutex_t events_mutex = PTHREAD_MUTEX_INITIALIZER;
static std::mutex events_mutex;

static struct gpsBrief gbf;
static struct rtkBrief rbf;
@@ -101,24 +102,24 @@
static struct event_t * FetchEvent(void);
static void RemoveEvent(void);

static void ReqRtkPlatformConfigTimeout(union sigval sig);
static void ReqRtkPlatformConfigTimeout(apptimer_var_t val);

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 ConnectPlatformLater(apptimer_var_t val);

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

static void RegisterPlatform(void);
static void RegisterPlatformTimeout(union sigval sig);
static void RegisterPlatformTimeout(apptimer_var_t val);

static void LoginPlatform(void);
static void LoginPlatformTimeout(union sigval sig);
static void TriggerHeartbeat(union sigval sig);
static void RequestRtkNoResp(union sigval sig);
static void LoginPlatformTimeout(apptimer_var_t val);
static void TriggerHeartbeat(apptimer_var_t val);
static void RequestRtkNoResp(apptimer_var_t val);


static void AddEvnet(uint32_t event, const uint8_t *data, int length)
@@ -138,7 +139,7 @@
        memcpy(nw->event.data, data, length);
    }

    pthread_mutex_lock(&events_mutex);
    lock_guard<std::mutex> lock(events_mutex);

    struct event_queue_t *p = eventQueue;

@@ -148,22 +149,18 @@
        eventQueue = nw;
    else
        p->next = nw;

    pthread_mutex_unlock(&events_mutex);
}

static struct event_t * FetchEvent(void)
{
    struct event_t *evp = NULL;

    pthread_mutex_lock(&events_mutex);
    lock_guard<std::mutex> lock(events_mutex);
    struct event_queue_t *p = eventQueue;

    if (p != NULL) {
        evp = &p->event;
    }

    pthread_mutex_unlock(&events_mutex);

    return evp;
}
@@ -172,7 +169,7 @@
{
    void *p1 = NULL, *p2 = NULL;

    pthread_mutex_lock(&events_mutex);
    lock_guard<std::mutex> lock(events_mutex);
    struct event_queue_t *p = eventQueue;

    if (p != NULL) {
@@ -180,7 +177,6 @@
        p1 = p;
        p2 = p->event.data;
    }
    pthread_mutex_unlock(&events_mutex);

    if (p2 != NULL)
        free(p2);
@@ -198,8 +194,6 @@
    memset(&rbf, 0, sizeof(rbf));
    memset(&defaultMcuRom, 0, sizeof(defaultMcuRom));

    pthread_mutex_init(&events_mutex, NULL);

    eventQueue = NULL;

    memset(&platformStatus, 0, sizeof(platformStatus));
@@ -209,17 +203,11 @@

    PlatformTxInit();

    pthread_t pid;
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
    pthread_create(&pid, &attr, StatusListenThread, NULL);
    std::thread(StatusListenThread).detach();
}

static void ReqRtkPlatformConfigTimeout(union sigval sig)
static void ReqRtkPlatformConfigTimeout(apptimer_var_t val)
{
    AppTimer_delete(ReqRtkPlatformConfigTimeout);

    AppTimer_add(ReqRtkPlatformConfigTimeout, D_SEC(2));
    MA_ReqRtkPlatformConfig();
}
@@ -327,12 +315,11 @@
    }
}

static void ConnectPlatformLater(union sigval sig) {
    AppTimer_delete(ConnectPlatformLater);
static void ConnectPlatformLater(apptimer_var_t val) {
    ConnectPlatform(exceptSocket.domain_name, exceptSocket.port);
}

static void *StatusListenThread(void *p) {
static void StatusListenThread(void) {
    while (true) {
        sem_wait(&sem_status_changed);

@@ -415,6 +402,8 @@
        case GPS_UPDATE_EVT: {
            const gpsStatus_t *gps = (gpsStatus_t *) data;

            DEBUG("GPS: %s", const_cast<gpsStatus_t *>(gps)->toString().c_str());

            gbf.qf = gps->gps_status;
            gbf.latitude = gps->latitude;
            gbf.longitude = gps->longitude;
@@ -434,6 +423,7 @@

                MA_SendRtkBrief(&rbf);
            }

//        MA_SendGpsBrief(&brief);
            RequestRtkDownload(gps, 1);
            break;
@@ -443,6 +433,8 @@
            uint32_t ost1 = ost;

            const rtk_info *rtk = (rtk_info *) data;

            DEBUG("RTK: %s", const_cast<rtk_info *>(rtk)->toString().c_str());

            rbf.qf = rtk->qf;
            rbf.coord_x = rtk->y;
@@ -517,7 +509,7 @@
                if (strlen(defaultMcuRom.verCode) > 0 && ver.size() >= 4 &&
                    strcmp(defaultMcuRom.verCode, ver[3].c_str()) != 0 &&
                    defaultMcuRom.rom != NULL) {
                    UploadDfuFile(defaultMcuRom.rom, defaultMcuRom.length);
                    /////////////////UploadDfuFile(defaultMcuRom.rom, defaultMcuRom.length);
                    delete[]defaultMcuRom.rom;
                    defaultMcuRom.rom = NULL;
                }
@@ -678,7 +670,7 @@
                if (sta == 3) {
                    // Connected
                    btConnected = true;
                    ParseMcuInit();
                    ////////////////////ParseMcuInit();
                    ConfigRTKModuleLater();

                    PlayTTS("蓝牙连接", NULL);
@@ -713,7 +705,7 @@
            if (Virtual2IsConnected()) {

            } else {
                ParseMcu(data, length);
                ///////////ParseMcu(data, length);
            }
            break;
        }
@@ -737,10 +729,10 @@
    return ret;
}

static void RegisterPlatformTimeout(union sigval sig)
static void RegisterPlatformTimeout(apptimer_var_t val)
{
    DEBUG("RTK平台注册超时");
    AppTimer_delete(RegisterPlatformTimeout);

    RegisterPlatform();
}

@@ -753,19 +745,17 @@
                       strlen((char *)deviceInfo.device_model), deviceInfo.device_sn, deviceInfo.imei);
}

static void TriggerHeartbeat(union sigval sig) {
    AppTimer_delete(TriggerHeartbeat);

static void TriggerHeartbeat(apptimer_var_t val) {
    if (platformStatus.login && platformStatus.connected) {
        SendHeartBeat(DATA_ACCESS_PLATFORM);
        AppTimer_add(TriggerHeartbeat, D_SEC(30));
    }
}

static void LoginPlatformTimeout(union sigval sig)
static void LoginPlatformTimeout(apptimer_var_t val)
{
    DEBUG("RTK平台登录超时");
    AppTimer_delete(LoginPlatformTimeout);

    LoginPlatform();
}

@@ -829,9 +819,8 @@
    AppTimer_add(RequestRtkNoResp, D_SEC(7));           // 应该每秒收到一次
}

static void RequestRtkNoResp(union sigval sig)
static void RequestRtkNoResp(apptimer_var_t val)
{
    AppTimer_delete(RequestRtkNoResp);
    requestPlatformSendRtk = true;
    rtcmLength = 0;
}

 lib/src/main/cpp/test_common/Geometry.cpp

@@ -23,22 +23,22 @@
const double EPSILON = 1e-6;
const double EPSILON2 = 1e-3;

inline bool isEqual(double a, double b)
 bool isEqual(double a, double b)
{
    return (fabs(a - b) <= EPSILON);
}

inline bool isEqual2(double a, double b)
 bool isEqual2(double a, double b)
{
    return (fabs(a - b) <= EPSILON2);
}

inline double toRadians(double degree)
 double toRadians(double degree)
{
    return (degree * M_PI) / 180.0;
}

inline double toDegree(double radians)
 double toDegree(double radians)
{
    return (radians * 180.0) / M_PI;
}

 lib/src/main/cpp/test_common/Geometry.h

@@ -34,10 +34,10 @@
    PointF *point;
} Polygon;

inline double toRadians(double degree);
inline double toDegree(double radians);
inline bool isEqual(double a, double b);
inline bool isEqual2(double a, double b);
 double toRadians(double degree);
 double toDegree(double radians);
 bool isEqual(double a, double b);
 bool isEqual2(double a, double b);

void MakeLine(Line *line, const PointF *p1, const PointF *p2);
void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set);

 lib/src/main/cpp/test_common/car_sensor.cpp

@@ -2,7 +2,7 @@
// Created by fctom on 2020/2/13.
//

#include <pthread.h>
#include <mutex>
#include "car_sensor.h"
#include "../driver_test.h"
#include "../defs.h"
@@ -57,14 +57,14 @@
static int SensorNum = 0;

static car_sensor_t Sensor;
static pthread_mutex_t sonser_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t status_rw_mutex = PTHREAD_MUTEX_INITIALIZER;
static std::mutex sonser_mutex;
static std::mutex status_rw_mutex;

inline static int BX(int value);
static void WriteCarStatus(uint16_t id, int value);

static void ConfirmTurnSigalLater(union sigval sig);
static void flashBeamLightClose(union sigval sig);
static void ConfirmTurnSigalLater(apptimer_var_t val);
static void flashBeamLightClose(apptimer_var_t val);

static void SensorChanged(int id, int value);

@@ -82,8 +82,6 @@

    left_turn_signal = right_turn_signal = false;

    pthread_mutex_init(&sonser_mutex, NULL);
    pthread_mutex_init(&status_rw_mutex, NULL);
}

void SetSensorCfg(int (*sensor)[3], int sensorNum)
@@ -124,10 +122,10 @@
{
    uint16_t chg = 0;

    pthread_mutex_lock(&sonser_mutex);
    sonser_mutex.lock();
    chg = gpioStore^gpio;
    gpioStore = gpio;
    pthread_mutex_unlock(&sonser_mutex);
    sonser_mutex.unlock();

    WriteCarStatus(OBD_SPEED, speed);
    WriteCarStatus(ENGINE_RPM, rpm);
@@ -218,9 +216,9 @@
{
    int value;

    pthread_mutex_lock(&status_rw_mutex);
    lock_guard<std::mutex> lock(status_rw_mutex);

    value = CarStatus[id];
    pthread_mutex_unlock(&status_rw_mutex);

    return value;
}
@@ -228,10 +226,9 @@
static void WriteCarStatus(uint16_t id, int value)
{
    int old_value;
    pthread_mutex_lock(&status_rw_mutex);
    lock_guard<std::mutex> lock(status_rw_mutex);
    old_value = CarStatus[id];
    CarStatus[id] = value;
    pthread_mutex_unlock(&status_rw_mutex);

    if (id != OBD_SPEED && id != ENGINE_RPM && old_value != value) {
        uint8_t buffer[6];
@@ -247,10 +244,8 @@
    }
}

static void ConfirmTurnSigalLater(union sigval sig)
static void ConfirmTurnSigalLater(apptimer_var_t val)
{
    AppTimer_delete(ConfirmTurnSigalLater);

    DEBUG("确认转向灯 左 %d 右 %d", left_turn_signal, right_turn_signal);

    if (!left_turn_signal && !right_turn_signal) {
@@ -264,7 +259,7 @@
    }
}

static void flashBeamLightClose(union sigval sig)
static void flashBeamLightClose(apptimer_var_t val)
{
    WriteCarStatus(FLASH_BEAM_LAMP, OFF_LIGHT);
}

 lib/src/main/cpp/test_items/area_exam.cpp

@@ -15,7 +15,7 @@
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"

#include "../test_common/Geometry.h"
#define DEBUG(fmt, args...)     LOGD("<area_exam> <%s>: " fmt, __func__, ##args)

static int CurrExamStatus = EXAM_AREA_NONE;      // 1 测试完成 0 测试中 -1 测试错误退出

 lib/src/main/cpp/test_items/stop_and_start.cpp

@@ -77,6 +77,7 @@

int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    static double distanceToStopLine = 0, distanceToEdge = 0;
    if (!testing)
        return 0;

@@ -100,18 +101,6 @@
        testing = false;
    }

    // 距离检测
    {
        vector<double> dtox;
        vector<Line> line_set;
        Line distance_line;

        MakeLine(&distance_line, &map->point[0], &map->point[8]);
        line_set.push_back(distance_line);
        DistanceOfTire2X(dtox, car, line_set);
        MA_SendDistance(dtox[0], dtox[1]);
    }

    if (prevMoveDirect != moveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
@@ -132,26 +121,26 @@

            stopPoint = car->carXY[car->body[0]];

            double dis1 = DistanceOfHead2Stopline(map, car);
            double dis2 = DistanceOfTire2Edge(map, car);
            distanceToStopLine = DistanceOfHead2Stopline(map, car);
            distanceToEdge = DistanceOfTire2Edge(map, car);

            DEBUG("DIS1 = %f  DIS2 = %f", dis1, dis2);
            DEBUG("DIS1 = %f  DIS2 = %f", distanceToStopLine, distanceToEdge);

            if (dis1 > examParam.ramp_stoppoint_red_distance) {
            if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
                // 距离停止线前后超出50厘米
                AddExamFault(20301, rtkTime);
                DEBUG("距离停止线前后超出50厘米，不合格");
            } else if (fabs(dis1) > EPSILON) {
            } else if (fabs(distanceToStopLine) > EPSILON) {
                // 前保险没有位于停止带内，但没有超出50厘米，扣10分
                AddExamFault(20304, rtkTime);
                DEBUG("前保险没有位于停止带内，但没有超出50厘米");
            }

            if (dis2 > examParam.ramp_edge_red_distance) {
            if (distanceToEdge > examParam.ramp_edge_red_distance) {
                // 距离边线超出50厘米,不合格
                AddExamFault(20302, rtkTime);
                DEBUG("距离边线超出50厘米");
            } else if (dis2 > examParam.ramp_edge_yellow_distance) {
            } else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
                // 距离边线超出30厘米，扣10分
                AddExamFault(20305, rtkTime);
                DEBUG("距离边线超出30厘米");
@@ -161,6 +150,20 @@
        if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
            handBreakActive = true;
        }
    }

    if (!stopCar) {
        // 距离检测
            vector<double> dtox;
            vector<Line> line_set;
            Line distance_line;

            MakeLine(&distance_line, &map->point[0], &map->point[8]);
            line_set.push_back(distance_line);
            DistanceOfTire2X(dtox, car, line_set);
            MA_SendDistance(dtox[0], dtox[1]);
    } else {
        MA_SendDistance(distanceToStopLine, distanceToEdge);
    }

    // 判断起步后滑状态
@@ -289,6 +292,7 @@
    double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);

//    return (l1+l2)/2.0;

    return MAX(l1, l2);     // 取最远的
}


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

@@ -21,7 +21,7 @@

static bool testing;

static void CheckSolution(union sigval sig);
static void CheckSolution(apptimer_var_t val);
static void ExamDummyLight(void);

void StartDummyLightExam(struct dummy_light_exam *ptr, int num, const struct RtkTime* rtkTime)
@@ -101,10 +101,8 @@
}

// 检查最终状态
static void CheckSolution(union sigval sig)
static void CheckSolution(apptimer_var_t val)
{
    AppTimer_delete(CheckSolution);

    if (content[question].process.size() > 0) {
        if (content[question].process.size() != process.size()) {
            AddExamFault(content[question].wrongCode, &currRtkTime);

 lib/src/main/cpp/test_items2/prepare.cpp

@@ -16,8 +16,9 @@
static bool exec = false;

static void TtsBack(int seq);
static void PrepareTimeout(union sigval sig);
static void PrepareTimeout2(union sigval sig);
static void PrepareTimeout(apptimer_var_t val);
static void PrepareTimeout2(apptimer_var_t val);

void StartPrepare(void)
{
    exec = true;
@@ -91,17 +92,14 @@
    hint_cnt++;
}

static void PrepareTimeout(union sigval sig)
static void PrepareTimeout(apptimer_var_t val)
{
    DEBUG("上车准备超时");
    AppTimer_delete(PrepareTimeout);
    exec = false;
    PrepareOver(-2);
}

static void PrepareTimeout2(union sigval sig)
static void PrepareTimeout2(apptimer_var_t val)
{
    AppTimer_delete(PrepareTimeout2);

    PlayTTS(examParam.prepare_tts, TtsBack);
}

 lib/src/main/cpp/utils/num.cpp

@@ -97,3 +97,12 @@
    ss >> number;
    return number;
}

string intw(int n, int w)
{
    stringstream ss;

    ss << setfill('0') << setw(w) << n;

    return ss.str();
}

 lib/src/main/cpp/utils/num.h

@@ -6,10 +6,12 @@
#define RTKDRIVERTEST_NUM_H

#include <cstdint>
#include <string>

long str2int(const uint8_t *s, uint16_t length);
bool str2float(double *f, const uint8_t *s, uint16_t length);
int BitCount(uint32_t n);
double round(double number, unsigned int bits);
std::string intw(int n, int w);

#endif //RTKDRIVERTEST_NUM_H