static void hxtMessageGeneral ( int messageLevel,
const char* func,
const char* file,
const char* line,
const char* encodingIssueString,
const char* alternativeString,
const char *fmt, va_list args){
HXTMessage msg;
char str[4096];
if(fmt!=NULL){
int err = vsnprintf(str, sizeof(str), fmt, args);
if(err>=0)
msg.string = str;
else
msg.string = encodingIssueString;
}
else{
msg.string = alternativeString;
}
msg.func = func;
msg.file = file;
msg.line = line;
msg.level = messageLevel;
msg.threadId = omp_get_thread_num();
msg.numThreads = omp_get_num_threads();
// #pragma omp critical
{
msgCallback(&msg);
}
}
boolean MySensor::process() {
uint8_t pipe;
boolean available = RF24::available(&pipe);
if (!available || pipe>6)
return false;
uint8_t len = RF24::getDynamicPayloadSize();
RF24::read(&msg, len);
RF24::writeAckPayload(pipe,&pipe, 1 );
// Add string termination, good if we later would want to print it.
msg.data[mGetLength(msg)] = '\0';
debug(PSTR("read: %d-%d-%d s=%d,c=%d,t=%d,pt=%d,l=%d:%s\n"),
msg.sender, msg.last, msg.destination, msg.sensor, mGetCommand(msg), msg.type, mGetPayloadType(msg), mGetLength(msg), msg.getString(convBuf));
if(!(mGetVersion(msg) == PROTOCOL_VERSION)) {
debug(PSTR("version mismatch\n"));
return false;
}
uint8_t command = mGetCommand(msg);
uint8_t type = msg.type;
uint8_t sender = msg.sender;
uint8_t last = msg.last;
uint8_t destination = msg.destination;
if (repeaterMode && command == C_INTERNAL && type == I_FIND_PARENT) {
// Relaying nodes should always answer ping messages
// Wait a random delay of 0-2 seconds to minimize collision
// between ping ack messages from other relaying nodes
delay(millis() & 0x3ff);
sendWrite(sender, build(msg, nc.nodeId, sender, NODE_SENSOR_ID, C_INTERNAL, I_FIND_PARENT_RESPONSE, false).set(nc.distance), true);
return false;
} else if (destination == nc.nodeId) {
// Check if sender requests an ack back.
if (mGetRequestAck(msg)) {
// Copy message
ack = msg;
mSetRequestAck(ack,false); // Reply without ack flag (otherwise we would end up in an eternal loop)
mSetAck(ack,true);
ack.sender = nc.nodeId;
ack.destination = msg.sender;
sendRoute(ack);
}
// This message is addressed to this node
if (repeaterMode && last != nc.parentNodeId) {
// Message is from one of the child nodes. Add it to routing table.
addChildRoute(sender, last);
}
if (command == C_INTERNAL) {
if (type == I_FIND_PARENT_RESPONSE && !isGateway) {
// We've received a reply to a FIND_PARENT message. Check if the distance is
// shorter than we already have.
uint8_t distance = msg.getByte();
if (distance<nc.distance-1) {
// Found a neighbor closer to GW than previously found
nc.distance = distance + 1;
nc.parentNodeId = msg.sender;
eeprom_write_byte((uint8_t*)EEPROM_PARENT_NODE_ID_ADDRESS, nc.parentNodeId);
eeprom_write_byte((uint8_t*)EEPROM_DISTANCE_ADDRESS, nc.distance);
debug(PSTR("new parent=%d, d=%d\n"), nc.parentNodeId, nc.distance);
}
return false;
} else if (sender == GATEWAY_ADDRESS) {
bool isMetric;
if (type == I_REBOOT) {
wdt_enable(WDTO_15MS);
for (;;);
} else if (type == I_ID_RESPONSE) {
if (nc.nodeId == AUTO) {
nc.nodeId = msg.getByte();
// Write id to EEPROM
if (nc.nodeId == AUTO) {
// sensor net gateway will return max id if all sensor id are taken
debug(PSTR("full\n"));
while (1); // Wait here. Nothing else we can do...
} else {
RF24::openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(nc.nodeId));
eeprom_write_byte((uint8_t*)EEPROM_NODE_ID_ADDRESS, nc.nodeId);
}
debug(PSTR("id=%d\n"), nc.nodeId);
}
} else if (type == I_CONFIG) {
// Pick up configuration from controller (currently only metric/imperial)
// and store it in eeprom if changed
isMetric = msg.getByte() == 'M' ;
if (cc.isMetric != isMetric) {
cc.isMetric = isMetric;
eeprom_write_byte((uint8_t*)EEPROM_CONTROLLER_CONFIG_ADDRESS, isMetric);
//eeprom_write_block((const void*)&cc, (uint8_t*)EEPROM_CONTROLLER_CONFIG_ADDRESS, sizeof(ControllerConfig));
}
} else if (type == I_CHILDREN) {
if (repeaterMode && msg.getByte() == 'C') {
// Clears child relay data for this node
debug(PSTR("rd=clear\n"));
for (uint8_t i=0;i< sizeof(childNodeTable); i++) {
removeChildRoute(i);
}
sendRoute(build(msg, nc.nodeId, GATEWAY_ADDRESS, NODE_SENSOR_ID, C_INTERNAL, I_CHILDREN,false).set(""));
}
} else if (type == I_TIME) {
if (timeCallback != NULL) {
// Deliver time to callback
timeCallback(msg.getULong());
}
}
return false;
}
}
// Call incoming message callback if available
if (msgCallback != NULL) {
msgCallback(msg);
}
// Return true if message was addressed for this node...
return true;
} else if (repeaterMode && pipe == CURRENT_NODE_PIPE) {
// We should try to relay this message to another node
uint8_t route = getChildRoute(msg.destination);
if (route>0 && route<255) {
// This message should be forwarded to a child node. If we send message
// to this nodes pipe then all children will receive it because the are
// all listening to this nodes pipe.
//
// +----B
// -A
// +----C------D
//
// We're node C, Message comes from A and has destination D
//
// lookup route in table and send message there
sendWrite(route, msg);
} else {
// A message comes from a child node and we have no
// route for it.
//
// +----B
// -A
// +----C------D <-- Message comes from D
//
// We're node C
//
// Message should be passed to node A (this nodes relay)
// This message should be routed back towards sensor net gateway
sendWrite(nc.parentNodeId, msg);
// Add this child to our "routing table" if it not already exist
addChildRoute(sender, last);
}
}
return false;
}
void TUIOMsgListener::ProcessMessage(const osc::ReceivedMessage &m, const IpEndpointName &remoteEndpoint) {
try {
const char *addr = m.AddressPattern();
std::cout << "TUIOMsgListener: Received message "
<< addr << std::endl;
const int prefixLen = strlen(tuioAddressPrefix);
if (strncmp(addr, tuioAddressPrefix, prefixLen) == 0 && strlen(addr) > prefixLen + 1) {
TUIOMsg *msg = new TUIOMsg;
const char *type = (addr + prefixLen + 1);
osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin();
const unsigned long numArgs = m.ArgumentCount();
if (strcmp(type, "2Dobj") == 0) {
msg->type = kTUIOMsgTypeObj;
} else if (strcmp(type, "2Dcur") == 0) {
msg->type = kTUIOMsgTypeCur;
} else if (strcmp(type, "2Dblb") == 0) {
msg->type = kTUIOMsgTypeBlb;
} else {
std::cerr << "TUIOMsgListener: unknown TUIO message type "
<< type << std::endl;
return;
}
const char *cmd = (arg++)->AsString();
if (strcmp(cmd, "source") == 0) {
msg->cmd = kTUIOMsgCmdSrc;
} else if (strcmp(cmd, "alive") == 0) {
msg->cmd = kTUIOMsgCmdAlive;
} else if (strcmp(cmd, "set") == 0) {
msg->cmd = kTUIOMsgCmdSet;
} else if (strcmp(cmd, "fseq") == 0) {
msg->cmd = kTUIOMsgCmdFSeq;
} else {
std::cerr << "TUIOMsgListener: unknown TUIO message command "
<< cmd << std::endl;
return;
}
switch (msg->cmd) {
case kTUIOMsgCmdSrc: {
const char *srcAddr = (arg++)->AsString();
const int addrLen = strlen(srcAddr + 1);
msg->data.source.addr = new char[addrLen];
strncpy(msg->data.source.addr, srcAddr, addrLen);
}
break;
case kTUIOMsgCmdAlive: {
msg->data.alive.numSessIds = numArgs - 1;
if (msg->data.alive.numSessIds > 0) {
msg->data.alive.sessIds = new int[msg->data.alive.numSessIds];
for (int i = 0; i < msg->data.alive.numSessIds; i++) {
msg->data.alive.sessIds[i] = (arg++)->AsInt32();
}
} else {
msg->data.alive.sessIds = NULL;
}
}
break;
case kTUIOMsgCmdSet: {
msg->data.set.sessId = (arg++)->AsInt32();
switch (msg->type) {
case kTUIOMsgTypeObj:
msg->data.set.classId = (arg++)->AsInt32();
msg->data.set.pos.x = (arg++)->AsFloat();
msg->data.set.pos.y = (arg++)->AsFloat();
msg->data.set.angle = (arg++)->AsFloat();
msg->data.set.size.x = 0.0f;
msg->data.set.size.y = 0.0f;
msg->data.set.area = 0.0f;
msg->data.set.vel.x = (arg++)->AsFloat();
msg->data.set.vel.y = (arg++)->AsFloat();
msg->data.set.angleVel = (arg++)->AsFloat();
msg->data.set.motAccel = (arg++)->AsFloat();
msg->data.set.rotAccel = (arg++)->AsFloat();
break;
case kTUIOMsgTypeCur:
msg->data.set.classId = 0;
msg->data.set.pos.x = (arg++)->AsFloat();
msg->data.set.pos.y = (arg++)->AsFloat();
msg->data.set.angle = 0.0f;
msg->data.set.size.x = 0.0f;
msg->data.set.size.y = 0.0f;
msg->data.set.area = 0.0f;
msg->data.set.vel.x = (arg++)->AsFloat();
msg->data.set.vel.y = (arg++)->AsFloat();
msg->data.set.angleVel = 0.0f;
msg->data.set.motAccel = (arg++)->AsFloat();
msg->data.set.rotAccel = 0.0f;
break;
case kTUIOMsgTypeBlb:
msg->data.set.classId = 0;
msg->data.set.pos.x = (arg++)->AsFloat();
msg->data.set.pos.y = (arg++)->AsFloat();
msg->data.set.angle = (arg++)->AsFloat();
msg->data.set.size.x = (arg++)->AsFloat();
msg->data.set.size.y = (arg++)->AsFloat();
msg->data.set.area = 0.0f;
msg->data.set.vel.x = (arg++)->AsFloat();
msg->data.set.vel.y = (arg++)->AsFloat();
msg->data.set.angleVel = (arg++)->AsFloat();
msg->data.set.motAccel = (arg++)->AsFloat();
msg->data.set.rotAccel = (arg++)->AsFloat();
break;
}
}
break;
case kTUIOMsgCmdFSeq: {
msg->data.fseq.frameId = (arg++)->AsInt32();
}
break;
}
if (msgCallback) {
msgCallback(msg);
}
} else {
std::cerr << "TUIOMsgListener: Address prefix " << addr << " did not match to "
<< tuioAddressPrefix << std::endl;
}
} catch( osc::Exception& e ){
// any parsing errors such as unexpected argument types, or
// missing arguments get thrown as exceptions.
std::cerr << "TUIOMsgListener: error while parsing message "
<< m.AddressPattern() << ": " << e.what() << "\n";
}
}
