Cockpit-cpp/EgoQt/EgoDLL/control_sensor.cpp

#define WIN32_LEAN_AND_MEAN
#include <WinSock2.h>
#include "PCANBasic.h"
#include "./include/EgoInterface.h"
#include "EgoClient.h"
#include "EgoWindow.h"
#include "protocol.pb.h"

#include "../common/iobuffer.h"
#include "protocol.pb.h"
#include "control_sensor.h"
#include <algorithm>
#include <string>

#define MAX_DGREE (int32_t)(4096/6)
inline int16_t make_int16(int8_t h, int8_t l)
{
    int16_t hi = (int16_t)(h & 0x00FF);
    int16_t low = (int16_t)(l & 0x00FF);

    return (hi << 8) | low;
}
inline int8_t hi_byte(int16_t value)
{
    int8_t hi =(int8_t)((value & 0xFF00) >> 8);
    return hi;
}
inline int8_t lo_byte(int16_t value)
{
    int8_t lo = (int8_t)(value & 0xff);
    return lo;
}
CControlSensor::CControlSensor(CMessageQueue* q)
{
    _window = static_cast<CEgoWindow*>(q);
    _collection = Sensor_Collection::Sensor_None;
}


void CControlSensor::Notify(TPCANMsg& canmsg, uint64_t timestamp)
{
    //OutputDebugString(str);
    //OutputDebugString(ret.c_str());
    //0x181   0x281    0x381   0x481   0x185   0x701
    //打印
    //OutputDebugStringA(("===================打印ID " + std::to_string(canmsg.ID) + "============\n").c_str());
    switch (canmsg.ID)
    {
        case 0x181:
        {
            //此处需要改成QT上位机按钮控制_automous
            if ((frames[2].data[7] & 0x1) != 1)
            {
                //取按钮的值
                isFront = (canmsg.DATA[1] & 0x1) == 1;
                frames[0].canid = canmsg.ID;
                frames[0].dlc = canmsg.LEN;

                memcpy(&frames[0].data, canmsg.DATA, sizeof(canmsg.DATA));
                if (_remote_active)
                {
                    frames[0].data[0] = 0x10;
                    _remote_active = false;
                    _active_tick = GetTickCount64();
                }

            }
            else
            {
                frames[0].canid = 0;
            }
            _collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_ER);
        }
            break;

        case 0x281:
        {
            frames[2].canid = canmsg.ID;
            frames[2].dlc = canmsg.LEN;
            memcpy(&frames[2].data, canmsg.DATA, sizeof(canmsg.DATA));
            _collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Signal);


            // 添加打印语句
            /*std::string receivedData = "Received data: ";
            for (int i = 0; i < canmsg.LEN; i++)
            {
                receivedData += std::to_string(canmsg.DATA[i]) + " ";
            }
            receivedData += "\n";
            OutputDebugStringA(receivedData.c_str());*/

        }
            break;

        case 0x381:
        {
            if ((frames[2].data[7] & 0x1) != 1)
                //QT按钮增加判断是遥操还是监控
                //if (_automous == true)
            {
                frames[3].canid = canmsg.ID;
                frames[3].dlc = canmsg.LEN;
                /*	if (isSliping)
                    {
                        canmsg.DATA[0] =  100;
                        canmsg.DATA[1] = 100;
                    }
                    */
                memcpy(&frames[3].data, canmsg.DATA, sizeof(canmsg.DATA));
            }
            else
            {
                frames[3].canid = 0;
            }
            _collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Drive);
        }
            break;
        case 0x1A0:
        {
            if ((frames[2].data[7] & 0x1) != 1)
            {
                int32_t steer = make_int16(canmsg.DATA[1], canmsg.DATA[0]);
                //OutputDebugStringA(("=====" + std::to_string(steer) + "=====\n").c_str());
                float period = steer - _old_steer;
                _old_steer = steer;
                float  diff = timestamp - _old_timestamp;
                diff /= 1000.f;
                diff /= 1000.f;

                _old_timestamp = timestamp;
                //float diff = tick - _cur_tick;
                frames[1].canid = canmsg.ID;
                frames[1].dlc = 8;
                //原来的脉冲值是每500秒
                //int32_t speed = (period / (diff * 2.0f));
                //改成每50毫秒的脉冲值
                int32_t speed = (period / (diff*40.0f));

                if (speed == 0)
                {
                    auto tick = GetTickCount64();
                    if (_remote_active == false && tick - _active_tick > 500)
                    {
                        _remote_active = true;
                    }
                }
                if (_window->GetSteerAngle() < -MAX_DGREE)
                {
                    if (isFront)
                    {
                        if (speed < 0)
                            speed = 0;
                    }
                    else
                    {
                        if (speed > 0)
                            speed = 0;
                    }
                }
                else if (_window->GetSteerAngle() > MAX_DGREE)
                {
                    if (isFront)
                    {
                        if (speed > 0)
                            speed = 0;
                    }
                    else {
                        if (speed < 0)
                            speed = 0;
                    }
                }
                else
                {
                    if (_last_speed != 0xF000)
                    {
                        if ((_last_speed * speed) < 0)
                        {
                            _last_speed = speed;
                            speed = 0;
                        }
                    }
                    else
                    {
                        _last_speed = speed;
                    }
                }

                frames[1].data[0] = 0xFF;
                frames[1].data[1] = 0x00;
                if (!isFront)
                    speed = (speed * -1);
                //OutputDebugStringA(("========速度" + std::to_string(speed) + "脉冲每50毫秒============\n").c_str());
                frames[1].data[2] = hi_byte(speed);
                frames[1].data[3] = lo_byte(speed);
                if (isFront)
                    frames[1].data[4] = speed < 0 ? 1 : 0;
                else
                    frames[1].data[4] = speed < 0 ? 0 : 1;
                frames[1].data[4] |= 0x02;
                frames[1].data[5] = 0;
                frames[1].data[6] = 0;
                frames[1].data[7] = 0;
            }
            else
            {
                frames[1].canid = 0;
            }
            _collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Steer);
        }
            break;
        case 0x481:
        {
            if ((frames[2].data[7] & 0x1) != 1)
            {

                frames[4].canid = canmsg.ID;
                frames[4].dlc = canmsg.LEN;
                int16_t d = canmsg.DATA[0];
                int16_t l = canmsg.DATA[2];
                int16_t k = canmsg.DATA[4];
                isSliping = std::abs(d) > 10 || std::abs(l) > 10 || std::abs(k) > 10;

                memcpy(&frames[4].data, canmsg.DATA, sizeof(canmsg.DATA));
                _collection = (Sensor_Collection)(_collection | Sensor_Collection::Sensor_Arm);
            }
            else
            {
                frames[4].canid = 0;
            }

        }
            break;
    }

    if (_window->GetControlState() == ControlState::Process && _collection == Sensor_Collection::Sensor_All)
    {
        static DWORD tick = GetTickCount();
        RemoNet::CCCanMsg req;
        for (int32_t i = 0; i < _countof(frames); i++)
        {
            if (frames[i].canid != 0)
            {
                RemoNet::can_net_frame* frame = req.add_frams();
                frame->set_canid(frames[i].canid);
                frame->set_dlc(frames[i].dlc);
                frame->set_data(frames[i].data, frames[i].dlc);
            }
        }
        MessageHead Head;
        CIOBuffer pBuffer;
        Head.Command = RemoNet::CC_CANMSG;
        Head.Length = req.ByteSizeLong();
        Head.Serialize(pBuffer.Buffer);
        auto ptr = pBuffer.Buffer + MessageHead::Size();
        req.SerializeToArray(ptr, Head.Length);
        pBuffer.Length = MessageHead::Size() + Head.Length;
        //使用pcan发送给下位机
        //_window->SendData(pBuffer);
        //使用udp发送给下位机
        _window->WriteCan(&pBuffer);


        //OutputDebugStringA(("=================" + std::to_string((frames[2].data[7] & 0x1) != 1) + "=======\n").c_str());


        _collection = Sensor_Collection::Sensor_None;
    }
}

