modbus_linux/PLCMonitor.cpp

#include "stdafx.h"
#include "Automatic.h"
#include "PLCMonitor.h"

namespace modbus
{
	const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_READY = 10000;		//����źŵ�������ʱ
	const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_PINGPANG = 3000;
	const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_START = 300;
	const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_MAXMEASURE = 600000;

	CPLCMonitor::CPLCMonitor(void* pOwnerObject)
		:_monitoring(FALSE)
		, _heartbeat_write_clock(clock())
		, m_lockValue(0)
		, m_lock_cmd_Value(0)
		, m_callback(0)
		,m_pointer(0)
		, m_logWnd(0)
		,m_laser_data_wnd(0)
    {
        cs_pcl.Initlock(&m_lockValue);
        cs_cmd.Initlock(&m_lock_cmd_Value);
        memset(&_value, 0, sizeof(_value));
    }

    CPLCMonitor::~CPLCMonitor()
    {
		_monitoring = false;
		if (m_thread_read)
		{
			WaitForSingleObject(m_thread_read->m_hThread, INFINITE);
			//CloseHandle(m_pThread);
			m_thread_read = NULL;
		}
    }

    int CPLCMonitor::connect(const char *ip, int port, int slave_id)
    {
        cs_pcl.Lock();
        if (-1 == dev.initialize(ip, port, slave_id))
        {
            cs_pcl.Unlock();
            return -1;
        }
        cs_pcl.Unlock();

        _monitoring = TRUE;
        if (FALSE == Start())
        {
            dev.deinitialize();
            return -1;
        }
		m_thread_read = AfxBeginThread(thread_monitor, this);
		m_strIP = ip;
		m_port = port;
		m_slave_id = slave_id;

        return 0;
    }

	void CPLCMonitor::set_callback(CommandCallback func, void* pointer)
	{
		m_callback = func;
		m_pointer = pointer;
	}

	int CPLCMonitor::reconnect()
	{
		cs_pcl.Lock();
		if (-1 == dev.initialize(m_strIP.c_str(), m_port, m_slave_id))
		{
			cs_pcl.Unlock();
			return -1;
		}
		cs_pcl.Unlock();
		return 0;
	}

    void CPLCMonitor::disconnect()
    {
        _monitoring = FALSE;
        Join(1000);
        Terminate();

        cs_pcl.Lock();
        dev.deinitialize();
        cs_pcl.Unlock();
    }

    int CPLCMonitor::read_registers(int addr, int nb, uint16_t *dest)
    {
        cs_pcl.Lock();
        int rc = dev.read_registers(addr, nb, dest);
        cs_pcl.Unlock();
		if (0 != rc)
		{
			reconnect();//�˿�������
		}
        return rc;
    }

    int CPLCMonitor::write_registers(int addr, int nb, uint16_t *dest)
    {
        cs_pcl.Lock();
        int rc = dev.write_registers(addr, nb, dest);
        cs_pcl.Unlock();

        return rc;
    }

    int CPLCMonitor::read_register(int addr, uint16_t *dest)
    {
        cs_pcl.Lock();
        int rc = dev.read_register(addr, dest);
        cs_pcl.Unlock();
        return rc;
    }

    int CPLCMonitor::write_register(int addr, uint16_t *dest)
    {
        cs_pcl.Lock();
        int rc = dev.write_register(addr, dest);
        cs_pcl.Unlock();

        return rc;
    }

    int CPLCMonitor::clear_laserstatus_register(uint16_t value)
    {
        cs_pcl.Lock();
        int rc = dev.write_register(REG_WHISKBOOMLASER_STATUS, &value);
        cs_pcl.Unlock();
        _heartbeat_write_clock = clock();

        return rc;
    }

