LidarMeasure/plc/plc_communicator.cpp

#include "plc_communicator.h"
#include <glog/logging.h>

// ××××××××××× 构造与析构 ×××××××××××
Plc_Communicator::Plc_Communicator(plc_module::plc_connection_params connection_params) : mb_plc_initialized(false),
                                                                             mb_plc_is_connected(false),
                                                                             mb_plc_is_updating(false),
                                                                             mp_plc_owner(0),
                                                                             m_plc_thread(0),
                                                                             m_plc_message_thread(0)
{
    m_plc_ip_str = connection_params.ip();
    m_plc_port = connection_params.port();
    m_plc_slave_id = connection_params.slave_id();
    m_plc_current_error = Error_manager(Error_code::SUCCESS, Error_level::NORMAL, "初始状态正常");
    m_plc_status_update_timeout = 10000;
    for (size_t i = 0; i < PLC_REGION_NUM; i++)
    {
        m_plc_region_status[i].last_time_point = std::chrono::steady_clock::now();
        m_plc_region_status[i].current_status = 255;
        m_plc_region_status[i].cmd = 0;
    }

    m_plc_data.resize(PLC_REGION_NUM * PLC_SIGNAL_NUM_PER_REGION);
    m_plc_current_error = connect();
    m_plc_cond_exit.Notify(false);
    m_plc_thread = new std::thread(plc_update_thread, this);
    m_plc_message_thread=new std::thread(plc_publish_message, this);
    mb_plc_initialized = true;
}

Plc_Communicator::~Plc_Communicator()
{
    m_plc_cond_exit.Notify(true);
    if (m_plc_thread)
    {
        if (m_plc_thread->joinable())
            m_plc_thread->join();
        delete m_plc_thread;
        m_plc_thread = 0;
    }

    if (m_plc_message_thread)
    {
        if (m_plc_message_thread->joinable())
            m_plc_message_thread->join();
        delete m_plc_message_thread;
        m_plc_message_thread = 0;
    }
    disconnect();
}

// ××××××××××× getters setters ×××××××××××
bool Plc_Communicator::get_connection()
{
    return mb_plc_is_connected;
}

Error_manager Plc_Communicator::get_error(){
    if(mb_plc_is_connected){
        return Error_manager(Error_code::SUCCESS, Error_level::NORMAL, "连接正常");
    }else{
        return Error_manager(Error_code::PLC_CONNECTION_FAILED, Error_level::NEGLIGIBLE_ERROR, "连接失败");
    }
}

bool Plc_Communicator::get_initialize_status()
{
    return mb_plc_initialized;
}

Error_manager Plc_Communicator::set_plc_callback(Command_Callback callback, void *p_owner)
{
    if (callback == 0 || p_owner == 0)
        return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR, "回调设置参数错误");
    m_plc_callback = callback;
    mp_plc_owner = p_owner;
    return Error_manager(Error_code::SUCCESS, Error_level::NORMAL, "回调设置参数正确");
}

Error_manager Plc_Communicator::set_status_update_timeout(int millisecond)
{
    m_plc_status_update_timeout = millisecond;
    return Error_manager(Error_code::SUCCESS, Error_level::NORMAL, "设置状态更新超时时间成功");
}

// ××××××××××× 内部调用连接与重连 ×××××××××××
Error_manager Plc_Communicator::connect()
{
    m_plc_mutex.lock();
    int rc = m_plc_wrapper.initialize(m_plc_ip_str.c_str(), m_plc_port, m_plc_slave_id);
    m_plc_mutex.unlock();
    switch (rc)
    {
    case 0:
        mb_plc_is_connected = true;
        return Error_manager(Error_code::SUCCESS, Error_level::NORMAL, "plc连接成功");
    case -1:
        mb_plc_is_connected = false;
        return Error_manager(Error_code::PLC_CONNECTION_FAILED, Error_level::MINOR_ERROR, "plc掉线");
    case -2:
        mb_plc_is_connected = false;
        return Error_manager(Error_code::PLC_SLAVE_ID_ERROR, Error_level::MINOR_ERROR, "plc的从站id错误");
    case -3:
        mb_plc_is_connected = false;
        return Error_manager(Error_code::PLC_IP_PORT_ERROR, Error_level::MINOR_ERROR, "plc的ip或端口设置错误");
    default:
        mb_plc_is_connected = false;
        return Error_manager(Error_code::PLC_UNKNOWN_ERROR, Error_level::MINOR_ERROR, "plc连接时出现未知返回值");
    }
}

Error_manager Plc_Communicator::disconnect()
{
    m_plc_mutex.lock();
    m_plc_wrapper.deinitialize();
    mb_plc_is_connected = false;
    m_plc_mutex.unlock();
    return Error_manager(Error_code::SUCCESS);
}

