Vehicle-cpp/ZJ_PRO/EgoSystem/can_bus.cpp

#include "can_bus.h"
#include <thread>
#include "message_queue.h"
#include <math.h>


#define	GET_BIT(x, bit)	((x & (1 << bit)) >> bit)

float Canb2f(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;
}

int hex_to_decimal(const char* hex_str) {
    int decimal = 0;
    int len = strlen(hex_str);

    for (int i = 0; i < len; i++) {
        char c = toupper(hex_str[i]);
        if (c >= '0' && c <= '9') {
            decimal = decimal * 16 + (c - '0');
        } else if (c >= 'A' && c <= 'F') {
            decimal = decimal * 16 + (c - 'A' + 10);
        } else {
            return -1; // 输入的字符串含有非十六进制字符
        }
    }

    return decimal;
}

int16_t make_int16(unsigned char h,unsigned char l)
{
  int16_t hi=(int16_t)(h&0x00FF);
  int16_t low= (int16_t)(l&0x00FF);

   return (hi<<8)|low;
}

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

    for (int32_t i = 0; i < sizeof(Sendframe)/sizeof(can_frame); i++)
        memset(&Sendframe[i], 0, sizeof(can_frame));
}

void CCanBusSensor::SetCanBusSensor(SensorCanBus<CCanBusSensor>* can)
{
    _canbus = can;
}