    int CPLCMonitor::write_laserstatus_register(uint16_t value)
    {
        uint16_t dest = 0xff;
        cs_pcl.Lock();

        int rc = dev.read_register(REG_WHISKBOOMLASER_STATUS, &dest);
        if (PLC_LASER_ERROR != dest)
        {
            rc = dev.write_register(REG_WHISKBOOMLASER_STATUS, &value);
        }
        cs_pcl.Unlock();

        _heartbeat_write_clock = clock();

		if (0 != rc)
		{
			reconnect();//�˿�������
		}

        return rc;
    }

    int CPLCMonitor::write_laserresult_register(int addr,uint16_t *pvalue)
    {
        uint16_t dest = 0xff;
        cs_pcl.Lock();

		int rc= dev.write_registers(addr, 7, pvalue);
        /*int rc = dev.read_register(REG_WHISKBOOMLASER_STATUS, &dest);
        if (PLC_LASER_ERROR != dest)
        {
            rc = dev.write_registers(REG_WHISKBOOMLASER_STATUS, 7, pvalue);
        }*/
        cs_pcl.Unlock();

        _heartbeat_write_clock = clock();

        return rc;
    }

	void CPLCMonitor::MeasureComplete(bool bOK)
	{
		//_measure_finished = TRUE;
		if (!bOK)
		{
			write_laserstatus_register(PLC_LASER_FINISH_FAILED);
		}
		else
		{
			write_laserstatus_register(PLC_LASER_FINISH_OK);
		}
	}

    bool CPLCMonitor::setMeasureResult(int addr, struct whiskboom_laser_value * p)
    {
		if (p == 0||addr<0)	//ʧ�ܣ�û�а�ɨ�����Ӳ������
		{
			return false;
		}
		else
		{
			uint16_t response[7] = { 0 };
			memset(response, 0, sizeof(uint16_t) * 7);
			if (p->corrected)
			{
				response[6] = eLaser_data_ok;
			}
			else {
				response[6] = eLaser_data_failed;
			}
			response[0] = p->x;
			response[1] = p->y;
			response[2] = (int(p->a) % 18000);
			response[3] = p->l;
			response[4] = p->w;
			response[5] = p->h;
			write_laserresult_register(addr,(uint16_t *)response);
			return true;
		}
    }

	void CPLCMonitor::Monitoring()
	{
		const int SIGNAL_NUM = 44;
		const int SIGNAL_ADDR = 0;
		static uint16_t value[SIGNAL_NUM] = { 0 };
		static uint16_t last_value[SIGNAL_NUM] = { 0 };
		int rc = read_registers(SIGNAL_ADDR, SIGNAL_NUM, last_value);
		LOG(INFO) << "\t����signal����߳�";
		while (_monitoring)
		{
			memset(value, 0, SIGNAL_NUM * sizeof(uint16_t));
			int rc = read_registers(SIGNAL_ADDR, SIGNAL_NUM, value);
			
			if (rc == 0)
			{
				for (int i = REG_PARKSTATUS; i < SIGNAL_NUM; ++i)
				{
					if (value[i] != last_value[i])
					{
						if (m_logWnd != NULL)
							::PostMessage(m_logWnd->m_hWnd, WM_LOG_MSG, (WPARAM)(i + SIGNAL_ADDR), (LPARAM)(value[i]));		
					}
				}
				memcpy(last_value, value, sizeof(uint16_t)*SIGNAL_NUM);
			}
			if (m_logWnd != NULL)
				::PostMessage(m_logWnd->m_hWnd, WM_MONITOR_MSG, (WPARAM)SIGNAL_NUM, (LPARAM)value);
			if (m_laser_data_wnd)
				::PostMessage(m_laser_data_wnd->m_hWnd, WM_MONITOR_MSG, (WPARAM)(SIGNAL_NUM), (LPARAM)(value));
			Sleep(100);
		}
		LOG(INFO) << "\t����signal����߳�";
	}

	UINT CPLCMonitor::thread_monitor(LPVOID lp)
	{
		CPLCMonitor* plc = (CPLCMonitor*)lp;
		plc->Monitoring();
		return 0;
	}