ControlStatus CControlSensor::CheckStatus()
{
    return ControlStatus::Ok;
}

bool CControlSensor::Start()
{
    int iBuffer;
    TPCANStatus stsResult;
    TPCANHandle _HandlesArray[16];
    _HandlesArray[0] = PCAN_USBBUS1;
    _HandlesArray[1] = PCAN_USBBUS2;
    _HandlesArray[2] = PCAN_USBBUS3;
    _HandlesArray[3] = PCAN_USBBUS4;
    _HandlesArray[4] = PCAN_USBBUS5;
    _HandlesArray[5] = PCAN_USBBUS6;
    _HandlesArray[6] = PCAN_USBBUS7;
    _HandlesArray[7] = PCAN_USBBUS8;
    _HandlesArray[8] = PCAN_USBBUS9;
    _HandlesArray[9] = PCAN_USBBUS10;
    _HandlesArray[10] = PCAN_USBBUS11;
    _HandlesArray[11] = PCAN_USBBUS12;
    _HandlesArray[12] = PCAN_USBBUS13;
    _HandlesArray[13] = PCAN_USBBUS14;
    _HandlesArray[14] = PCAN_USBBUS15;
    _HandlesArray[15] = PCAN_USBBUS16;
    for (int i = 0; i < (sizeof(_HandlesArray) / sizeof(TPCANHandle)); i++)
    {
        stsResult = CAN_GetValue(_HandlesArray[i], PCAN_CHANNEL_CONDITION, &iBuffer, sizeof(iBuffer));
        if (((stsResult) == PCAN_ERROR_OK) && ((iBuffer & PCAN_CHANNEL_AVAILABLE) == PCAN_CHANNEL_AVAILABLE))
        {
            stsResult = CAN_GetValue((TPCANHandle)_HandlesArray[i], PCAN_CHANNEL_FEATURES, (void*)&iBuffer, sizeof(iBuffer));
            _isFD = (stsResult == PCAN_ERROR_OK) && (iBuffer & FEATURE_FD_CAPABLE);
            _handle = _HandlesArray[i];
            break;
        }
    }
    if (_handle != -1)
    {
        //波特率设置位250k
        TPCANStatus stsResult = CAN_Initialize(_handle, PCAN_BAUD_250K);
        if (stsResult != PCAN_ERROR_OK)
        {
            return false;
        }
        _thread = std::thread(&CControlSensor::Run, this);
    }
    return _handle != -1;
}