void CCanBusSensor::CanSendProc()
{
    while (_run)
    {
       for(int32_t i=0;i<sizeof(Sendframe)/sizeof(can_frame);i++)
        {
            if(Sendframe[i].can_id != 0)
            {
                // //接收到的frame
                // for (int i = 0; i < Sendframe[i]->can_dlc; i++) {
                //     printf("%02x ", Sendframe[i]->data[i]);
                // }
                // printf("\n");
                if(Sendframe[i].can_id == 0x184)
                {
                    _message->_dst  = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                    //printf("data delay:%lld\r\n",_message->_dst - _message->_source);
                }
                _canbus->Write(&Sendframe[i]);
                Sendframe[i].can_id  = 0;
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(5)); 

    } 
}

void CCanBusSensor::Start()
{
    _run = true;

    _can_thread = std::thread(&CCanBusSensor::CanSendProc, this);

    Direction = 0;//方向
    Hand_Throttle = 0;//手油门
    Foot_Throttle = 0;//脚油门
    Brake = 0;//刹车
    _message->DataMqtt_SendDate = false;
    _message->DataMqtt_curTick = 0;
}


void CCanBusSensor::Run()
{
    
}
  
void CCanBusSensor::Stop()
{
    if (_run)
    {
        _run = false;
        _can_thread.join();
    }

    for (int32_t i = 0; i < sizeof(Sendframe)/sizeof(can_frame); i++)
        memset(&Sendframe[i], 0, sizeof(can_frame));

    Direction = 0;//方向
    Hand_Throttle = 0;//手油门
    Foot_Throttle = 0;//脚油门
    Brake = 0;//刹车

    _message->DataMqtt_SendDate = false;
    _message->DataMqtt_curTick = 0;
}

//_data 
void CCanBusSensor::Notify(struct can_frame *date)
{
    //std::cout << std::hex << date->can_id << std::endl;
    if(_run)
    {
        
        switch (date->can_id)
        {
        case 0x283:
            _data.Main_pump_1_pressure = make_int16(date->data[1],date->data[0]);//主泵1压力
            _data.Main_pump_2_pressure = make_int16(date->data[3],date->data[2]);//主泵2压力
            break;
        
        case 0x285:
            _data.Stick_angle = make_int16(date->data[1],date->data[0]);//斗杆角度
            _data.Boom_angle = make_int16(date->data[3],date->data[2]);//动臂角度
            break;

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

        case 0x28B:
            _data.Engine_speed = make_int16(date->data[1],date->data[0]);//发动机转速
            {
                long long tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

                if(_data.Engine_speed > 700 && _message->bStopedCar && tick-_message->_curStopTick > 500)
                    _message->bStopedCar = false;
            }

            _data.engine_fmi = (static_cast<uint16_t>(date->data[3]) << 8 | static_cast<uint16_t>(date->data[2]));
            _data.engine_spn = (static_cast<uint16_t>(date->data[5]) << 8 | static_cast<uint16_t>(date->data[4]));
            _data.engine_num = (static_cast<uint16_t>(date->data[7]) << 8 | static_cast<uint16_t>(date->data[6]));
            
            break;

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

        case 0x28F:
            _data.Idle_protection = (date->data[3] & 0x80) == 0x80 ? 1 : 0;//怠速保护
            _data.Front_toggle = (date->data[1] & 0x08) == 0x08 ? 1 : 0;//前轮对中
            _data.Back_toggle = (date->data[1] & 0x10) == 0x10 ? 1 : 0;//后轮对中
            _data.interlock = (date->data[5] & 0x08) == 0x08 ? 1 : 0;//启动联锁

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

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

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

            //增加
            memcpy(_data.all_Buff,date->data,date->can_dlc);

            break;

        case 0x280:
            memcpy(_data.Error_Buff,date->data,date->can_dlc);
            SendStatusToMSG();;
            break;
        
        case 0x384:
            // _data.function_code= make_int16(date->data[1],date->data[0])  ;
            // _data.main_add = make_int16(date->data[3],date->data[2]);
            // _data.sub_add = make_int16(date->data[5],date->data[4]);
            // _data.paramter = make_int16(date->data[7],date->data[6]);

            _data.function_code = (static_cast<uint16_t>(date->data[0]) << 8 | static_cast<uint16_t>(date->data[1]));
            _data.main_add = (static_cast<uint16_t>(date->data[2]) << 8 | static_cast<uint16_t>(date->data[3]));
            _data.sub_add = (static_cast<uint16_t>(date->data[4]) << 8 | static_cast<uint16_t>(date->data[5]));
            _data.paramter = (static_cast<uint16_t>(date->data[6]) << 8 | static_cast<uint16_t>(date->data[7]));
            break;

        default:
            break;
        }
    }
}



//sendframe 
void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count)
{
    // vehicle_status_t vehicle_status;
    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:
                Sendframe[0].can_id = 0x181;
                Sendframe[0].can_dlc = frame.dlc;

                Hand_Throttle = make_int16(frame.data[1],frame.data[0]);
                Foot_Throttle = make_int16(frame.data[3],frame.data[2]);
                can_status.Brake = make_int16(frame.data[5],frame.data[4]);   //刹车
                can_status.Outrigger_status = make_int16(frame.data[7],frame.data[6]);  //支腿
                // Turn_angle = 


                memcpy(Sendframe[0].data, frame.data, frame.dlc);
                //std::cout << "revice 181" << std::endl;
            break;

            case 0x182:
                Sendframe[1].can_id = 0x182;
                Sendframe[1].can_dlc = frame.dlc;

                memcpy(Sendframe[1].data, frame.data, frame.dlc);
                //std::cout << "182" << std::endl;
            break;

            case 0x183:
                Sendframe[2].can_id = 0x183;
                Sendframe[2].can_dlc = frame.dlc;

                // Direction = make_int16(frame.data[3],frame.data[2]);
                can_status.Turn_angle = make_int16(frame.data[3], frame.data[2]);  //转弯角度


                // Turn_angle =                         //转弯角度

                 
                // _message->SendVehicleStatus(Direction,Hand_Throttle,Foot_Throttle,Brake);

                memcpy(Sendframe[2].data, frame.data, frame.dlc);
                //std::cout << "revice 183" << std::endl;
            break;

            case 0x184:
            
                Sendframe[3].can_id = 0x184;
                Sendframe[3].can_dlc = frame.dlc;

                can_status.fire_status = frame.data[2] & 0x80 ? 1 : 0;     //启动/熄火？   
                can_status.remote_forward =  frame.data[0] & 0x10 ? 1 : 0;  //前进      0 - 5  
                can_status.remote_backward =  frame.data[0] & 0x20 ? 1 : 0;   //后退      0 - 6  
                can_status.Park = frame.data[1] & 0x01 ? 1 : 0;    //驻车    1-0
                can_status.Left_turn = frame.data[7] & 0x40 ? 1 : 0; //左旋    右手柄  左  7-7
                can_status.Right_turn = frame.data[7] & 0x80 ? 1 : 0 ;   //右旋    右手柄   右  7-8
                can_status.Raised = frame.data[7] & 0x02 ? 1 : 0;              //小臂抬起   左手柄  向后        7-2
                can_status.Decrease = frame.data[7] & 0x01 ? 1 : 0;            //小臂下降   左手柄  向前   7-1
                can_status.Stretch = frame.data[7] &  0x20 ? 1 : 0;             //大臂伸起   右手柄  向后   7-6
                can_status.Bulls_decline = frame.data[7] &  0x10  ? 1 : 0;      //大臂下降    右手柄  向前  7-5 
                can_status.Material  = frame.data[4]  & 0x01  ? 1 : 0;          //吸盘吸   4-1
                can_status.Discharge  = frame.data[4]  & 0x02  ? 1 : 0;         //吸盘放  4-2
                can_status.zhua = frame.data[7] & 0x40 ? 1 : 0;                      //抓斗抓料   右手柄 左边  7-7
                can_status.fang = frame.data[7] & 0x80 ? 1 : 0 ;                      //抓斗放料   右手柄 右边  7-8
                can_status.Shell_rotation_left = frame.data[6] &0x40 ? 1 : 0  ;              //贝壳斗旋转  shun   6-7
                can_status.Shell_rotation_right = frame.data[7] & 0x80? 1 : 0 ;              //贝壳斗旋转   ni   6-8
                can_status.Cabin = frame.data[1] & 0x40 ? 1 : 0;                //驾驶室升  1-7
                can_status.Cab = frame.data[1] & 0x80? 1 : 0;                //驾驶室降   1-8
                can_status.Working_signal = frame.data[2] & 0x20? 1 : 0 ;         //作业灯   2-6
                can_status.Turn_left_signal = frame.data[0] &  0x08? 1 : 0 ;           //左转向      0-4
                can_status.Turn_right_signal = frame.data[0] &  0x10? 1 : 0;            //右转向   0-5
                can_status.Trumpet = frame.data[1] & 0x02 ? 1 : 0;        //喇叭   1-2
                can_status.Urget_stop = frame.data[3] & 0x02 ? 1 : 0;       //急停  3-2
                can_status.Vehicle_id = 535;      //车辆id

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

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

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

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

                if(!_message->btimeStopedCar)
                {
                    memcpy(Sendframe[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;
                    }
                    */
                    if(tick-_message->_curStopTick < 500) 
                        Emergency();
                    else
                        _message->btimeStopedCar = false;
                }
                //_message->_dst  = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                //    printf("data delay:%ld\r\n",_message->_dst - _message->_source);
                //std::cout << "revice 184" << std::endl;

                // _message -> SendVehicleStatus( vehicle_status);
            break;


            case 0x185:
                Sendframe[4].can_id = 0x185;
                Sendframe[4].can_dlc = frame.dlc;

                memcpy(Sendframe[4].data, frame.data, frame.dlc);
                _message -> SendVehicleStatus( can_status);
            break;
        }
    } 
}

