智慧驾培首次提交

yy1717

2022-12-08 f7a18ec4494b9c5c9ef3fd440bbf68ffc6425e18

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