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

#include "udp_state.h"
#include <thread>
#include "message_queue.h"

CUdpStateSensor::CUdpStateSensor(CMessageQueue* q):_message(q)
{
    _run = false;
}

void CUdpStateSensor::Notify(int8_t * buffer,int32_t size)
{
    if(_run)
    {
         if(!_message->_GSML)
        {
            for(int i = 0 ; i < size  ; i += 48)
            {   
                memcpy(&m_StateDat,buffer + i,48);
                _message->_Radardata.r0 = m_StateDat.rader[0];
                _message->_Radardata.r1 = m_StateDat.rader[1];
                _message->_Radardata.r2 = m_StateDat.rader[2];
                _message->_Radardata.r3 = m_StateDat.rader[3];
                _message->_Radardata.r4 = m_StateDat.rader[4];
                _message->_Radardata.r5 = m_StateDat.rader[5];
                _message->_Radardata.r6 = m_StateDat.rader[6];
                _message->_Radardata.r7 = m_StateDat.rader[7];
                _message->_Radardata.r8 = m_StateDat.rader[8];

                std::cout << _message->_Radardata.r0 << _message->_Radardata.r1 << _message->_Radardata.r2 << _message->_Radardata.r3 <<
                _message->_Radardata.r4 << _message->_Radardata.r5 << _message->_Radardata.r6 << _message->_Radardata.r7 << _message->_Radardata.r8 << std::endl;

                _count++;

                if (_count > 4)
                {
                    _message->WriteRadarData(_message->_Radardata);
                    _count = 0;
                }
                

                _message->_Feeddata.wire_sensor_l = m_StateDat.outrigger[0];
                _message->_Feeddata.wire_sensor_r = m_StateDat.outrigger[1];

                unsigned char Source[12];
                memset(Source,0,12);
                if(m_StateDat.easypro[0] == '+')
                {
                    sprintf((char *)Source,"0.%d",m_StateDat.easypro[1]);
                    _message->_Feeddata.steer_angle = strtof((const char *)Source,NULL);
                }
                else if(m_StateDat.easypro[0] == '-')
                {
                    sprintf((char *)Source,"1.%d",m_StateDat.easypro[1]);
                    _message->_Feeddata.steer_angle = strtof((const char *)Source,NULL);
                }

                _message->_Feeddata.speed = m_StateDat.speed;

                std::cout << _message->_Feeddata.wire_sensor_l << _message->_Feeddata.wire_sensor_r << _message->_Feeddata.steer_angle << _message->_Feeddata.steer_angle <<
                _message->_Feeddata.speed << std::endl;
                //SendStatusToMSG();

            }  
        }
        else
        {
            uint8_t temp = ':',temp1 = ',',*p = NULL,*p1 = NULL;

            p = (uint8_t *)strchr((const char *)buffer,temp);
            unsigned long m_time = strtol((const char *)p + 1,NULL,10);

            p = (uint8_t *)strchr((const char *)buffer,temp1);
            unsigned long m_startrevord = strtol((const char *)p + 1,NULL,10);

            p1 = (uint8_t *)strchr((const char *)p + 1,temp1);
            unsigned long m_deviceid = strtol((const char *)p1 + 1,NULL,10);
            
            _message->SetGsmlInfo(m_time,m_startrevord,m_deviceid);
        }
    } 
}

void CUdpStateSensor::Start()
{
    if(!_run)
    {
        //_udp_thread = std::thread(&CUdpStateSensor::Run, this);
        _run = true;
    } 
}

void CUdpStateSensor::Run()
{
    CIOBuffer buffer;

    while (_run)
    {

    }
}

void CUdpStateSensor::SetSensorSocket(SensorSocket<CUdpStateSensor>* udpstate)
{
    _udpcan = udpstate;
}

void CUdpStateSensor::Stop()
{
    if (_run)
    {
        _run = false;
        //_udp_thread.join();
    }
}

void CUdpStateSensor::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);
    */

    req.set_work_pressure(_message->_Feeddata.work_pressure);
    req.set_brake_pressure(_message->_Feeddata.brake_pressure);
    req.set_gearbox_oil_temp(_message->_Feeddata.gearbox_oil_temp);
    req.set_gearbox_oil_pressure(_message->_Feeddata.gearbox_oil_pressure);
    req.set_engine_rpm(_message->_Feeddata.engine_rpm);//发动机转速                    
	req.set_speed(_message->_Feeddata.speed);//不知道？---可能是后加的传感器---驱动轮增量编码器
	req.set_gear(_message->_Feeddata.gear);
	req.set_engine_pressure(_message->_Feeddata.engine_pressure);//发动机压力               
	req.set_cold_water(_message->_Feeddata.cold_water);//冷却液温度-发动机水温          
	req.set_steer_angle(_message->_Feeddata.steer_angle);//转向角度（原来金川是轮子的现在大冶用铰链位置）          铰接转向姿态编码器
	req.set_left_lock(_message->_Feeddata.left_lock);//遥操模式
	req.set_right_lock(_message->_Feeddata.right_lock);//人工模式
    req.set_engine_time(_message->_Feeddata.engine_time);//发动机运行小时
	req.set_wire_sensor_l(_message->_Feeddata.wire_sensor_l);//拉线传感器左侧
	req.set_wire_sensor_r(_message->_Feeddata.wire_sensor_r);//拉线传感器右侧
	req.set_fuel_level(_message->_Feeddata.fuel_level);//燃油油位
	req.set_lock_switch_l(_message->_Feeddata.lock_switch_l);//锁钩到位开关左
	req.set_lock_switch_r(_message->_Feeddata.lock_switch_r); //锁钩到位开关右
	req.set_tip_limit(_message->_Feeddata.tip_limit);//倾翻限位
	req.set_turn_left(_message->_Feeddata.turn_left);//左转
	req.set_turn_right(_message->_Feeddata.turn_right);//右转
	req.set_hight_beam(_message->_Feeddata.hight_beam);//远光灯
	req.set_low_beam(_message->_Feeddata.low_beam);//近光灯 
	req.set_parking(_message->_Feeddata.parking);//驻车
	req.set_brake(_message->_Feeddata.brake);//刹车
	req.set_back_car(_message->_Feeddata.back_car);//倒车
	req.set_front_work_lamp(_message->_Feeddata.front_work_lamp);//前工作灯
	req.set_rear_work_lamp(_message->_Feeddata.rear_work_lamp);//后工作灯
	req.set_cargo_weight(_message->_Feeddata.cargo_weight);//渣包重量
	req.set_system_vol(_message->_Feeddata.system_vol);//系统电压
	req.set_error_buff((char *)_message->_Feeddata.Error_Buff,8);//报警信息
	req.set_f_gear(_message->_Feeddata.f_gear);//1F，2R，3N
	req.set_gear_1(_message->_Feeddata.gear_1);//1，2，3，4

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