void CCanBusSensor::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_function_code(_data.function_code);
    req.set_main_add(_data.main_add);
    req.set_sub_add(_data.sub_add);
    req.set_paramter(_data.paramter);


    //增加0x28b  
	req.set_engine_spn(_data.engine_spn);        //发动机实时故障SPN
	req.set_engine_fmi(_data.engine_fmi);       //发动机实时故障FMI
	req.set_engine_num(_data.engine_num);        //发动机故障次数

    //增加0X28F
    req.set_all_buff((char *)_data.all_Buff,8);

	// //增加0x28f
	// req.set_p_flameout(_data.p_flameout);        //P档熄火；
	// req.set_engine_start(_data.engine_start);      //发动机启动
	// req.set_emergency_signal(_data.emergency_signal);  //紧停信号
	// req.set_preheating_monitoring(_data.preheating_monitoring);  //预热监控
	// req.set_park_switch(_data.park_switch);   //驻车开关
	// req.set_safety_bar_lock (_data.safety_bar_lock); //安全杆锁定
	// req.set_forward_gear (_data.forward_gear);     //前进档
	// req.set_backward_gear (_data.backward_gear );//后退档
	
	// req.set_first_gear(_data.first_gear);   //行驶一档
	// req.set_sencond_gear(_data.sencond_gear);  //行驶2档
	// req.set_n_gear(_data.n_gear);       //实际N 档
	// req.set_cabin_end(_data.cabin_end);     //驾驶室上升到位 
	// req.set_cab_end(_data.cab_end);       //驾驶室下降到位
	// req.set_reserve(_data.reserve);       //保留

	// req.set_for_left_outrigger(_data.for_left_outrigger);   //前左支腿选择
	// req.set_for_right_outrigger(_data.for_right_outrigger);  //前右支腿选择
	// req.set_back_left_outrigger(_data.back_left_outrigger);  //后左支腿选择
	// req.set_back_right_outrigger(_data.back_right_outrigger);  //后右支腿选择
	// req.set_cab_start(_data.cab_start); //驾驶室下降
	// req.set_canbin_start(_data.canbin_start);//驾驶室上升
	// req.set_swinging_bridge_lock(_data.swinging_bridge_lock); //摆动桥锁定
	// req.set_cab_emery(_data.cab_emery);       //驾驶室应急下降

	// req.set_safety_bypass (_data.safety_bypass);   //安全旁通
	// req.set_clear_alarm(_data.clear_alarm);    //清除蜂鸣器报警
	// req.set_front_wheel_steering(_data.front_wheel_steering);  //前轮转向
	// req.set_back_wheel_steering(_data.back_wheel_steering);  //后轮转向
	// req.set_same_steering(_data.same_steering);  //前后轮同向转向
	// req.set_reverse_steering(_data.reverse_steering);  //前后轮反向转向
	// req.set_sound_signal_alarm(_data.sound_signal_alarm);  //故障声光报警


	// req.set_rod_intake_limit_valve(_data.rod_intake_limit_valve);   //斗杆内收限制阀
	// req.set_rod_outtake_limit_valve(_data.rod_outtake_limit_valve);  //斗杆外摆限制阀
	// req.set_boom_lift_limit_valve(_data.boom_lift_limit_valve);  //动臂提升限制阀
	// req.set_boom_out_limit_valve(_data.boom_out_limit_valve);  //动臂下降限制阀
	// req.set_cab_emery_value(_data.cab_emery_value);  //驾驶室应急下降阀
	// req.set_charging_valve(_data.charging_valve);  //充液阀
	// req.set_parking_valve(_data.parking_valve);   //驻车阀

	// req.set_parking_back_signal(_data.parking_back_signal);    //驻车反馈灯
	// req.set_brake_signal(_data.brake_signal);   //刹车灯
	// req.set_power_delay(_data.power_delay);    //断电延时
	// req.set_flash_signal(_data.flash_signal);    //闪光信号
	// req.set_left_for_outrigger_value (_data.left_for_outrigger_value);  //左前支腿伸缩电磁阀
	// req.set_right_for_outrigger_value (_data.right_for_outrigger_value);   //右前支腿伸缩电磁阀
	// req.set_left_back_outrigger_value(_data.left_back_outrigger_value);  //左后支腿伸缩电磁阀

	// req.set_right_back_outrigger_value(_data.right_back_outrigger_value);    //右后支腿伸缩电磁阀
	// req.set_for_steering_value(_data.for_steering_value);   //前轮转向电磁阀
	// req.set_same_steering_value (_data.same_steering_value);   //四轮同向转向电磁阀
	// req.set_rever_steering_value (_data.rever_steering_value);   //四轮反向转向电磁阀
	// req.set_first_gear_solenoid_valve(_data.first_gear_solenoid_valve);   //一档电磁阀
	// req.set_sen_gear_solenoid_valve(_data.sen_gear_solenoid_valve);   //二档电磁阀
	// req.set_grab_rotary_switch_valve(_data.grab_rotary_switch_valve);    //抓具旋转切换电磁阀
   
    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 CCanBusSensor::Emergency()
{
    if(model)
    {
        Sendframe[3].can_id = 0x184;
        Sendframe[3].can_dlc = 8;

        Sendframe[3].data[3] &= 0xFD;
        //_canbus->Write(&Sendframe[3]);
    }
}