void CControlSensor::Run()
{
    _run = true;
    while (_run)
    {
        for (int32_t i = 0; i < sizeof(frames) / sizeof(cannet_frame); i++)
        {
            frames[i].canid = 0;
        }
        Sleep(10);
        ReadMessages();
    }
    CAN_Uninitialize(_handle);

}

void CControlSensor::ReadMessages()
{
    TPCANStatus stsResult;

    // We read at least one time the queue looking for messages. If a message is found, we look again trying to
    // find more. If the queue is empty or an error occurr, we get out from the dowhile statement.
    do
    {
        ReadMessage();
        if (stsResult != PCAN_ERROR_OK && stsResult != PCAN_ERROR_QRCVEMPTY)
        {
            //ShowStatus(stsResult);
            return;
        }
    } while (!(stsResult & PCAN_ERROR_QRCVEMPTY));

}

//读取can消息并处理
TPCANStatus CControlSensor::ReadMessage()
{
    TPCANMsg CANMsg;                 //存放接收到的can消息
    TPCANTimestamp CANTimeStamp;     //存放can消息时间戳

    // 读取can消息并存放在CANMsg和CANTimeStamp中，函数返回一个TPCANStatus类型状态值
    TPCANStatus stsResult = CAN_Read(_handle, &CANMsg, &CANTimeStamp);

    //检查是否成功接受到can信息，成功则计算新时间戳并调用Notify函数
    if (stsResult != PCAN_ERROR_QRCVEMPTY)
    {	// We process the received message
        uint64_t newTimestamp = (CANTimeStamp.micros + 1000 * CANTimeStamp.millis + 0x100000000 * 1000 * CANTimeStamp.millis_overflow);
        Notify(CANMsg, newTimestamp);
    }

    return stsResult;
}

//将PCAN信号传回座椅PLC中
TPCANStatus CControlSensor::SendCANMessage(TPCANMsg* CANMsg)
{
    // 执行PCANBasic的“写入”功能
    TPCANStatus stsResult = CAN_Write(_handle, CANMsg);
    return stsResult;
}
void CControlSensor::Stop()
{
    _run = false;
    _thread.join();
}
void CControlSensor::SetEngineRPM(int32_t value)
{
    _rpm = value;
}