Vehicle-cpp/remote_drive/EgoZJ/EgoSystem/robot_sensor.cpp

#include <stdint.h>
#include "../common/comm.h"
#include "../common/iobuffer.h"
#include "../common/sensor_socket.h"
#include "protocol.pb.h"
#include "robot_sensor.h"
#include "message_queue.h"
#include "Rtk.h"

#include <thread>
#include <math.h>

struct can_array
{
    int32_t count;
    cannet_frame frames[1];
};
#pragma pack()

uint8_t HIBYTE(int16_t value)
{
  return (uint8_t)((value>>8)&0xFF);
}
uint8_t LOBYTE(int16_t value)
{
  return (uint8_t)((value)&0xFF);
}

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

float b2f(uint8_t m0, uint8_t m1, uint8_t m2, uint8_t m3)
{
    //求符号位
    float sig = 1.;
    if (m0 >=128.)
        sig = -1.;
  
    //求阶码
    float jie = 0.;
     if (m0 >=128.)
    {
        jie = m0-128.  ;
    }
    else
    {
        jie = m0;
    }
    jie = jie * 2.;
    if (m1 >=128.)
        jie += 1.;

    jie -= 127.;
 
    //求尾码
 
    float tail = 0.;
    if (m1 >=128.)
        m1 -= 128.;
    tail =  m3 + (m2 + m1 * 256.) * 256.;
    tail  = (tail)/8388608;   //   8388608 = 2^23
 
    float f = sig * pow(2., jie) * (1+tail);
 
    return f;
}

CRobotSensor::CRobotSensor(CMessageQueue * q):_message(q)
{
    for (int32_t i = 0; i < 4; i++)
        memset(&_msg[i], 0, sizeof(TPCANMsg));
}

void CRobotSensor::Start()
{
    _message->btimeStopedCar = false;
    _run = true;
    //_can_thread = std::thread(&CRobotSensor::CanSendProc, this);
}

void CRobotSensor::Stop()
{
     /*_run = false;

     auto tn = _can_thread.native_handle();
     if(tn != 0)
        _can_thread.join();*/
    if (_run)
    {
        _run = false;
        //_can_thread.join();
    }

    for (int32_t i = 0; i < 4; i++)
        memset(&_msg[i], 0, sizeof(TPCANMsg));
        

} 

void CRobotSensor::SendStatusToMSG()
{
    RemoNet::State req;
    /*
    req.set_engine_speed(_data.Engine_speed);
    req.set_travel_speed(_data.Travel_speed);
    req.set_fuel_level(_data.Fuel_level);
    req.set_engine_temperature(_data.Engine_temperature);
    req.set_hydraulic_oil_temperature(_data.Hydraulic_oil_temperature);
    req.set_main_pump_1_pressure(_data.Main_pump_1_pressure);
    req.set_main_pump_2_pressure(_data.Main_pump_2_pressure);
    req.set_hand_gear(_data.Hand_gear);
    req.set_actual_gear(_data.Actual_gear);
    req.set_gripper_height(_data.Gripper_height);
    req.set_amplitude(_data.amplitude);
    req.set_boom_angle(_data.Boom_angle);
    req.set_stick_angle(_data.Stick_angle);

    req.set_idle_protection(_data.Idle_protection);
    req.set_front_toggle(_data.Front_toggle);
    req.set_back_toggle(_data.Back_toggle);
    */
    //std::string s;
    //s.assign((char*)_data.Error_Buff);
    //req.set_error_buff(s.c_str());

    req.set_error_buff((char *)_data.Error_Buff,8);

    //req.set_error_buff(s,s.size());
    /*
    req.set_low_oil_pressure(_data.Low_oil_pressure);
    req.set_low_coolant_level(_data.Low_coolant_level);
    req.set_high_coolant_level(_data.High_coolant_level);
    req.set_hydraulic_oil_temperature_high(_data.hydraulic_oil_temperature_high);
    req.set_hydraulic_oil_temperature_low(_data.hydraulic_oil_temperature_low);
    req.set_accumulator_pressure_abnormal(_data.accumulator_pressure_abnormal);
    req.set_engine_spn_failure(_data.Engine_SPN_failure);
    req.set_engine_warming_up(_data.engine_warming_up);
    req.set_engine_warming_up_timeout(_data.engine_warming_up_timeout);
    req.set_low_battery_voltage(_data.Low_battery_voltage);
    req.set_high_battery_voltage(_data.High_battery_voltage);
    req.set_system_overloaded(_data.system_overloaded);
    */
    


    MessageHead Head;
    CIOBuffer pBuffer;
    Head.Command = RemoNet::CC_STATE;
    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;

    _message->WritePacket(ChannelType::CHANNEL_CAR, pBuffer);
}

