void MessageBuffer::logBadPacketSize() {
uint32_t msgSize = *reinterpret_cast<const uint32_t*>(buffer->buffer.base);
if(bufferOverflow) {
log(LL_Error,
"Packet read/write overflow: id: %d, packet size: %d, buffer size: %d, offset: %d, field: %s\n",
getMessageId(),
msgSize,
getSize(),
uint32_t(p - buffer->buffer.base),
bufferOverflow ? getFieldInOverflow().c_str() : "<no overflow>");
} else if(msgSize != getSize()) {
log(LL_Error,
"Packet size is not buffer size: id: %d, packet size: %d, buffer size: %d, offset: %d\n",
getMessageId(),
msgSize,
getSize(),
uint32_t(p - buffer->buffer.base));
} else if(uint32_t(p - buffer->buffer.base) != msgSize) {
log(LL_Error,
"Packet was not fully read/written: id: %d, packet size: %d, buffer size: %d, offset: %d\n",
getMessageId(),
msgSize,
getSize(),
uint32_t(p - buffer->buffer.base));
} else {
log(LL_Error,
"Packet has invalid data: id: %d, packet size: %d, buffer size: %d, offset: %d, field: %s\n",
getMessageId(),
msgSize,
getSize(),
uint32_t(p - buffer->buffer.base),
bufferOverflow ? getFieldInOverflow().c_str() : "<no overflow>");
}
}
bool Packet::parse()
{
_packet.messageId = 0;
_packet.messageLenghtType = 0;
_packet.messageLength = 0;
_packet.messageData = nullptr;
if (_data.bytesAvailable() < sizeof(short))
return false;
unsigned short staticHeader = _data.readUShort();
_packet.messageId = getMessageId(staticHeader);
_packet.messageLenghtType = getMessageLenghtType(staticHeader);
if (_data.bytesAvailable() < _packet.messageLenghtType)
return false;
_packet.messageLength = readMessageLenght(_packet.messageLenghtType);
if (_data.bytesAvailable() < _packet.messageLength)
return false;
_packet.messageData = _data.readBytes(_packet.messageLength);
return true;
}
/*******************************************************************
* Function Name: externalFunction
* Description: the router gets input from either the "outside" (a new messgae to route) or from the router (next step for routing)
********************************************************************/
Model &Dispatcher::externalFunction( const ExternalMessage &msg ){
if(VERBOSE) cout<<"external message: "<<msg.value()<<endl;
if ( this->state() == passive)
{
if (msg.port() == msg_in) //putting something in the routing table
{
//expecting a message including the peerid and a messageId encoded according to the spec of "complexmessages.h"
int peer = getPeerId(msg.value());// get originating peer (from value of external msg)
if(VERBOSE)cout<<" peer :"<<peer<<endl;
int qq = getMessageId(msg.value()); // get query (encoded as messageid)
if(VERBOSE)cout<<" query :"<<qq<<endl;
//generate new message id
int msgid;
msgid = id_counter;
id_counter = (id_counter + 1)%MAXID; //count
nextOutput_q = buildMessage(msgid, qq);
nextOutput_p = buildMessage(msgid, peer);
}
} //end if state is passive
else{
cout<<"Dispatcher error: message received while in active state"<<endl;
}
// we have an instantaneous change back to the passive state (will output the next output values where relevant)
holdIn( active, Time(0.00f));
return *this ;
}
bool Packet::deserialize(ByteArray& buffer)
{
_header = 0;
_id = 0;
_lengthType = 0;
_length = 0;
_data.clear();
BinaryReader reader(buffer);
uint countReadedBytes = 0;
if (reader.bytesAvailable() < sizeof(_header))
{
return false;
}
_header = reader.readUShort();
_id = getMessageId(_header);
_lengthType = getMessageLengthType(_header);
countReadedBytes += sizeof(_header);
if (reader.bytesAvailable() < _lengthType)
{
return false;
}
_length = getMessageLength(_lengthType, reader);
countReadedBytes += _lengthType;
if (reader.bytesAvailable() < _length)
{
return false;
}
_data = reader.readBytes(_length);
countReadedBytes += _length;
buffer.erase(buffer.begin(), buffer.begin() + countReadedBytes);
return true;
}
/*******************************************************************
* Function Name: externalFunction
* Description: the router gets input from either the "outside" (a new messgae to route) or from the router (next step for routing)
********************************************************************/
Model &Server::externalFunction( const ExternalMessage &msg ){
if(VERBOSE) cout<<endl<<"external message: "<<msg.value()<<endl;
/*if ( this->state() == passive)
{*/
if (msg.port() == query_in) //putting something in the routing table
{
//expecting a message including the peerid and a messageId encoded according to the spec of "complexmessages.h"
/* int payload = getPeerId(msg.value());// we encode the msg payload instead of the peerid
if(VERBOSE)cout<<" query: "<<payload<<endl;
int id = getMessageId(msg.value()); // get message id
if(VERBOSE)cout<<" message id: "<<id<<endl;
//create "mapping entry" list. The message ids are float values (that way we can generate them using a random function)
// to create an empty list I use the default constructor, throught the shortcut of calling the [] operator.
routingTable[id]=payload; //associates peerid to id in routing table.
if(VERBOSE)cout<<" RoutingTable id: "<<id<<endl << endl;//" associated with query: "<<routingTable[id]<<endl;
*/
int payload = getPeerId(msg.value());// we encode the msg payload instead of the peerid
int identifier = getMessageId(msg.value());
if(VERBOSE) cout<< endl <<" query: "<<payload<<endl;
set<int> docsFromQuery;
docsFromQuery = serverdoc -> getDocsFromQuery(payload); // set of documents associated with query
int docsLeft = docsFromQuery.size();
set<int>::iterator documents;
if(VERBOSE) cout << "document(s) associated with query: "<<endl;
if(docsFromQuery.size() > 0){
for(documents = docsFromQuery.begin() ; documents != docsFromQuery.end(); documents++){
if(VERBOSE) cout<< *documents<<" Identifier: "<<identifier<<endl;
docsLeft--;
QueryhitQ.push(buildMessage(*documents,identifier,docsLeft));
}
}
else{
if(VERBOSE) cout << "none"<<endl;
}
}
else if (msg.port() == peer_in){
//expecting value = TTL * 100 + peerid + id, where id is the decimal part (<1)
//if(VERBOSE) cout<<"Message: a query reaches a peer... "<<msg.value()<<"\n";
/*
long inval = msg.value();
//extract our 3 values using static functions from complexmessages.h
//HACK : the msg payload will be encoded in the space meant for the peerId
int inpeer = getPeerId(inval);
//if(VERBOSE)cout<<" peer :"<<inpeer<<endl;
int id = getMessageId(inval);
if(VERBOSE)cout<<" message id: "<<id<<endl;
//check for already visited peer : search for "peerid" in set mapped to id in routing table
int query = routingTable[id]; // get the query associated wityh this id
if(VERBOSE)cout<<" query: "<<query<<endl;
//TODO : what does this return if the msgid is not in the table ?
//TODO : query the peerdoc table, the querydoc table, enqueue all results for output
//set<int> docsFromPeer; //documents associated with peer
set<int> docsFromQuery; //documents associated with the query
//docsFromPeer=peerdoc->getsDocsOfPeer(inpeer);
//docsFromQuery=querydoc->getConnectedNodes(query);
docsFromQuery = serverdoc -> getDocsFromQuery(query);
//if(VERBOSE) cout<<"intersection of document sets: ";
set<int>::iterator doc;
//if(docsFromPeer.size() > 0){
// if(VERBOSE) cout<<"peer has: ";
//}
if(VERBOSE) cout << "document(s) associated with query: ";
for(doc = docsFromQuery.begin() ; doc != docsFromQuery.end(); doc++){
if(VERBOSE) cout <<endl<< *doc << " which also links to: ";
set<int> linkeddocs;
linkeddocs = serverdoc->getDocsFromLink(*doc); //set of all the documents connected to by *doc (doesn't work atm)
set<int>::iterator docs;
if(linkeddocs.size() == 0){
if(VERBOSE) cout << "none";
}
for(docs = linkeddocs.begin(); docs != linkeddocs.end();docs++){
if(VERBOSE) cout << *docs;
}
}
if(VERBOSE) cout << endl << endl;
}
*/
/* Original stuff
for ( doc=docsFromPeer.begin() ; doc != docsFromPeer.end(); doc++ ){ // loop through docs stored by peer
if(VERBOSE) cout<<"peer has : "<<*doc;
if(docsFromQuery.count(*doc)==1) {// if the considered doc matches the query
if(VERBOSE) cout<<" (hit!); ";
QueryhitQ.push(buildMessage(id,*doc, inpeer)); //enqueue a message made from the queryId and the queryhit document
}else {
if(VERBOSE) cout<<" (no match); ";
}
}
}
*/
int docID = getPeerId(msg.value());
int identifier = getMessageId(msg.value());
if(VERBOSE) cout<< endl <<" document: "<<docID<<endl;
set<int> docLinks; // set of documents associated with query
docLinks = serverdoc -> getDocsFromLink(docID);
set<int>::iterator docs;
int docsLeft = docLinks.size();
if(VERBOSE) cout << "This document links to: "<<endl;
if(docLinks.size() > 0){
for(docs = docLinks.begin() ; docs != docLinks.end(); docs++){
if(VERBOSE) cout << *docs << " identifier: " << identifier <<endl;
docsLeft--;
QueryhitQ.push(buildMessage(*docs,identifier,docsLeft));
}
}
else{
if(VERBOSE) cout << "none"<<endl;
}
}
////////////////DATA OPERATIONS : publish, remove //////////////////
// else if (msg.port() == publish){
//extract message content using static functions from complexmessages.h
////add query doc-link
// int query = getPeerId(msg.value());
//if(VERBOSE)cout<<" peer :"<<peer<<endl;
// int doc = getMessageId(msg.value());
//if(VERBOSE)cout<<" message id:"<<doc<<endl;
// if(VERBOSE) cout<<"Query "<<query<<" associated with document "<<doc <<"\n";
// serverdoc->querydoclink(query,doc); //store in the graph
//Peer publishing documents
/*
int peer = getPeerId(msg.value());
//if(VERBOSE)cout<<" peer :"<<peer<<endl;
int doc = getMessageId(msg.value());
//if(VERBOSE)cout<<" message id:"<<doc<<endl;
if(VERBOSE) cout<<"peer "<<peer<<" publishes document "<<doc <<"\n";
peerdoc->publish(peer,doc); //store in the graph
*/
// all below is out of date
//if the peer or the document are not in the matrix, we need to add the peer / doc to the graph
/*if(peerdoc->find(peer)==peerdoc->end()){ // peer not found in graph
if(VERBOSE) cout<<"new peer"<<endl;
peerdoc->insert(peer);
} else
if(VERBOSE) cout<<"found peer:"<<*(peerdoc->find(peer))<<endl;;
if(peerdoc->find(doc)==peerdoc->end()){ // doc not found in graph
if(VERBOSE) cout<<"new doc"<<endl;
peerdoc->insert(doc);
} else
if(VERBOSE) cout<<"found doc:"<<*(peerdoc->find(peer))<<endl;;
peerdoc->connect(peer,doc);
if(VERBOSE) peerdoc->write(cout);*/
//}
/* else if (msg.port() == remove){
//extract message content using static functions from complexmessages.h
int peer = getPeerId(msg.value());
if(VERBOSE)cout<<" peer : "<<peer<<endl;
int doc = getMessageId(msg.value());
if(VERBOSE)cout<<" message id: "<<doc<<endl;
if(VERBOSE) cout<<"peer "<<peer<<" removes document "<<doc <<"\n";
//peerdoc->disconnect(peer,doc);
peerdoc->remove(peer,doc); // this is the appropriate disconnection, not removal of nodes
}
*/
/*} //end if state is passive
else{
cout<<"DB error: message received while in active state"<<endl;
}*/
if ( this->state() == passive) {
holdIn( active, Time(0,0,0,2));//pause for 20 milliseconds
}
/*/Any responses to output ?
if (!QueryhitQ.empty()) { // if we were or now are in the process of routing messages
holdIn( active, Time(0.01f)); // we wait 0.01s to dequeue
} else {
holdIn( active, Time(0.00f)); // we just passivate immediately
}*/
return *this ;
}
/*******************************************************************
* Function Name: externalFunction
* Description: the Network gets input from outside
********************************************************************/
Model <SNetwork::externalFunction( const ExternalMessage &msg ){
if (msg.port() == peer_online)
{
thegraph->online(msg.value()); //adds a node to the graph with the given value
if(VERBOSE) cout<<"node "<<msg.value()<<" inserted\n";
//holdIn( active, Time(0.00f));
}
else if (msg.port() == peer_offline){
int inpeer = msg.value();
//get all the connected nodes a disconnect them, plus let them know they've been disconnected
set<int> connected = thegraph->getConnectedNodes(inpeer);
set<int>::iterator sit;
for ( sit=connected.begin() ; sit != connected.end(); sit++ ){
thegraph->disconnect(inpeer, *sit); //disconnect them
DisconnectionQueue.push(buildMessage(*sit, inpeer)); //enqueue a message saying peer "inpeer" disconnects from "*sit"
}
thegraph->offline(inpeer);
if(VERBOSE) cout<<"node "<<msg.value()<<" removed\n";
//holdIn( active, Time(0.00f));
}
else if (msg.port() == peer_connect){
int twonumbers, from, to;
twonumbers = msg.value();
from = getPeerId(twonumbers); //first and second field encoding of the peers
to = getMessageId(twonumbers);
if(VERBOSE) cout<<"connecting "<<from<<" to "<< to<<"\n";
if(thegraph->connect(from,to))
ConnectionQueue.push(buildMessage(to, from, 1)); // enqueue a connection message : adding the TTL makes it different from a disconnect message, further down the road
//holdIn( active, Time(0.00f));
}
else if (msg.port() == peer_disconnect){
int twonumbers, from, to;
twonumbers = msg.value();
from = getPeerId(twonumbers); //first and second field encoding of the peers
to = getMessageId(twonumbers);
if(VERBOSE) cout<<"disconnecting "<<from<<" and "<< to<<"\n";
if(thegraph->disconnect(from, to))
DisconnectionQueue.push(twonumbers); // enqueue the original message to be re-output as confirmation that connection took place
//holdIn( active, Time(0.00f));
}
else if (msg.port() == inroute){
//routing=true;
int inpeer, TTL, messageId;
inpeer = getPeerId(msg.value());
TTL= getTTL(msg.value());
messageId = getMessageId(msg.value());
if(VERBOSE) cout<<"about to route a message from"<<inpeer<<"\n";
//get all the connected nodes and enqueue the "arrival of the message" event for all these new nodes
//find the nodes connected to this one
set<int> connected = thegraph->getConnectedNodes(inpeer);
//if(VERBOSE) cout<<"loop for enqueuing nodes :"<<connected.size()<<" nodes to enqueue";
set<int>::iterator sit;
//if(VERBOSE) cout << "connected nodes contains:";
for ( sit=connected.begin() ; sit != connected.end(); sit++ ){
// if(VERBOSE) cout<<"bang! "<<*sit<<"\n";
EvQ.push(makeNetworkEvent(messageId, *sit, TTL, 0.0f)); //enqueue a network event with the "*sit" peer (the other parts are not used for now)
//holdIn( active, Time(0.01f));
}
}
// TEST : no external transition unless we're passive
if (this->state()==passive){
holdIn( active, Time(0,0,0,120)); //wait 120ms before doing something
}
return *this ;
}
/*******************************************************************
* Function Name: externalFunction
* Description: the Network gets input from outside
********************************************************************/
Model <SNetwork::externalFunction( const ExternalMessage &msg ){
//advance my internal time
if(VERBOSE) cout<<"External Transition :"<<endl<<"time: "<<msg.time().asMsecs()<<endl;
currenttimefloat = msg.time().asMsecs();
if (msg.port() == peer_online)
{
thegraph->online(msg.value()); //adds a node to the graph with the given value
if(VERBOSE) cout<<"node "<<msg.value()<<" inserted\n";
}
else if (msg.port() == peer_offline){
int inpeer = msg.value();
//get all the connected nodes a disconnect them, plus let them know they've been disconnected
set<int> connected = thegraph->getConnectedNodes(inpeer);
set<int>::iterator sit;
int count = 0;
for ( sit=connected.begin() ; sit != connected.end(); sit++ ){
count++;
thegraph->disconnect(inpeer, *sit); //disconnect them
EvQ.push(makeDisConnectEvent(inpeer, *sit,currenttimefloat+count)); // enqueue messages to be output with just 1 millisecond delay
//DisconnectionQueue.push(buildMessage(*sit, inpeer)); //enqueue a message saying peer "inpeer" disconnects from "*sit"
}
thegraph->offline(inpeer);
if(VERBOSE) cout<<"node "<<msg.value()<<" removed\n";
//holdIn( active, Time(0.00f));
}
else if (msg.port() == peer_connect){
int twonumbers, from, to;
twonumbers = msg.value();
from = getPeerId(twonumbers); //first and second field encoding of the peers
to = getMessageId(twonumbers);
if(VERBOSE) cout<<"connecting "<<from<<" to "<< to<<"\n";
if(thegraph->connect(from,to))
EvQ.push(makeConnectEvent(from, to,currenttimefloat+1)); // enqueue message to be output with just 1 millisecond delay
//ConnectionQueue.push(buildMessage(to, from, 1)); // enqueue a connection message : adding the TTL makes it different from a disconnect message, further down the road
//holdIn( active, Time(0.00f));
}
else if (msg.port() == peer_disconnect){
int twonumbers, from, to;
twonumbers = msg.value();
from = getPeerId(twonumbers); //first and second field encoding of the peers
to = getMessageId(twonumbers);
if(VERBOSE) cout<<"disconnecting "<<from<<" and "<< to<<"\n";
if(thegraph->disconnect(from, to))
EvQ.push(makeDisConnectEvent(from, to,currenttimefloat+1));//DisconnectionQueue.push(twonumbers); // enqueue the original message to be re-output as confirmation that connection took place
//holdIn( active, Time(0.00f));
}
else if (msg.port() == inroute){
//routing=true;
int inpeer, TTL, messageId;
inpeer = getPeerId(msg.value());
TTL= getTTL(msg.value());
messageId = getMessageId(msg.value());
if(VERBOSE) cout<<"about to route a message from"<<inpeer<<"\n";
//get all the connected nodes and enqueue the "arrival of the message" event for all these new nodes
//find the nodes connected to this one
set<int> connected = thegraph->getConnectedNodes(inpeer);
//if(VERBOSE) cout<<"loop for enqueuing nodes :"<<connected.size()<<" nodes to enqueue";
set<int>::iterator sit;
//if(VERBOSE) cout << "connected nodes contains:";
for ( sit=connected.begin() ; sit != connected.end(); sit++ ){
//generate a random network delay:
float delay = static_cast<float>(distribution().get());
if(VERBOSE) cout<<"enqueueing event for time = "<<currenttimefloat +delay*1000<<"\n"; //delay in milliseconds !
EvQ.push(makeNetworkEvent(messageId, *sit, TTL, currenttimefloat + (delay*1000))); //enqueue a network event with the "*sit" peer (the other parts are not used for now)
//holdIn( active, Time(0.01f));
}
}
if (EvQ.empty()){ // this transition didn't enqueue anything
if(VERBOSE) cout<<"LTS:Event queue is empty. Passivating."<<endl;
passivate();
} else {
//calculate time until next scheduled change :
float remainingdelay = EvQ.top().time - currenttimefloat; //EvQ.top() is the next NetworkEvent to be output
if(VERBOSE) cout<<"next change at (ms):"<<EvQ.top().time<<"time until next change (s):"<<remainingdelay/1000<<endl;
// hold until then
holdIn(active, Time(remainingdelay/1000)); // time remaining until next scheduled change
//then it will be time for an internal transition...
}
/*/ TEST : no external transition unless we're passive
if (this->state()==passive){
holdIn( active, Time(0,0,0,120)); //wait 120ms before doing something
}*/
return *this ;
}
void MqttBridge::setError(QString error){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),"data/mediaservice/error",error.toStdString().c_str(),0,true);
client->publish(*message);
}
void MqttBridge::setPlaybackStatus(QString playbackstatus){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaStateData,playbackstatus.toStdString().c_str(),0,true);
client->publish(*message);
}
void MqttBridge::setLoopStatus(QString loopstatus){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaRepeatData,loopstatus.toStdString().c_str(),0,true);
client->publish(*message);
}
/*******************************************************************
* Function Name: externalFunction
* Description: the Network gets input from outside
********************************************************************/
Model <SNetwork::externalFunction( const ExternalMessage &msg ){
//if(VERBOSE) cout<<"coucou\n";
// if ( this->state() == passive) // in fact all this should happen whatever the state ! TODO !!
// {
if (msg.port() == peer_online)
{
thegraph->online(msg.value()); //adds a node to the graph with the given value
if(VERBOSE) cout<<"node "<<msg.value()<<" inserted\n";
//holdIn( active, Time(0.00f));
}
else if (msg.port() == peer_offline){
thegraph->offline(msg.value());
if(VERBOSE) cout<<"node "<<msg.value()<<" removed\n";
//holdIn( active, Time(0.00f));
}
else if (msg.port() == peer_connect){
int twonumbers, from, to;
twonumbers = msg.value();
from = floor(twonumbers /1000);
to = twonumbers%1000;
if(VERBOSE) cout<<"connecting "<<from<<" to "<< to<<"\n";
thegraph->connect(from,to);
//holdIn( active, Time(0.00f));
}
else if (msg.port() == peer_disconnect){
int twonumbers, from, to;
twonumbers = msg.value();
from = floor(twonumbers /1000);
to = twonumbers%1000;
if(VERBOSE) cout<<"disconnecting "<<from<<" and "<< to<<"\n";
thegraph->disconnect(from, to);
//holdIn( active, Time(0.00f));
}
else if (msg.port() == inroute){
//routing=true;
int inpeer, TTL, messageId;
inpeer = getPeerId(msg.value());
TTL= getTTL(msg.value());
messageId = getMessageId(msg.value());
if(VERBOSE) cout<<"LTS --- about to route a message from "<<inpeer<<"\n";
//get all the connected nodes and enqueue the "arrival of the message" event for all these new nodes
//find the nodes connected to this one
cout<<"LTS --- about to get Connected peers : \n";
/*thegraph->getConnectedNodes(inpeer);
cout<<"getConnectedpeers (1) passed\n about to declare another intbag\n";*/
set<int> connected;
//cout<<"ok--Intbag Declared----------------\nabout to get connected peers again";
connected = thegraph->getConnectedNodes(inpeer);
//cout<<"ok--after connected peers----------------\n";
if(VERBOSE) cout<<"LTS --- loop for enqueuing nodes :"<<connected.size()<<" nodes to enqueue\n";
set<int>::iterator sit;
if(VERBOSE) cout << "LTS --- connected nodes contains:";
for ( sit=connected.begin() ; sit != connected.end(); sit++ ){
if(VERBOSE) cout<<*sit<<";";
EvQ.push(makeNetworkEvent(messageId, *sit, TTL, 0.0f)); //enqueue a network event with the "*sit" peer (the other parts are not used for now)
//holdIn( active, Time(0.01f));
}
if(VERBOSE) cout<<endl;
//}
if (!EvQ.empty()) { // if we were or now are in the process of routing messages
holdIn( active, Time(0.03f)); // we wait 0.03s to dequeue
} else {
holdIn( active, Time(0.00f)); // we just passivate immediately
}
return *this ;
}
}
void MqttBridge::setVolume(QString volume){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaVolumeData,volume.toStdString().c_str(),0,true);
client->publish(*message);
}
// This is the actual task that is run
static portTASK_FUNCTION( sensorTask, pvParameters )
{
uint8_t rxLen, status; // not using status, and rxLen is always maxReceiveBytesForSpecificNumOfSensorSamples (define in .h)
uint8_t buffer[vtI2CMLen]; // receive message
uint8_t recvMsgType; // message type of the message put on I2C queue by moveTask (defined in I2CtaskMsgtypes)
// Get the parameters
SensorAStruct *sensorT = (SensorAStruct *) pvParameters;
webServerTaskStruct *webServerData = sensorT->webServerData;
// Get the I2C device pointer
vtI2CStruct *i2cDevPtr = sensorT->i2cDev;
MoveTaskStruct *moveTPtr = sensorT->moveT;
uint8_t count = 0;
uint16_t frontBuf[numOfSensorToDebounce] = {0, 0, 0};
uint16_t sideTopBuf[numOfSensorToDebounce] = {0, 0, 0};
uint16_t sideBottomBuf[numOfSensorToDebounce] = {0, 0, 0};
int i = 0;
for( i = 0; i < numOfSensorToDebounce; i++ )
{
frontBuf[i] = 0;
sideTopBuf[i] = 0;
sideBottomBuf[i] = 0;
}
// Like all good tasks, this should never exit
for(;;)
{
// Wait for a message from an I2C operation
if (vtI2CDeQ(i2cDevPtr, vtI2CMLen,buffer,&rxLen,&recvMsgType,&status) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
// printf("buffer %x\n", buffer[0]);
//
// uint8_t index = 0;
// for (index = 0; index < rxLen; index++)
// {
// printf("buffer %x\n", buffer[index]);
// }
switch(recvMsgType)
{
case (sensor1):
{
break;
}
case (sensor2):
{
if( rxLen != 0 )
{
// make sure the check sum is ok
//printf("valid = %d\n", validateMessage(buffer, rxLen));
if( validateMessage(buffer, rxLen) )
{
switch( getMessageId(buffer, rxLen) ) // recognize the msgID
{
case RoverWaitingForCommand:
{
/*
* We need to disable the timer so that the arm doesn't
* retrieve multiple sensor values when the rover is stationary.
* It is important to enable the timer back again so that the
* Arm receives distance values the rover has travelled from the
* last poll.
*/
/*
* Retrieve the sensor distances
*/
uint16_t frontDistanceInches = getFrontSensorDistanceInInches(buffer, rxLen);
uint16_t topRightDistanceInches = getTopRightSensorDistanceInInches(buffer, rxLen);
uint16_t bottomRightDistanceInches = getBottomRightSensorDistanceInInches(buffer, rxLen);
uint8_t canFront = updateSensorBuf(frontBuf, frontDistanceInches);
uint8_t canSideT = updateSensorBuf(sideTopBuf, topRightDistanceInches);
uint8_t canSideB = updateSensorBuf(sideBottomBuf, bottomRightDistanceInches);
uint8_t rightFeet = getRightFeet(buffer, rxLen);
uint8_t rightInches = getRightInches(buffer, rxLen);
uint8_t leftFeet = getLeftFeet(buffer, rxLen);
uint8_t leftInches = getLeftInches(buffer, rxLen);
// printf("rightF = %d, rightI = %d, leftF = %d, leftI = %d\n", rightFeet, rightInches, leftFeet
// , leftInches);
if ( canFront != 1 || canSideT != 1 || canSideB != 1){
break;
}
sendWheelDistancesOnly(sensorT->mapT, leftFeet, leftInches, rightFeet, rightInches, 0);
printf("Front %d\n", frontDistanceInches);
printf("TR %d\n", topRightDistanceInches);
printf("BR %d\n", bottomRightDistanceInches);
SendWebServerSensorData(webServerData,frontDistanceInches,
topRightDistanceInches,bottomRightDistanceInches, 0);
performLogic(buffer , rxLen, moveTPtr, sensorT);
break;
}
case RoverBusy:
{
/*
* Simply pass the buffer received from the rover to the MAP Task.
*/
/*
* Since the rover has received the command, we want
* to enable the timer so that we poll rover to receive
* the distance in feet and inches the rover has moved.
*/
break;
};
case RoverHitStartFinshLine:
{
/*
* Simply pass the buffer received from the rover to the MAP Task.
*/
uint8_t cmd [] = {stop_cmd_ToRover};
uint8_t t [] = {0};
uint8_t deg [] = {0};
uint8_t dist [] = {0,0};
SendWebServerCommandBuf(webServerData, fillWebServerBuf(cmd[0], t[0], deg[0], dist[0], dist[1]), portMAX_DELAY);
//SendStopToRover(moveTPtr, portMAX_DELAY);
SendStFinNotification(sensorT->mapT, portMAX_DELAY);
performLogic(buffer , rxLen, moveTPtr, sensorT);
RUN++;
break;
};
case RoverReceiveMoveCommand:
{
/*
* Since the rover has received the command, we want
* to enable the timer so that we poll rover to receive
* the distance in feet and inches the rover has moved.
*/
break;
};
case MessageDrop:
{
// msg wasn't received in time
SendDroppedNotification(moveTPtr, 0);
break;
};
}
}
else
{
// notify the moveTask that a message was droped or went bad
SendDroppedNotification(moveTPtr, 0);
}
}
else
{
// message dropped
SendDroppedNotification(moveTPtr, 0);
}
break;
}
case (cmdSend):
{
break;
}
default:
{
//VT_HANDLE_FATAL_ERROR(0);
break;
}
}
}
}
void MqttBridge::setAlbumArt(QString art){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaAlbumArtData,art.toStdString().c_str(),0,true);
client->publish(*message);
}
void MqttBridge::setPosition(QString position){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaPositionData,position.toStdString().c_str());
client->publish(*message);
}
void MqttBridge::setLength(QString length){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaLengthData,length.toStdString().c_str(),0,true);
client->publish(*message);
}
void MqttBridge::setTitle(QString title){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaTitleData,title.toStdString().c_str(),0,true);
client->publish(*message);
}
void EthernetSensorBase::addTo(EthernetSensorContainer& map) {
map.insert(getMessageId(), sensor());
}
void MqttBridge::setShuffle(QString shuffle){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaMixData,shuffle.toStdString().c_str(),0,true);
client->publish(*message);
}
void MqttBridge::setTrackList(QString metadatatracklist){
QMQTT::Message *message = new QMQTT::Message(getMessageId(),mediaLibraryListData,metadatatracklist.toStdString().c_str(),0,true);
qDebug()<< metadatatracklist;
client->publish(*message);
}
/*******************************************************************
* Function Name: externalFunction
* Description: the router gets input from either the "outside" (a new messgae to route) or from the router (next step for routing)
********************************************************************/
Model &Gnutella::externalFunction( const ExternalMessage &msg ){
if ( this->state() == passive)
{
if (msg.port() == route_in) //new message to route
{
//expecting float values looking like 6,123 meaning route from peer 6 with msg id 123 (id<1000)
//get the peerid, message id, generate new TTL, then put in "to output" variable // not a list ! [id, TTL, peer]
if(VERBOSE) cout<<"Gnutella : new routing message : "<<msg.value()<<endl;
int peerid = getPeerId(msg.value());// get originating peer (from value of external msg)
if(VERBOSE)cout<<" peerid:"<<peerid<<endl;
int id = getMessageId(msg.value()); // get message id
if(VERBOSE)cout<<" message id:"<<id<<endl;
//create "seen" list. The message ids are float values (that way we can generate them using a random function)
// to create an empty list I use the default constructor, throught the shortcut of calling the [] operator.
routingTable[id]; //creates empty set mapped to id
routingTable[id].insert(peerid); // the new peer has now been visited (because the first thing will be to send himself the message !)
// we need to propagate this message to the DB and route it in the network (separate issues)
hitting = true;
routing = true;
//set the output values
// from right, digits 1-3 are id, digits 4-6 are peerid, digit 7 is TTL exampel value : 6005123 = TTL=6, peerid 5, id = 123
nextOutputDB = buildMessage(id, peerid);
nextOutputR =buildMessage(id, peerid, STANDARDTTL); //standardTTL is a constant defined in the h, should be a model parameter
//if(VERBOSE) cout<<"next output:"<<nextOutputR<<endl;
}
else if (msg.port() == in_n){
//expecting value = TTL * 100 + peerid + id, where id is the decimal part (<1)
if(VERBOSE) cout<<"Gnutella : message from the network routing loop... "<<msg.value()<<"\n";
long inval = msg.value();
//extract our 3 values using static functions from complexmessages.h
int OldTTL = getTTL(inval);
//if(VERBOSE)cout<<"old ttl:"<<OldTTL<<endl;
int peerid = getPeerId(inval);
//if(VERBOSE)cout<<" peerid:"<<peerid<<endl;
int id = getMessageId(inval);
//if(VERBOSE)cout<<" message id:"<<id<<endl;
//check for already visited peer : search for "peerid" in set mapped to id in routing table
set<long>::iterator finder = routingTable[id].find(peerid);
if(finder==routingTable[id].end()){ //it's NOT in there
//the unseen peer must get the message
if(VERBOSE)cout<<" peerid:"<<peerid<<"unseen by msg id ="<<id<< endl;
hitting = true;
nextOutputDB = buildMessage(id, peerid); // we don't put in a TTL
// the new peer has now been visited : we add him to the set of seen peers
routingTable[id].insert(peerid);
if(OldTTL>0){ // the unseen peer will also propagate the message because it still has some TTL
routing=true;
nextOutputR = buildMessage(id, peerid, OldTTL-1); //add the TTL for the routing msg
if(VERBOSE)cout<<" message will be re-cycled TTL >0 "<< endl;
}
else {
routing = false;
}
} else {
if(VERBOSE)cout<<" peerid:"<<peerid<<"already visited by msg id ="<<id<< endl;
hitting = false;
routing = false;
}
}
} //end if state is passive
else{
cout<<"error: message received while in active state"<<endl;
}
// we have an instantaneous change back to the passive state (will output the next output values where relevant)
holdIn( active, Time(0.01f));
return *this ;
}