// ××××××××××× 外部调用执行任务单 ×××××××××××
Error_manager Plc_Communicator::execute_task(Task_Base *task)
{
    if (task == 0)
        return Error_manager(Error_code::PARAMETER_ERROR, Error_level::NEGLIGIBLE_ERROR, "传入空任务");
    if (task->get_task_type() != Task_type::PLC_TASK)
    {
        return Error_manager(Error_code::PARAMETER_ERROR, Error_level::NEGLIGIBLE_ERROR, "传入非plc任务");
    }
    Plc_Task *plc_task_temp = (Plc_Task *)task;
    plc_task_temp->update_statu(Task_statu::TASK_SIGNED, "received by plc_communicator.");

    struct measure_result measure_result_temp;
    Error_manager err = plc_task_temp->get_result(measure_result_temp);
    if (err.is_equal_error_manager(Error_manager(Error_code::SUCCESS)))
    {
        Error_manager write_err = write_result_to_plc(measure_result_temp);
//        // added by yct, 检查为何2号位测量88度，发送86.9到plc
//        char description[480] = {0};
//        sprintf(description, "to plc, id %d center(%.1f, %.1f) size:(%.1f, %.1f)  angle:%.2f",
//                measure_result_temp.terminal_id, measure_result_temp.x, measure_result_temp.y, measure_result_temp.wheel_base, measure_result_temp.width, measure_result_temp.angle);
//        LOG(INFO) << description;
        plc_task_temp->update_statu(Task_statu::TASK_OVER, "executed by plc_communicator.");
        return write_err;
    }
    else
    {
        return err;
    }
}

// ××××××××××× 外部调用获取数据, 终端id[1-6], -1则获取所有数据 ×××××××××××
Error_manager Plc_Communicator::get_plc_data(std::vector<uint16_t> &plc_data, int terminal_id)
{
    std::lock_guard<std::mutex> lck(m_plc_mutex);
    plc_data.clear();
    if (terminal_id == -1)
    {
        plc_data.operator=(m_plc_data);
    }
    else if (terminal_id <= PLC_REGION_NUM && terminal_id * PLC_SIGNAL_NUM_PER_REGION <= m_plc_data.size())
    {
        for (size_t i = (terminal_id - 1) * PLC_SIGNAL_NUM_PER_REGION; i < terminal_id * PLC_SIGNAL_NUM_PER_REGION; i++)
        {
            plc_data.push_back(m_plc_data[i]);
        }
        return Error_manager(Error_code::SUCCESS);
    }
    else
        return Error_manager(Error_code::PLC_NOT_ENOUGH_DATA_ERROR);
}

