Vehicle-cpp/ZJ_PRO/EgoSystem/pcan_sensor.cpp

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


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

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

void CPcanSensor::Start()
{
    _message->btimeStopedCar = false;
    _run = true;
}

void CPcanSensor::Stop()
{
    if (_run)
    {
        _run = false;
    }

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

void CPcanSensor::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);


    req.set_error_buff((char *)_data.Error_Buff,8);
   
    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 CPcanSensor::Notify(TPCANMsg *buffer, int32_t size)
{
     //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;//后轮对中

            _data.interlock = (buffer->DATA[5] & 0x08) == 0x08 ? 1 : 0;//启动联锁

            _data.safety_switch = (buffer->DATA[6] & 0x80) == 0x80 ? 1 : 0;//安全开关阀异常

            _data.arm_lift_pilot = (buffer->DATA[7] & 0x01) == 0x01 ? 1 : 0;//大臂提升先导比例阀异常
            _data.arm_lowering_pilot = (buffer->DATA[7] & 0x02) == 0x02 ? 1 : 0;//大臂下降先导比例阀异常
            _data.lever_lifting_pilot = (buffer->DATA[7] & 0x04) == 0x04 ? 1 : 0;//斗杆提升先导比例阀异常
            _data.rod_lowering_pilot = (buffer->DATA[7] & 0x08) == 0x08 ? 1 : 0;//斗杆下降先导比例阀异常
            _data.left_rotary_pilot = (buffer->DATA[7] & 0x10) == 0x10 ? 1 : 0;//左回转先导比例阀异常
            _data.right_rotary_pilot = (buffer->DATA[7] & 0x20) == 0x20 ? 1 : 0;//右回转先导比例阀异常
            _data.grab_open_pilot = (buffer->DATA[7] & 0x40) == 0x40 ? 1 : 0;//抓斗开启先导比例阀异常
            _data.grab_close_pilot = (buffer->DATA[7] & 0x80) == 0x80 ? 1 : 0;//抓斗闭合先导比例阀异常

            _data.safety_valves = (buffer->DATA[4] & 0x10) == 0x10 ? 1 : 0;//安全开关阀
            break;

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

        default:
            break;
        }
        
    }
}

void CPcanSensor::OnMessage(cannet_frame* frames, int32_t count)
{
    for (int32_t i = 0; i < count; i++)
    {
        cannet_frame& frame = frames[i];
        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;

                Hand_Throttle = make_int16(frame.data[1],frame.data[0]);
                Foot_Throttle = make_int16(frame.data[3],frame.data[2]);
                Brake = make_int16(frame.data[5],frame.data[4]);

                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;

                Direction = make_int16(frame.data[3],frame.data[2]);
//                _message->SendVehicleStatus(Direction,Hand_Throttle,Foot_Throttle,Brake);
                
                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 = (frame.data[3] & 0x04) == 0x04 ? 1 : 0;

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

                if((frame.data[4] & 0x10) == 0x10 ? 1 : 0)//已就绪 抓钢机就绪状态，待无人车入场
                {
                  _message->SendZGJStatus(1);
                }

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

                if(!_message->btimeStopedCar)
                {
                    _socket->Write(&_msg[3]);
                } 
                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;
                    }
                    */
                    if(tick-_message->_curStopTick < 500) 
                        Emergency();
                    else
                        _message->btimeStopedCar = false;
                }
                
                //std::cout << "revice 184" << std::endl;
            break;
        }
    }
}

bool CPcanSensor::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 CPcanSensor::Run()
{

}

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