pip/piprotocol.cpp

#include "piprotocol.h"


PIProtocol::PIProtocol(PIString devName, int speed, void * headerPtr, int headerSize, void * dataPtr, int dataSize) {
    init();
    type = PIProtocol::Serial;
    serial = new PISerial(devName, this, received);
    serial->setReadData(headerPtr, headerSize, dataSize);
    this->devName = devName;
    this->headerPtr = (unsigned char * )headerPtr;
    this->headerSize = headerSize;
    this->dataPtr = (unsigned char * )dataPtr;
    this->dataSize = dataSize;
    packet = new char[headerSize + dataSize];
}


PIProtocol::PIProtocol(PIString ip, int port, void * dataPtr, int dataSize) {
    init();
    type = PIProtocol::Ethernet;
    ether = new PIEthernet(ip, port, this, received);
    this->devName = ip + ":" + itos(port);
    this->dataPtr = (unsigned char * )dataPtr;
    this->dataSize = dataSize;
    packet = new char[dataSize];
}


PIProtocol::~PIProtocol() {
	delete sendtimer;
    if (timer != 0) delete timer;
    if (type == PIProtocol::Serial) delete serial;
    if (type == PIProtocol::Ethernet) delete ether;
}


void PIProtocol::init() {
    work = false;
    net_diag = PIProtocol::Unknown;
    cur_pckt = 0;
    timer = 0;
    sendtimer = new PITimer(run, this);
    wrong_count = receive_count = send_count = 0;
    immediateFreq = integralFreq = 0.f;
    headerPtr = dataPtr = sendDataPtr = 0;
    headerSize = dataSize = sendDataSize = 0;
    exp_freq = 20.f;
    devState = "Unknown";
}


void PIProtocol::setDevice(const PIString & dev_ip, int speed_port) {
	if (type == PIProtocol::Serial) {
		serial->setDevice(dev_ip);
		serial->setSpeed(speed_port);
		devName = dev_ip;
	}
    if (type == PIProtocol::Ethernet) {
		ether->setReadAddress(dev_ip, speed_port);
		devName = dev_ip + ":" + itos(speed_port);
	}
}


void PIProtocol::setExpectedFrequency(float frequency)
{
    exp_freq = frequency;
    if (exp_freq < 3.33) pckt_cnt_max = 10;
    else pckt_cnt_max = 3 * (int)exp_freq;
    last_packets.resize(pckt_cnt_max);
    timer = new PITimer(diag_event, this);
	timer->start(1000. / exp_freq);
	timer->reset();
}


void PIProtocol::startReceive()
{
    if (type == PIProtocol::Serial) serial->start();
    if (type == PIProtocol::Ethernet) ether->start();
}


void PIProtocol::stopReceive()
{
    if (type == PIProtocol::Serial) serial->stop();
    if (type == PIProtocol::Ethernet) ether->stop();
}


bool PIProtocol::received(void * t, char * data) {
    PIProtocol * p = (PIProtocol * )t;
    memcpy(p->dataPtr, data, p->dataSize);
    p->work = true;
    //p->lock();
    if (p->validate())
    {
        //p->unlock();
        p->receive_count++;
        p->cur_pckt = 1;
        return true;
    }
    //p->unlock();
    p->wrong_count++;
    return false;
}


void PIProtocol::diag_event(void * t) {
    PIProtocol * p = (PIProtocol * )t;
    p->calc_freq();
    p->calc_diag();
    p->check_state();
}


void PIProtocol::calc_diag() {
    PIProtocol::Quality diag;
    if (!work) {
        diag = PIProtocol::Unknown;
        return;
    }
    if (pckt_cnt < pckt_cnt_max) {
        last_packets[pckt_cnt] = cur_pckt;
        pckt_cnt++;
    } else {
        packets[(int)last_packets.back()]--;
        last_packets.pop_back();
        last_packets.push_front(cur_pckt);
    }
    packets[(int)cur_pckt]++;
    cur_pckt = 0;
    float good_percents;
    good_percents = (float)packets[1] / pckt_cnt * 100.0f;
    if (good_percents == 0.0) diag = PIProtocol::Failure;
    else if (good_percents <= 20.0) diag = PIProtocol::Bad;
    else if (good_percents > 20.0 && good_percents <= 80.0) diag = PIProtocol::Average;
    else diag = PIProtocol::Good;
    if (diag != net_diag) net_diag = diag;
}


void PIProtocol::calc_freq() {
	tf = 1000. / timer->elapsed_m();
	timer->reset();
	if (cur_pckt != 1) tf = 0.;
    immediateFreq = tf;
    if (last_freq.size() >= pckt_cnt_max) last_freq.pop_front();
    last_freq.push_back(tf);
    tf = last_freq[0];
    for (uint i = 1; i < last_freq.size(); ++i)
        tf += last_freq[i];
    integralFreq = tf / last_freq.size();
}


void PIProtocol::check_state() {
    if (type == PIProtocol::Serial) {
        if (serial->initialized()) devState = "Initialized";
        else devState = "Uninitialized";
    }
    if (type == PIProtocol::Ethernet) {
        if (ether->initialized()) devState = "Initialized";
        else devState = "Uninitialized";
    }
}



void PIProtocol::send() {
    //lock();
    memcpy(packet, sendDataPtr, sendDataSize);
    //unlock();
    if (type == PIProtocol::Serial)
        if (serial->send(packet, sendDataSize))
            send_count++;
    if (type == PIProtocol::Ethernet)
        if (ether->send(packet, sendDataSize))
            send_count++;
}