void CRobotSensor::CanSendProc()
{
    while (_run)
    {
        
        ////volatile int64_t tick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

        ////if (tick - _last_can_uptime < 50)
        
        {
            std::lock_guard<std::mutex> lg(_last_can_lock);
            
            for (int32_t i = 0; i < sizeof(_msg) / sizeof(TPCANMsg); i++)
            {
                if (_msg[i].ID != 0)
                {
                    _socket->Write(&_msg[i]);
                    if((_msg[i].ID == 0x182) && (_msg[i].DATA[0] != 0))
                        std::cout << "182" << _msg[i].DATA[0] << _msg[i].DATA[1] << std::endl;
                }
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        
    }
    
   
}

void CRobotSensor::Notify(TPCANMsg *buffer, int32_t size)
{   
    //cannet_frame_receive* Source_data = reinterpret_cast<cannet_frame_receive*>(buffer);
    //std::cout << std::hex << Source_data->canid << std::endl;
    if(_run)
    {
        /*
        switch (buffer->ID)
        {
        case 0x283:
            _data.Main_pump_1_pressure = make_int16(buffer->DATA[1],buffer->DATA[0]);//主泵1压力
            _data.Main_pump_2_pressure = make_int16(buffer->DATA[3],buffer->DATA[2]);//主泵2压力
            break;
        
        case 0x285:
            _data.Stick_angle = make_int16(buffer->DATA[1],buffer->DATA[0]);//斗杆角度
            _data.Boom_angle = make_int16(buffer->DATA[3],buffer->DATA[2]);//动臂角度
            break;

        case 0x286:
            _data.Hydraulic_oil_temperature = make_int16(buffer->DATA[1],buffer->DATA[0]);//液压油油温
            _data.Fuel_level = (int32_t)buffer->DATA[6];//燃油油位
            break;
        
        case 0x28A:
            _data.Travel_speed = make_int16(buffer->DATA[1],buffer->DATA[0]);//行驶速度
            _data.Engine_temperature = make_int16(buffer->DATA[3],buffer->DATA[2]);//发动机水温
            _data.Hand_gear = make_int16(buffer->DATA[5],buffer->DATA[4]);//水箱水位
            _data.Actual_gear = make_int16(buffer->DATA[7],buffer->DATA[6]);//发动机机油压力
            break;

        case 0x28B:
            _data.Engine_speed = make_int16(buffer->DATA[1],buffer->DATA[0]);//发动机转速

            if(_data.Engine_speed > 700 && _message->bStopedCar)
                _message->bStopedCar = false;
            break;

        case 0x28D:
            _data.Gripper_height = b2f(buffer->DATA[3], buffer->DATA[2], buffer->DATA[1], 
                buffer->DATA[0]);//抓具高度
            _data.amplitude = b2f(buffer->DATA[7], buffer->DATA[6], buffer->DATA[5], 
                buffer->DATA[4]);//幅度
            break;

        case 0x28F:
            _data.Idle_protection = (buffer->DATA[3] & 0x80) == 0x80 ? 1 : 0;//怠速保护

            _data.Front_toggle = (buffer->DATA[1] & 0x08) == 0x08 ? 1 : 0;//前轮对中
            _data.Back_toggle = (buffer->DATA[1] & 0x10) == 0x10 ? 1 : 0;//后轮对中
            break;

        case 0x280:
            memcpy(_data.Error_Buff,buffer->DATA,buffer->LEN);
            SendStatusToMSG();
            break;

        default:
            break;
        }
        */
    }
    

    /*
    can_array* data = reinterpret_cast<can_array*>(buffer);

    for (auto i = 0; i < data->count; i++)
    {
        // TPCANMsg msg;
        cannet_frame& frame = data->frames[i];
        if (frame.canid == 0x386)
        {
            RemoNet::State req;
            req.set_engine_speed(_data.Engine_speed);
            req.set_travel_speed(_data.Travel_speed);
            req.set_fuel_level(_data.Fuel_level);
            req.set_engine_temperature(_data.Engine_temperature);
            req.set_hydraulic_oil_temperature(_data.Hydraulic_oil_temperature);
            req.set_main_pump_1_pressure(_data.Main_pump_1_pressure);
            req.set_main_pump_2_pressure(_data.Main_pump_2_pressure);
            req.set_hand_gear(_data.Hand_gear);
            req.set_actual_gear(_data.Actual_gear);
            req.set_gripper_height(_data.Gripper_height);
            req.set_amplitude(_data.amplitude);
            req.set_boom_angle(_data.Boom_angle);
            req.set_stick_angle(_data.Stick_angle);


            req.set_can_communication_alarm(_data.Can_Communication_alarm);
            req.set_low_oil_pressure(_data.Low_oil_pressure);
            req.set_low_coolant_level(_data.Low_coolant_level);
            req.set_high_coolant_level(_data.High_coolant_level);
            req.set_hydraulic_oil_temperature_high(_data.hydraulic_oil_temperature_high);
            req.set_hydraulic_oil_temperature_low(_data.hydraulic_oil_temperature_low);
            req.set_accumulator_pressure_abnormal(_data.accumulator_pressure_abnormal);
            req.set_engine_spn_failure(_data.Engine_SPN_failure);
            req.set_engine_warming_up(_data.engine_warming_up);
            req.set_engine_warming_up_timeout(_data.engine_warming_up_timeout);
            req.set_low_battery_voltage(_data.Low_battery_voltage);
            req.set_high_battery_voltage(_data.High_battery_voltage);
            req.set_system_overloaded(_data.system_overloaded);
            req.set_idle_protection(_data.Idle_protection);


            MessageHead Head;
            CIOBuffer pBuffer;
            Head.Command = RemoNet::CC_STATE;
            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;

            _message->WritePacket(ChannelType::CHANNEL_CAR, &pBuffer);
        }
    }
    */
}

void CRobotSensor::OnMessage(cannet_frame* frames, int32_t count)
{

    //std::lock_guard<std::mutex> lg(_last_can_lock);
    //_last_can_uptime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
 
    for (int32_t i = 0; i < count; i++)
    {
        cannet_frame& frame = frames[i];
        // TPCANMsg msg;
        // msg.LEN=frame.dlc;
        // msg.MSGTYPE=PCAN_MESSAGE_STANDARD;
        int32_t canid = frame.canid;

        //std::cout << std::hex << canid << std::endl;
        switch (canid)
        {
            case 0x181:
                
                _msg[0].ID = 0x181;
                _msg[0].LEN = frame.dlc;
                _msg[0].MSGTYPE = PCAN_MESSAGE_STANDARD;
                memcpy(_msg[0].DATA, frame.data, frame.dlc);
                _socket->Write(&_msg[0]);
                //std::cout << "revice 181" << std::endl;
            break;

            case 0x182:
                _msg[1].ID = 0x182;
                _msg[1].LEN = frame.dlc;
                _msg[1].MSGTYPE = PCAN_MESSAGE_STANDARD;
                memcpy(_msg[1].DATA, frame.data, frame.dlc);
                _socket->Write(&_msg[1]);
                //if(_msg[1].DATA[0] != 0)
                //    std::cout << "182" << _msg[i].DATA[0] << _msg[i].DATA[1] << std::endl;
            break;

            case 0x183:
                _msg[2].ID = 0x183;
                _msg[2].LEN = frame.dlc;
                _msg[2].MSGTYPE = PCAN_MESSAGE_STANDARD;
                memcpy(_msg[2].DATA, frame.data, frame.dlc);
                _socket->Write(&_msg[2]);
                //std::cout << "revice 183" << std::endl;
            break;

            case 0x184:
            
                _msg[3].ID = 0x184;
                _msg[3].LEN = frame.dlc;
                _msg[3].MSGTYPE = PCAN_MESSAGE_STANDARD;

                //模式
                model = (_msg[3].DATA[3] & 0x04) == 0x04 ? 1 : 0;

                //切换前后摄像头
                _front_view = (_msg[3].DATA[4] & 0x04) == 0x04 ?  false: true;
                _message->SwitchCamera(_front_view);

                //b_ready = (_msg[3].DATA[4] & 0x10) == 0x10 ? 1 : 0;
                if((_msg[3].DATA[4] & 0x10) == 0x10 ? 1 : 0)//已就绪 抓钢机就绪状态，待无人车入场
                {
                  _message->SendZGJStatus(1);
                  std::cout << "SendZGJStatus" << std::endl;
                  //_msg[3].DATA[4] &= 0xEF;
                }

                //b_over = (_msg[3].DATA[4] & 0x20) == 0x20 ? 1 : 0;
                if((_msg[3].DATA[4] & 0x20) == 0x20 ? 1 : 0)//已完成 抓钢机抓钢结束，待无人车离场
                {
                  _message->SendZGJStatus(2);
                  //_msg[3].DATA[4] &= 0xD7;
                }

                if(!_message->btimeStopedCar)
                {
                    memcpy(_msg[3].DATA, frame.data, frame.dlc);  
                } 
                else
                {
                    long long tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                    if(tick-_message->_curStopTick > 500) 
                    {
                        Emergency();
                        _message->btimeStopedCar = false;
                    }
                }
                _socket->Write(&_msg[3]);
                //std::cout << "revice 184" << std::endl;
                //_last_can_uptime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
            break;
        }
    }
}

unsigned int CRobotSensor:: CRC16(uint8_t *buf, int len)
{
   unsigned int crc = 0xFFFF;

    for (int pos = 0; pos < len; pos++)
    {
        crc ^= (unsigned int)buf[pos]; // XOR byte into least sig. byte of crc

        for (int i = 8; i != 0; i--)
        { // Loop over each bit
            if ((crc & 0x0001) != 0)
            {              // If the LSB is set
                crc >>= 1; // Shift right and XOR 0xA001
                crc ^= 0xA001;
            }
            else           // Else LSB is not set
                crc >>= 1; // Just shift right
        }
    }
    
     
    return crc;
}

bool CRobotSensor::Emergency()
{
    if(model)
    {
        _msg[3].ID = 0x184;
        _msg[3].LEN = 8;
        _msg[3].MSGTYPE = PCAN_MESSAGE_STANDARD;

        _msg[3].DATA[3] &= 0xFD;
        _socket->Write(&_msg[3]);
    }
    return false;
}

void CRobotSensor::OnPark(bool on)
{
    if(model)
    {
        _msg[0].ID = 0x181;
        _msg[0].LEN = 8;
        _msg[0].MSGTYPE = PCAN_MESSAGE_STANDARD;

        _msg[0].DATA[4] = 0x13;
        _msg[0].DATA[5] = 0x88;
    }
}

void CRobotSensor::Run()
{

}

void CRobotSensor::SetSensorSocket(SensorPeakCan<CRobotSensor>* can)
{
  _socket=can;
}