29.07.2011 - fundamental new
This commit is contained in:
peri4
411
piprotocol.cpp
411
piprotocol.cpp
@@ -1,180 +1,327 @@
|
||||
#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(const PIString & config, const PIString & name, void * recHeaderPtr, int recHeaderSize, void * recDataPtr, int recDataSize, void * sendDataPtr_, int sendDataSize_) {
|
||||
init();
|
||||
PIConfig conf(config, PIFile::Read);
|
||||
if (!conf.isOpened()) {
|
||||
cout << "[PIProtocol \"" << name << "\"] Can`t open \"" << config << "\"!" << endl;
|
||||
devReceiverState = devSenderState = "Config error";
|
||||
return;
|
||||
}
|
||||
int ps;
|
||||
bool ok;
|
||||
PIFlags<PISerial::Parameters> pp;
|
||||
PIConfig::Entry b = conf.getValue(name),
|
||||
rb = b.getValue("receiver"),
|
||||
sb = b.getValue("sender");
|
||||
PIString dev;
|
||||
|
||||
/// receiver section
|
||||
if (rb.isEntryExists("ip") && rb.isEntryExists("device")) {
|
||||
cout << "[PIProtocol \"" << name << "\"] Ambiguous receiver type in \"" << config << "\"!" << endl;
|
||||
devReceiverState = "Config error";
|
||||
return;
|
||||
}
|
||||
dev = rb.getValue("ip", "", &ok);
|
||||
if (ok) {
|
||||
ps = rb.getValue("port", 0, &ok);
|
||||
if (!ok) {
|
||||
type_rec = PIProtocol::None;
|
||||
cout << "[PIProtocol \"" << name << "\"] Can`t find \"" << name << ".receiver.port\" in \"" << config << "\"!" << endl;
|
||||
devReceiverState = "Config error";
|
||||
return;
|
||||
}
|
||||
type_rec = PIProtocol::Ethernet;
|
||||
eth = new PIEthernet(dev, ps, this, receiveEvent);
|
||||
devReceiverName = dev + ":" + PIString::fromNumber(ps);
|
||||
}
|
||||
dev = rb.getValue("device", "", &ok);
|
||||
if (ok) {
|
||||
ps = rb.getValue("speed", 0, &ok);
|
||||
if (!ok) {
|
||||
cout << "[PIProtocol \"" << name << "\"] Can`t find \"" << name << ".receiver.speed\" in \"" << config << "\"!" << endl;
|
||||
devReceiverState = "Config error";
|
||||
return;
|
||||
}
|
||||
if (rb.getValue("parity", false)) pp |= PISerial::ParityControl;
|
||||
if (rb.getValue("twoStopBits", false)) pp |= PISerial::TwoStopBits;
|
||||
type_rec = PIProtocol::Serial;
|
||||
ser = new PISerial(dev, this, receiveEvent);
|
||||
ser->setInSpeed((PISerial::Speed)ps);
|
||||
ser->setParameters(pp);
|
||||
ser->setReadData(recHeaderPtr, recHeaderSize, recDataSize);
|
||||
devReceiverName = dev;
|
||||
}
|
||||
|
||||
/// sender section
|
||||
if (sb.isEntryExists("ip") && sb.isEntryExists("device")) {
|
||||
cout << "[PIProtocol \"" << name << "\"] Ambiguous sender type in \"" << config << "\"!" << endl;
|
||||
devSenderState = "Config error";
|
||||
return;
|
||||
}
|
||||
dev = sb.getValue("ip", "", &ok);
|
||||
if (ok) {
|
||||
ps = sb.getValue("port", 0, &ok);
|
||||
if (!ok) {
|
||||
type_send = PIProtocol::None;
|
||||
cout << "[PIProtocol \"" << name << "\"] Can`t find \"" << name << ".sender.port\" in \"" << config << "\"!" << endl;
|
||||
devSenderState = "Config error";
|
||||
return;
|
||||
}
|
||||
type_send = PIProtocol::Ethernet;
|
||||
if (eth == 0) eth = new PIEthernet(dev, ps, this, receiveEvent);
|
||||
setSenderAddress(dev, ps);
|
||||
devSenderName = dev + ":" + PIString::fromNumber(ps);
|
||||
}
|
||||
dev = sb.getValue("device", "", &ok);
|
||||
if (ok) {
|
||||
ps = sb.getValue("speed", 0, &ok);
|
||||
if (!ok) {
|
||||
cout << "[PIProtocol \"" << name << "\"] Can`t find \"" << name << ".sender.speed\" in \"" << config << "\"!" << endl;
|
||||
devSenderState = "Config error";
|
||||
return;
|
||||
}
|
||||
if (sb.getValue("parity", false)) pp |= PISerial::ParityControl;
|
||||
if (sb.getValue("twoStopBits", false)) pp |= PISerial::TwoStopBits;
|
||||
type_send = PIProtocol::Serial;
|
||||
if (ser == 0) ser = new PISerial(dev, this, receiveEvent);
|
||||
ser->setOutSpeed((PISerial::Speed)ps);
|
||||
ser->setParameters(pp);
|
||||
ser->setReadData(recHeaderPtr, recHeaderSize, recDataSize);
|
||||
devSenderName = dev;
|
||||
}
|
||||
|
||||
float fr = rb.getValue("frequency", 0.f, &ok);
|
||||
if (ok) setExpectedFrequency(fr);
|
||||
fr = sb.getValue("frequency", 0.f, &ok);
|
||||
if (ok) setSenderFrequency(fr);
|
||||
headerPtr = (uchar * )recHeaderPtr;
|
||||
headerSize = recHeaderSize;
|
||||
dataPtr = (uchar * )recDataPtr;
|
||||
dataSize = recDataSize;
|
||||
sendDataPtr = (uchar * )sendDataPtr_;
|
||||
sendDataSize = sendDataSize_;
|
||||
if (type_rec == PIProtocol::Ethernet) {
|
||||
if (recHeaderPtr != 0) {
|
||||
dataPtr = (uchar * )recHeaderPtr;
|
||||
dataSize = recHeaderSize + recDataSize;
|
||||
if (dataSize > 0) packet = new char[dataSize];
|
||||
} else if (recDataSize > 0) packet = new char[recDataSize];
|
||||
} else if (recHeaderSize + recDataSize > 0) packet = new char[recHeaderSize + recDataSize];
|
||||
}
|
||||
|
||||
|
||||
PIProtocol::~PIProtocol() {
|
||||
delete sendtimer;
|
||||
if (timer != 0) delete timer;
|
||||
if (type == PIProtocol::Serial) delete serial;
|
||||
if (type == PIProtocol::Ethernet) delete ether;
|
||||
delete diagTimer;
|
||||
delete sendTimer;
|
||||
if (packet != 0) delete packet;
|
||||
if (eth != 0) delete eth;
|
||||
if (ser != 0) delete ser;
|
||||
}
|
||||
|
||||
|
||||
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";
|
||||
work = false;
|
||||
ret_func = 0;
|
||||
net_diag = PIProtocol::Unknown;
|
||||
cur_pckt = 0;
|
||||
diagTimer = 0;
|
||||
packet = 0;
|
||||
sendTimer = new PITimer(sendEvent, this);
|
||||
diagTimer = new PITimer(diagEvent, this);
|
||||
wrong_count = receive_count = send_count = 0;
|
||||
send_freq = -1.f;
|
||||
immediateFreq = integralFreq = exp_freq = 0.f;
|
||||
headerPtr = dataPtr = sendDataPtr = 0;
|
||||
headerSize = dataSize = sendDataSize = 0;
|
||||
eth = 0;
|
||||
ser = 0;
|
||||
type_rec = type_send = PIProtocol::None;
|
||||
devSenderState = devReceiverState = "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::setReceiverDevice(const PIString & device, PISerial::Speed speed) {
|
||||
if (type_rec == PIProtocol::Serial) {
|
||||
ser->setDevice(device);
|
||||
ser->setSpeed(speed);
|
||||
devReceiverName = device;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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::setReceiverDevice(const PIString & ip, int port) {
|
||||
if (type_rec == PIProtocol::Ethernet) {
|
||||
eth->setReadAddress(ip, port);
|
||||
devReceiverName = ip + ":" + PIString::fromNumber(port);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void PIProtocol::startReceive()
|
||||
{
|
||||
if (type == PIProtocol::Serial) serial->start();
|
||||
if (type == PIProtocol::Ethernet) ether->start();
|
||||
void PIProtocol::setSenderDevice(const PIString & device, PISerial::Speed speed) {
|
||||
if (type_send == PIProtocol::Serial) {
|
||||
ser->setDevice(device);
|
||||
ser->setSpeed(speed);
|
||||
devSenderName = device;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void PIProtocol::stopReceive()
|
||||
{
|
||||
if (type == PIProtocol::Serial) serial->stop();
|
||||
if (type == PIProtocol::Ethernet) ether->stop();
|
||||
void PIProtocol::setSenderDevice(const PIString & ip, int port) {
|
||||
if (type_send == PIProtocol::Ethernet) {
|
||||
eth->setReadAddress(ip, port);
|
||||
devSenderName = ip + ":" + PIString::fromNumber(port);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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::setExpectedFrequency(float frequency) {
|
||||
exp_freq = frequency;
|
||||
if (exp_freq < 10.f / 3.f) pckt_cnt_max = 10;
|
||||
else pckt_cnt_max = round(3.f * exp_freq);
|
||||
last_packets.resize(pckt_cnt_max);
|
||||
}
|
||||
|
||||
|
||||
void PIProtocol::diag_event(void * t) {
|
||||
PIProtocol * p = (PIProtocol * )t;
|
||||
p->calc_freq();
|
||||
p->calc_diag();
|
||||
p->check_state();
|
||||
void PIProtocol::startReceive(float exp_frequency) {
|
||||
if (exp_frequency > 0.f) exp_freq = exp_frequency;
|
||||
if (type_rec == PIProtocol::Serial) ser->start();
|
||||
if (type_rec == PIProtocol::Ethernet) eth->start();
|
||||
if (exp_freq <= 0.f) return;
|
||||
setExpectedFrequency(exp_freq);
|
||||
diagTimer->start(1000. / exp_freq);
|
||||
diagTimer->reset();
|
||||
}
|
||||
|
||||
|
||||
void PIProtocol::startSend(float frequency) {
|
||||
if (frequency > 0.f) send_freq = frequency;
|
||||
if (send_freq <= 0.f) return;
|
||||
sendTimer->start(1000. / send_freq);
|
||||
}
|
||||
|
||||
|
||||
void PIProtocol::stopReceive() {
|
||||
if (type_rec == PIProtocol::Serial) ser->stop();
|
||||
if (type_rec == PIProtocol::Ethernet) eth->stop();
|
||||
diagTimer->stop();
|
||||
}
|
||||
|
||||
|
||||
bool PIProtocol::receiveEvent(void * t, char * data, int size) {
|
||||
PIProtocol * p = (PIProtocol * )t;
|
||||
if (!p->receive(data, size)) return false;
|
||||
p->work = true;
|
||||
//p->lock();
|
||||
if (p->validate())
|
||||
{
|
||||
//p->unlock();
|
||||
p->receive_count++;
|
||||
p->cur_pckt = 1;
|
||||
if (p->ret_func != 0) p->ret_func(p);
|
||||
return true;
|
||||
}
|
||||
//p->unlock();
|
||||
p->wrong_count++;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void PIProtocol::diagEvent(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;
|
||||
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.f;
|
||||
if (good_percents == 0.f) diag = PIProtocol::Failure;
|
||||
else if (good_percents <= 20.f) diag = PIProtocol::Bad;
|
||||
else if (good_percents > 20.f && good_percents <= 80.f) 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();
|
||||
tf = 1000.f / diagTimer->elapsed_m();
|
||||
diagTimer->reset();
|
||||
if (cur_pckt != 1) tf = 0.f;
|
||||
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";
|
||||
}
|
||||
if (type_send == PIProtocol::Serial) {
|
||||
if (ser->initialized()) devSenderState = "Initialized";
|
||||
else devSenderState = "Uninitialized";
|
||||
return;
|
||||
}
|
||||
if (type_send == PIProtocol::Ethernet) {
|
||||
if (eth->senderInitialized()) devSenderState = "Initialized";
|
||||
else devSenderState = "Uninitialized";
|
||||
return;
|
||||
}
|
||||
if (type_rec == PIProtocol::Serial) {
|
||||
if (ser->initialized()) devReceiverState = "Initialized";
|
||||
else devReceiverState = "Uninitialized";
|
||||
return;
|
||||
}
|
||||
if (type_rec == PIProtocol::Ethernet) {
|
||||
if (eth->receiverInitialized()) devReceiverState = "Initialized";
|
||||
else devReceiverState = "Uninitialized";
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void PIProtocol::send(const void * data, int size) {
|
||||
if (data == 0 || size == 0) return;
|
||||
if (!aboutSend()) return;
|
||||
if (type_rec == PIProtocol::Serial)
|
||||
if (ser->send((char * )data, size))
|
||||
send_count++;
|
||||
if (type_rec == PIProtocol::Ethernet)
|
||||
if (eth->send((char * )data, size))
|
||||
send_count++;
|
||||
}
|
||||
|
||||
|
||||
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++;
|
||||
//lock();
|
||||
//memcpy(packet, sendDataPtr, sendDataSize);
|
||||
//unlock();
|
||||
if (sendDataPtr == 0 || sendDataSize == 0) return;
|
||||
if (!aboutSend()) return;
|
||||
if (type_rec == PIProtocol::Serial)
|
||||
if (ser->send((char * )sendDataPtr, sendDataSize))
|
||||
send_count++;
|
||||
if (type_rec == PIProtocol::Ethernet)
|
||||
if (eth->send((char * )sendDataPtr, sendDataSize))
|
||||
send_count++;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user