    void CPLCMonitor::Run()
    {
        uint16_t value[2] = { 0xffff,0xffff };
        uint16_t parking_status = 0xff;
        uint16_t response[7] = { 0 };
        int rc = -1;
        clock_t current_clock;

        uint16_t finite_state_machines = PLC_LASER_READY;
        write_laserstatus_register(finite_state_machines);
        uint16_t las_parkstatus = 0;

        while (_monitoring)
        {
            memset(value, 0xff, sizeof(value));

            rc = read_registers(REG_PARKSTATUS, 2, value);
            if (-1 == rc)
            {
                fprintf(stderr, "CPLCMonitor: read regiser failed\n");
            }

            parking_status = value[0];
            finite_state_machines = value[1];

            //PLC ��Ҫͣ���ȴ�״̬
            if (PLC_PARKING_WAIT == parking_status)
            {
                current_clock = clock();

                //�����ϴ�״̬Ϊ��������ɳɹ����߲������ʧ�ܵ�״̬
                if (PLC_LASER_FINISH_OK == finite_state_machines
                    || PLC_LASER_FINISH_FAILED == finite_state_machines)
                {
                    //����10�룬��ʼ���ؾ���״̬
                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_READY)
                    {
						//���������
						if (PLC_LASER_FINISH_FAILED == finite_state_machines)
						{
							/*memset(response, 0, 7 * sizeof(uint16_t));
							write_laserresult_register((uint16_t *)response);*/
						}
                        write_laserstatus_register(PLC_LASER_READY);
						finite_state_machines = PLC_LASER_READY;
                    }
                }
                //�����ϴ�״̬Ϊ��������������״̬1��
                else if (PLC_LASER_READY == finite_state_machines
                    || PLC_LASER_PONG == finite_state_machines)
                {
                    //����3�룬��ʼ��������״̬2
                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
                    {
                        write_laserstatus_register(PLC_LASER_PING);
						finite_state_machines = PLC_LASER_PING;
                    }
                }
                //�����ϴ�״̬Ϊ������״̬2
                else if (PLC_LASER_PING == finite_state_machines)
                {
                    //����3�룬��ʼ��������״̬1
                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
                    {
                        write_laserstatus_register(PLC_LASER_PONG);
						finite_state_machines = PLC_LASER_PONG;
                    }
                }
                //�����ϴ�״̬Ϊ��������ʼ�������У�
                else if (PLC_LASER_START == finite_state_machines
                    || PLC_LASER_WORKING == finite_state_machines)
                {
                    //��ʱ��������ڲ���״̬��,�ü������ֹͣ�������
                    //    1. ���δ�ڲ���״̬���������״̬�������κη���ֵ��
                    //    2. ����ڲ���״̬���в���ʧ����Ϣ�봦��
					LOG(ERROR) << "\t �����п�ʼ�ź���ֹ";
                   /* cs_cmd.Lock();
					for (std::vector<CLaserNet*>::iterator it = _cmdlistLaserPointer.begin(); it != _cmdlistLaserPointer.end(); ++it)
						(*it)->Stop();
					cs_cmd.Unlock();*/

                    write_laserstatus_register(PLC_LASER_READY);
					finite_state_machines = PLC_LASER_READY;
                }
                //�����޸����󣬵�ϵͳ����
                else if (PLC_LASER_ERROR == finite_state_machines)
                {
                    fprintf(stderr, "\n\n\n\n\n\n�����޸����󣬵�ϵͳ����\n\n\n\n\n\n");
                }
                else
                {
                    fprintf(stderr, "\n\n\n\n\n\n&&&&&&&&&!!!!!!!!!!!!!@@@@@@@@@@@@@@$$$$$$$$$$$$$$$$$$$��ȫ�����ܳ��ֵ�״̬\n\n\n\n\n\n");
                }
                las_parkstatus = PLC_PARKING_WAIT;

            }
            //PLC ��Ҫͣ��������ʼ
            else if (parking_status>0)
            {
                //�����ϴ�״̬Ϊ������������״̬1������״̬2
                if(PLC_LASER_READY == finite_state_machines
                        || PLC_LASER_PING == finite_state_machines
                        || PLC_LASER_PONG == finite_state_machines
                        || PLC_LASER_FINISH_OK == finite_state_machines
                        || PLC_LASER_FINISH_FAILED == finite_state_machines)
                {
                    if (PLC_PARKING_WAIT == las_parkstatus)
                        //�ò������Ϊδ���״̬
                    {
                        _measure_finished = FALSE;
						
                        //������ʼ
						if (m_callback)
						{
							//char plate = parking_status;
							
							uint16_t param = 0x03;
							m_callback(true, param, m_pointer);
						}

                        //���ز�����ʼ״̬
                        write_laserstatus_register(PLC_LASER_START);
						finite_state_machines = PLC_LASER_START;
                    }
                }
				//�ϴ�״̬Ϊ��������ɳɹ���������ɴ���
				if (PLC_LASER_FINISH_OK == finite_state_machines
					|| PLC_LASER_FINISH_FAILED == finite_state_machines)
				{
					if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
					{
						write_laserstatus_register(PLC_LASER_PING);
						finite_state_machines = PLC_LASER_PING;
					}
				}

                //�����ϴ�״̬Ϊ:��ʼ����
                else if (PLC_LASER_START == finite_state_machines)
                {
					current_clock = clock();  
                    //����200���룬���ز�����
                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_START)
                    {
                        write_laserstatus_register(PLC_LASER_WORKING);
						finite_state_machines = PLC_LASER_WORKING;
                    }
                }
                else if (PLC_LASER_WORKING == finite_state_machines)
                {
                    if (TRUE == _measure_finished)
                    {
						//�������
                       /* memset(response, 0, sizeof(uint16_t) * 7);
                        if (_value.corrected)
                        {
                            response[0] = PLC_LASER_FINISH_OK;
                        }
                        else {
                            response[0] = PLC_LASER_FINISH_FAILED;
                        }
						response[1] = _value.x;
						response[2] = _value.y;
						response[3] = (int(_value.a) % 18000);
						response[4] = _value.l;
						response[5] = _value.w;
						response[6] = _value.h;
                        write_laserresult_register((uint16_t *)response);*/
						write_laserstatus_register(PLC_LASER_FINISH_FAILED);
						finite_state_machines = PLC_LASER_FINISH_FAILED;
                    }
                    else {
                        current_clock = clock();
                        if (current_clock - _heartbeat_write_clock > PLC_LASER_TIMEOUT_MAXMEASURE) //10���ӻ�û���ز�������� ˵��ϵͳ���ܳ�����
                        {
                            write_laserstatus_register(PLC_LASER_ERROR);
							finite_state_machines = PLC_LASER_ERROR;
                        }
                    }
                }
                else if (PLC_LASER_ERROR == finite_state_machines)
                {

                    fprintf(stderr, "\n\n\n\n\n\n�����޸����󣬵�ϵͳ����\n\n\n\n\n\n");
                }
                else
                {
                    fprintf(stderr, "\n\n\n\n\n\n&&&&&&&&&!!!!!!!!!!!!!@@@@@@@@@@@@@@$$$$$$$$$$$$$$$$$$$��ȫ�����ܳ��ֵ�״̬\n\n\n\n\n\n");
                }

                las_parkstatus = PCL_PARKING_REQUEST;
            }
            else
            {
				if (PCL_PARKING_ERROR != las_parkstatus)
                {
                    LOG(ERROR)<<("\t���ؾ���:PLC��Ҫͣ��״̬�쳣�������޸����󣬵�ϵͳ����");
					//ֹͣ���м���
					if (m_callback)
					{
						char laser = 0xFF;
						m_callback(false, uint16_t(laser), m_pointer);
					}
                    write_laserstatus_register(PLC_LASER_READY);
					finite_state_machines = PLC_LASER_READY;
                }
                Sleep(200);
				las_parkstatus = PCL_PARKING_ERROR;
            }
            Sleep(50);
        }
    }
}