Error_manager Plc_Communicator::write_result_to_plc(struct measure_result result){
    // std::cout<<" write result 000 "<<result.terminal_id<<std::endl;
    if(result.terminal_id<=0 || result.terminal_id>PLC_REGION_NUM){
        Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR, "写plc传入参数错误");
    }
    // std::cout<<" write result 111 "<<std::endl;
    int offset = PLC_SIGNAL_BEGIN_OFFSET + PLC_LASER_STATUS_ADDR + (result.terminal_id-1) * PLC_SIGNAL_NUM_PER_REGION;
    int result_length = PLC_LASER_WHEELBASE_ADDR - PLC_LASER_STATUS_ADDR + 1;
    uint16_t result_info_temp[result_length];
    memset(result_info_temp, 0, result_length * sizeof(uint16_t));

    // 之后设置正确位,先将数据写入,状态写2
    if(result.correctness){
        m_plc_region_status[result.terminal_id - 1].current_status = 3;
        result_info_temp[PLC_LASER_STATUS_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(2);
        result_info_temp[PLC_LASER_X_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(result.x + 0.5f);
        result_info_temp[PLC_LASER_Y_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(result.y + 0.5f);
        result_info_temp[PLC_LASER_ANGLE_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(result.angle * 100);
        result_info_temp[PLC_LASER_LENGTH_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(result.length + 0.5f);
        result_info_temp[PLC_LASER_WIDTH_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(result.width + 0.5f);
        result_info_temp[PLC_LASER_HEIGHT_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(result.height + 0.5f);
        result_info_temp[PLC_LASER_CORRECTNESS_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(0);
        result_info_temp[PLC_LASER_WHEELBASE_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(result.wheel_base + 0.5f);
    }else{
        m_plc_region_status[result.terminal_id-1].current_status = 4;
        result_info_temp[PLC_LASER_STATUS_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(2);
        result_info_temp[PLC_LASER_CORRECTNESS_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(0);
    }
    if(!m_plc_wrapper.is_connected()){
        return Error_manager(Error_code::PLC_CONNECTION_FAILED);
    }
    else{
        // std::cout<<" write result start "<<std::endl;
        int retry_times = 3;
        int plc_write_return_code = -1;
        while(retry_times-->0 && plc_write_return_code<0) {
		// 更新时间
		m_plc_region_status[result.terminal_id - 1].last_time_point = std::chrono::steady_clock::now();
            // 写入数据
            m_plc_mutex.lock();
            plc_write_return_code = m_plc_wrapper.write_registers(offset, result_length, result_info_temp);
            m_plc_mutex.unlock();
            if(plc_write_return_code<0)
                continue;
            usleep(1000* 100);
            // 写入状态
            if(result.correctness)
            {
                result_info_temp[PLC_LASER_STATUS_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(3);
                result_info_temp[PLC_LASER_CORRECTNESS_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(1);
            }else
            {
                result_info_temp[PLC_LASER_STATUS_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(4);
                result_info_temp[PLC_LASER_CORRECTNESS_ADDR - PLC_LASER_STATUS_ADDR] = uint16_t(0);
            }
            m_plc_mutex.lock();
            plc_write_return_code = m_plc_wrapper.write_registers(offset, result_length, result_info_temp);
            m_plc_mutex.unlock();
            usleep(1000* 100);
            // std::cout<<" write result end "<<rc<<std::endl;
        }
        if(retry_times <=0 || plc_write_return_code != 0)
            return Error_manager(Error_code::PLC_WRITE_FAILED);
        else
            return Error_manager(Error_code::SUCCESS);
    }
}
/*
 *
 */

void Plc_Communicator::plc_publish_message(Plc_Communicator* plc)
{
    if(plc==0)
    {
        LOG(ERROR)<<"";
    }
    while(plc->m_plc_cond_exit.WaitFor(100)==false) {
        plc_message::plcMsg msg;
        for(int i=0;i<PLC_SIGNAL_BEGIN_OFFSET;++i)
        {
            msg.add_plc_values(0);
        }
        for (int i = 0; i < plc->m_plc_data.size(); ++i) {
            msg.add_plc_values(plc->m_plc_data[i]);
        }
        plc_message::plcStatus status;
        if (plc->mb_plc_is_connected)
            status = plc_message::ePLCConnected;
        else
            status = plc_message::ePLCDisconnected;
        msg.set_plc_status(status);
        MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());
    }

}

// ××××××××××× 更新线程静态函数 ×××××××××××
void Plc_Communicator::plc_update_thread(Plc_Communicator *plc_communicator)
{
    if (plc_communicator == 0)
        return;
    while (!plc_communicator->m_plc_cond_exit.WaitFor(100))
    {
        // std::cout<<" thread 000 "<<std::endl;

        // 断线重连
        if (!plc_communicator->mb_plc_is_connected)
        {
            Error_manager code=plc_communicator->connect();
            if(code!=SUCCESS)
            {
                LOG(ERROR)<<code.to_string();
            }
            usleep(1000 * 200);
        }
        else
        {
            // std::cout<<" thread 111 "<<std::endl;
            // 读取所有数据，更新本地并发布消息到UI
            int plc_length_temp = PLC_REGION_NUM * PLC_SIGNAL_NUM_PER_REGION;
            uint16_t plc_data_temp[plc_length_temp];
            int rc = plc_communicator->m_plc_wrapper.read_registers(PLC_SIGNAL_BEGIN_OFFSET, plc_length_temp, plc_data_temp);
            if(rc <0)
            {
                std::cout<<"find plc disconnected while read. try to reconnect."<<std::endl;
                plc_communicator->mb_plc_is_connected = false;
                continue;
            }
            else if (rc == 0)
            {
                plc_communicator->m_plc_mutex.lock();
                int terminal_id_temp = 0;
                for (size_t i = 0; i < plc_length_temp; i++)
                {
                    terminal_id_temp = i / PLC_SIGNAL_NUM_PER_REGION;
                    plc_communicator->m_plc_data[i] = plc_data_temp[i];
                    // 读取后检查是否存在差异, 存在则赋值。调用回调函数启动外部扫描
                    if (i % PLC_SIGNAL_NUM_PER_REGION == PLC_LASER_START_ADDR && plc_data_temp[i] != plc_communicator->m_plc_region_status[terminal_id_temp].cmd)
                    {
                        plc_communicator->m_plc_region_status[terminal_id_temp].cmd = plc_data_temp[i];
                        bool modified = false;
                        // 判断指令存在
                        // std::cout<<" thread 222 "<<std::endl;
                        if (plc_data_temp[i] == 1)
                        {
                            // 判空
                            if (plc_communicator->m_plc_callback != 0 && plc_communicator->mp_plc_owner != 0)
                            {
                                Error_manager ec = plc_communicator->m_plc_callback(terminal_id_temp + 1, plc_communicator->mp_plc_owner);
                                if (ec.is_equal_error_manager(Error_manager(Error_code::SUCCESS)))
                                {
                                    plc_communicator->m_plc_region_status[terminal_id_temp].current_status = 1;
                                    // 更新时间
                                    plc_communicator->m_plc_region_status[terminal_id_temp].last_time_point = std::chrono::steady_clock::now();
                                    modified = true;
                                }
                                else{
                                    LOG(ERROR)<<ec.to_string();
                                }
                            }
                        }
                        // 指令清空，暂不恢复心跳
                        else if (plc_data_temp[i] == 0)
                        {
                            // int status_temp = plc_communicator->m_plc_region_status[terminal_id_temp].current_status;
                            // if (status_temp != 254 && status_temp != 255)
                            // {
                            //     plc_communicator->m_plc_region_status[terminal_id_temp].current_status = 255;
                            //     // 更新时间
                            //     plc_communicator->m_plc_region_status[terminal_id_temp].last_time_point = std::chrono::steady_clock::now();
                            //     modified = true;
                            // }
                        }
                        // 写入plc
                        if (modified)
                        {
                            int address_temp = PLC_SIGNAL_BEGIN_OFFSET + PLC_SIGNAL_NUM_PER_REGION * terminal_id_temp + PLC_LASER_STATUS_ADDR;
                            uint16_t value_temp = plc_communicator->m_plc_region_status[terminal_id_temp].current_status;
                            int rc = plc_communicator->m_plc_wrapper.write_registers(address_temp, 1, &value_temp);
                            if (rc != 0)
                                plc_communicator->m_plc_current_error = Error_manager(Error_code::PLC_WRITE_FAILED, Error_level::MINOR_ERROR, "写入状态1失败");
                        }
                    }
                }
                plc_communicator->m_plc_mutex.unlock();
            }
            else
            {
                plc_communicator->m_plc_current_error = Error_manager(Error_code::PLC_READ_FAILED, Error_level::MINOR_ERROR, "循环读plc失败");
            }
            // std::cout<<" thread 333 "<<std::endl;

            // 写入心跳或当前运行状态
            for (size_t i = 0; i < PLC_REGION_NUM; i++)
            {
                int time_interval = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - plc_communicator->m_plc_region_status[i].last_time_point).count();
                // 判断超时且非心跳，则转回心跳
                bool modified = false;
                if (time_interval > plc_communicator->m_plc_status_update_timeout && plc_communicator->m_plc_region_status[i].current_status < 5)
                {
                    plc_communicator->m_plc_region_status[i].current_status = 255;
                    modified = true;
                }
                else
                {
                    // 状态切换后写入plc
                    switch (plc_communicator->m_plc_region_status[i].current_status)
                    {
                    case 254:
                        if (time_interval > 1000)
                        {
                            plc_communicator->m_plc_region_status[i].current_status = 255;
                            modified = true;
                        }
                        break;
                    case 255:
                        if (time_interval > 1000)
                        {
                            plc_communicator->m_plc_region_status[i].current_status = 254;
                            modified = true;
                        }
                        break;
                    case 0:
                        plc_communicator->m_plc_region_status[i].current_status = 254;
                        modified = true;
                        break;
                    case 1:
                        if (time_interval > 1000)
                        {
                            plc_communicator->m_plc_region_status[i].current_status = 2;
                            modified = true;
                        }
                        break;
                    case 2:
                        break;
                    case 3:
                        if (time_interval > 3000)
                        {
                            plc_communicator->m_plc_region_status[i].current_status = 254;
                            modified = true;
                        }
                        break;
                    case 4:
                        if (time_interval > 8000)
                        {
                            plc_communicator->m_plc_region_status[i].current_status = 254;
                            modified = true;
                        }
                        break;
                    case 5:
                        break;
                    default:
                        break;
                    }
                }
                
                if (modified && plc_communicator->mb_plc_is_connected)
                {
                    plc_communicator->m_plc_mutex.lock();
                    // std::cout<<" thread 444 "<<std::endl;
                    // 更新时间
                    plc_communicator->m_plc_region_status[i].last_time_point = std::chrono::steady_clock::now();
                    // 写入plc
                    int address_temp = PLC_SIGNAL_BEGIN_OFFSET + PLC_SIGNAL_NUM_PER_REGION * i + PLC_LASER_STATUS_ADDR;
                    uint16_t value_temp = plc_communicator->m_plc_region_status[i].current_status;
                    int rc = plc_communicator->m_plc_wrapper.write_registers(address_temp, 1, &value_temp);
                    if (rc != 0)
                        plc_communicator->m_plc_current_error = Error_manager(Error_code::PLC_WRITE_FAILED, Error_level::MINOR_ERROR, "写入当前状态失败");
                    plc_communicator->m_plc_mutex.unlock();
                    // std::cout<<" thread 555 "<<std::endl;
                }
            }
        }

        usleep(1000 * PLC_SLEEP_IN_MILLISECONDS);
    }
}