int main() {
DBHandler db("/home/sailbot/sailingrobot/asr.db");
/*"Mock" system state model*/
SystemStateModel systemStateModel = SystemStateModel(
GPSModel("",PositionModel(0,0),0,0,0,0),
WindsensorModel(0,0,0),
CompassModel(0,0,0,AccelerationModel(0,0,0) ),
AnalogArduinoModel(0, 0, 0, 0),
0,
0
);
SystemState systemState(systemStateModel);
ExternalCommand externalCommand("<timestamp>", false, 0, 0);
try {
xBeeSync sync(&externalCommand, &systemState, &db, false, true, true, 0.2);
/*Insert mock data into db*/
db.insertDataLog(systemStateModel,1,1,1.0,1.0,1.0,true,true,1,1.0,0);
sync.run();
} catch(const char* error) {
std::cout << error << std::endl;
}
/*Uncomment the run section to test integeration*/
/*This test aims to push one mock datalog up to the server*/
}
static inline std::auto_ptr<SystemState>
createSystemFromConfig(const char *filename, const XESector *configSector)
{
uint64_t length = configSector->getLength();
const scoped_array<char> buf(new char[length + 1]);
if (!configSector->getData(buf.get())) {
std::cerr << "Error reading config from \"" << filename << "\"" << std::endl;
std::exit(1);
}
if (length < 8) {
std::cerr << "Error unexpected config config sector length" << std::endl;
std::exit(1);
}
length -= 8;
buf[length] = '\0';
/*
* this initialize the library and check potential ABI mismatches
* between the version it was compiled for and the actual shared
* library used.
*/
LIBXML_TEST_VERSION
xmlDoc *doc = xmlReadDoc((xmlChar*)buf.get(), "config.xml", NULL, 0);
xmlRelaxNGParserCtxtPtr schemaContext =
xmlRelaxNGNewMemParserCtxt(configSchema, sizeof(configSchema));
xmlRelaxNGPtr schema = xmlRelaxNGParse(schemaContext);
xmlRelaxNGValidCtxtPtr validationContext =
xmlRelaxNGNewValidCtxt(schema);
if (xmlRelaxNGValidateDoc(validationContext, doc) != 0) {
std::exit(1);
}
xmlNode *root = xmlDocGetRootElement(doc);
xmlNode *system = findChild(root, "System");
xmlNode *nodes = findChild(system, "Nodes");
std::auto_ptr<SystemState> systemState(new SystemState);
std::map<long,Node*> nodeNumberMap;
for (xmlNode *child = nodes->children; child; child = child->next) {
if (child->type != XML_ELEMENT_NODE ||
strcmp("Node", (char*)child->name) != 0)
continue;
systemState->addNode(createNodeFromConfig(child, nodeNumberMap));
}
xmlNode *connections = findChild(system, "Connections");
for (xmlNode *child = connections->children; child; child = child->next) {
if (child->type != XML_ELEMENT_NODE ||
strcmp("SLink", (char*)child->name) != 0)
continue;
long nodeID1, link1, nodeID2, link2;
if (!parseXLinkEnd(findAttribute(child, "end1"), nodeID1, link1)) {
std::cerr << "Failed to parse \"end1\" attribute" << std::endl;
std::exit(1);
}
if (!parseXLinkEnd(findAttribute(child, "end2"), nodeID2, link2)) {
std::cerr << "Failed to parse \"end2\" attribute" << std::endl;
std::exit(1);
}
Node *node1 = lookupNodeChecked(nodeNumberMap, nodeID1);
if (link1 >= node1->getNumXLinks()) {
std::cerr << "Invalid sLink number " << link1 << std::endl;
std::exit(1);
}
Node *node2 = lookupNodeChecked(nodeNumberMap, nodeID2);
if (link2 >= node2->getNumXLinks()) {
std::cerr << "Invalid sLink number " << link2 << std::endl;
std::exit(1);
}
node1->connectXLink(link1, node2, link2);
node2->connectXLink(link2, node1, link1);
}
xmlNode *jtag = findChild(system, "JtagChain");
unsigned jtagIndex = 0;
for (xmlNode *child = jtag->children; child; child = child->next) {
if (child->type != XML_ELEMENT_NODE ||
strcmp("Node", (char*)child->name) != 0)
continue;
long nodeID = readNumberAttribute(child, "id");
lookupNodeChecked(nodeNumberMap, nodeID)->setJtagIndex(jtagIndex++);
}
systemState->finalize();
xmlFreeDoc(doc);
xmlCleanupParser();
return systemState;
}
/** \brief Loop for state polling in modes script and polling. Also sends command in script mode. */
void timer_cb(const ros::TimerEvent& ev)
{
// ==== Get state values by built-in commands ====
gripper_response info;
float acc = 0.0;
info.speed = 0.0;
if (g_mode_polling) {
const char * state = systemState();
if (!state)
return;
info.state_text = std::string(state);
info.position = getOpening();
acc = getAcceleration();
info.f_motor = getForce();//getGraspingForce();
} else if (g_mode_script) {
// ==== Call custom measure-and-move command ====
int res = 0;
if (!std::isnan(g_goal_position)) {
ROS_INFO("Position command: pos=%5.1f, speed=%5.1f", g_goal_position, g_speed);
res = script_measure_move(1, g_goal_position, g_speed, info);
} else if (!std::isnan(g_goal_speed)) {
ROS_INFO("Velocity command: speed=%5.1f", g_goal_speed);
res = script_measure_move(2, 0, g_goal_speed, info);
} else
res = script_measure_move(0, 0, 0, info);
if (!std::isnan(g_goal_position))
g_goal_position = NAN;
if (!std::isnan(g_goal_speed))
g_goal_speed = NAN;
if (!res) {
ROS_ERROR("Measure-and-move command failed");
return;
}
// ==== Moving msg ====
if (g_ismoving != info.ismoving) {
std_msgs::Bool moving_msg;
moving_msg.data = info.ismoving;
g_pub_moving.publish(moving_msg);
g_ismoving = info.ismoving;
}
} else
return;
// ==== Status msg ====
wsg_50_common::Status status_msg;
status_msg.status = info.state_text;
status_msg.width = info.position;
status_msg.speed = info.speed;
status_msg.acc = acc;
status_msg.force = info.f_motor;
status_msg.force_finger0 = info.f_finger0;
status_msg.force_finger1 = info.f_finger1;
g_pub_state.publish(status_msg);
// ==== Joint state msg ====
// \todo Use name of node for joint names
sensor_msgs::JointState joint_states;
joint_states.header.stamp = ros::Time::now();;
joint_states.header.frame_id = "summit_xl_wsg50_base_link";
joint_states.name.push_back("summit_xl_wsg50_finger_left_joint");
joint_states.name.push_back("summit_xl_wsg50_finger_right_joint");
joint_states.position.resize(2);
joint_states.position[0] = -info.position/2000.0;
joint_states.position[1] = info.position/2000.0;
joint_states.velocity.resize(2);
joint_states.velocity[0] = info.speed/1000.0;
joint_states.velocity[1] = info.speed/1000.0;
joint_states.effort.resize(2);
joint_states.effort[0] = info.f_motor;
joint_states.effort[1] = info.f_motor;
g_pub_joint.publish(joint_states);
// printf("Timer, last duration: %6.1f\n", ev.profile.last_duration.toSec() * 1000.0);
}
int main( int argc, char **argv )
{
ros::init(argc, argv, "wsg_50");
ros::NodeHandle nh;
std::string ip;
int port;
ROS_INFO("WSG 50 - ROS NODE");
nh.param("wsg_50_tcp/ip", ip, std::string("192.168.1.20"));
nh.param("wsg_50_tcp/port", port, 1000);
// Connect to device using TCP
if( cmd_connect_tcp( ip.c_str(), port ) == 0 )
{
ROS_INFO("TCP connection stablished");
// Services
ros::ServiceServer moveSS = nh.advertiseService("wsg_50/move", moveSrv);
ros::ServiceServer graspSS = nh.advertiseService("wsg_50/grasp", graspSrv);
ros::ServiceServer releaseSS = nh.advertiseService("wsg_50/release", releaseSrv);
ros::ServiceServer homingSS = nh.advertiseService("wsg_50/homing", homingSrv);
ros::ServiceServer stopSS = nh.advertiseService("wsg_50/stop", stopSrv);
ros::ServiceServer ackSS = nh.advertiseService("wsg_50/ack", ackSrv);
ros::ServiceServer incrementSS = nh.advertiseService("wsg_50/move_incrementally", incrementSrv);
ros::ServiceServer setAccSS = nh.advertiseService("wsg_50/set_acceleration", setAccSrv);
ros::ServiceServer setForceSS = nh.advertiseService("wsg_50/set_force", setForceSrv);
// Publisher
ros::Publisher state_pub = nh.advertise<wsg_50_common::Status>("wsg_50/status", 1000);
ros::Publisher joint_states_pub = nh.advertise<sensor_msgs::JointState>("/joint_states", 10);
ROS_INFO("Ready to use.");
homing();
ros::Rate loop_rate(1); // loop at 1Hz
while( ros::ok() ){
//Loop waiting for orders and updating the state
//Create the msg to send
wsg_50_common::Status status_msg;
//Get state values
const char * aux;
aux = systemState();
int op = getOpening();
int acc = getAcceleration();
int force = getGraspingForceLimit();
std::stringstream ss;
ss << aux;
status_msg.status = ss.str();
status_msg.width = op;
status_msg.acc = acc;
status_msg.force = force;
state_pub.publish(status_msg);
sensor_msgs::JointState joint_states;
joint_states.header.stamp = ros::Time::now();;
joint_states.header.frame_id = "wsg_50_gripper_base_link";
joint_states.name.push_back("wsg_50_gripper_base_joint_gripper_left");
joint_states.name.push_back("wsg_50_gripper_base_joint_gripper_right");
joint_states.position.resize(2);
joint_states.position[0] = -op/2000.0;
joint_states.position[1] = op/2000.0;
//joint_states.velocity.resize(2);
//joint_states.velocity[0];
//joint_states.velocity[1];
joint_states.effort.resize(2);
joint_states.effort[0] = force;
joint_states.effort[1] = force;
joint_states_pub.publish(joint_states);
loop_rate.sleep();
ros::spinOnce();
}
}else{
ROS_ERROR("Unable to connect via TCP, please check the port and address used.");
}
// Disconnect - won't be executed atm. as the endless loop in test()
// will never return.
cmd_disconnect();
return 0;
}
static size_t parse_message(msg_t msg)
{
struct msginfo minfo;
struct LogEntry entry;
struct NodeCfg ncfg;
struct ZoneCfg zcfg;
char str[64];
uint16_t i;
size_t msg_size = 0;
int err;
// last_msg_pos = msg_table_get_next_idx(last_msg_pos);
if(msg_info(msg, &minfo) != ENOERR)
{
//memcpy(&message_table[last_msg_pos], "Error retrieving msg info", 26);
//memcopy(&message_table[last_msg_pos], "Error retrieving msg info", 26);
//return;
memcopy(str, "Error retrieving msg info", 26);
goto finish_parse_msg;
}
str[0] = 0;
switch(minfo.msg_type)
{
case 0x02:
{
unsigned int thresholds;
int zone_idx, node_idx;
unsigned char attr_num;
unsigned char addMsg = 0;
msg_size = deserialize(minfo.body_ptr, MSG_THRESHOLDS_SIZE, "8:1:2:", &entry.timestamp, &attr_num, &thresholds);
msg_size = MSG_THRESHOLDS_SIZE + 3;
entry.entry_type = 0;
if((node_idx = strgFindNode(minfo.src_ext_addr, &ncfg)) < 0)
{
strcpy(str, FSTR_NODE_NOCFG); // "Node is not configured.");
break;
}
if((zone_idx = strgFindZone(ncfg.zone, &zcfg)) < 0)
{
strcpy(str, FSTR_NODE_NOZONE); //"Node doesn't belong to any zone.");
break;
}
if(zoneState(zone_idx, ZONE_STATE_ON) != ZONE_STATE_ON)
break;
//strcpy(str, "Zone: ");
//strgGetZoneName(zone_idx, str + sizeof("Zone: ") - 1, 64 - sizeof("Zone: ") - 1);
//itoa(zcfg.zone, str + sizeof("Zone: ") - 1, 10);
/*
itoa(zcfg.zone, str, 10);
strcat(str, ", ");
itoa(ncfg.node_num, str + strlen(str), 10);
strcat(str, ", ");
*/
format_str(str, 64, FSTR_ZONE_NODE_FMT, zcfg.zone, ncfg.node_num, minfo.src_ext_addr);
//format_str(str, 64, "Z%u, N%u ", zcfg.zone, ncfg.node_num, minfo.src_ext_addr);
/*
itoa(attr_num, str, 16);
strcat(str, ", ");
itoa(thresholds, str + strlen(str), 16);
strcat(str, ", ");
*/
i = strlen(str);
//if(attr_num == 0xe2)//0x26) // Thresholds for measured value
if(attr_num == 0x26) // Thresholds for measured value
{
//setNodeState(node_idx, thresholds, NODE_MEASUREMENT);
if(thresholds & (NODE_NO_FIRE_DANGER_DAY | NODE_NO_FIRE_DANGER_NIGHT))
{
setNodeState(node_idx, ~NODE_STATE_PREFIRE, NODE_STATE_PREFIRE);
strcpy(str + i, FSTR_NODANGER); //"No danger." );
}
if(thresholds & (NODE_NO_FIRE_DAY | NODE_NO_FIRE_NIGHT))
{
setNodeState(node_idx, ~NODE_STATE_FIRE, NODE_STATE_FIRE);
strcpy(str + i, FSTR_NOFIRE); //"No fire.");
}
if(thresholds & (NODE_FIRE_DANGER_DAY | NODE_FIRE_DANGER_NIGHT))
{
setNodeState(node_idx, NODE_STATE_PREFIRE, NODE_STATE_PREFIRE);
strcpy(str + i, FSTR_FIREDANGER); //"Fire danger!");
if(!systemState(SYS_STATE_MASK))
addMsg = 1;
}
if(thresholds & (NODE_FIRE_DAY | NODE_FIRE_NIGHT))
{
setNodeState(node_idx, NODE_STATE_FIRE, NODE_STATE_FIRE);
strcpy(str + i, FSTR_FIRE); //"Fire!");
zoneSet(zone_idx, SYS_STATE_FIRE, SYS_STATE_FIRE);
addMsg = 1;
}
compileZoneState(zone_idx, zcfg.zone);
/*
if(thresholds & NODE_FIRE_DANGER_NIGHT)
{
setNodeState(node_idx, NODE_STATE_PREFIRE, NODE_STATE_PREFIRE);
strcpy(str + i, "Fire danger! (night)");
if(!systemState(SYS_STATE_MASK))
addMsg = 1;
}
if(thresholds & NODE_NO_FIRE_DANGER_NIGHT)
{
setNodeState(node_idx, ~NODE_STATE_PREFIRE, NODE_STATE_PREFIRE);
strcpy(str + i, "No danger. (night)" );
}
if(thresholds & NODE_FIRE_NIGHT)
{
setNodeState(node_idx, NODE_STATE_FIRE, NODE_STATE_FIRE);
strcpy(str + i, "Fire! (night)");
zoneSet(zone_idx, SYS_STATE_FIRE, SYS_STATE_FIRE);
addMsg = 1;
}
if(thresholds & NODE_NO_FIRE_NIGHT)
{
setNodeState(node_idx, ~NODE_STATE_FIRE, NODE_STATE_FIRE);
strcpy(str + i, "No fire. (night)");
}
*/
}
//else if(attr_num == 0xe3)//0x4c)
else if(attr_num == 0x4c)
{
if(thresholds & NODE_NO_PRECLEANING)
strcpy(str + i, FSTR_NOPRECLEAN); //"Sensor doesn't require cleaning");
if(thresholds & NODE_PRECLEANING)
strcpy(str + i, FSTR_PRECLEAN); //"Sensor will require cleaning soon.");
if(thresholds & NODE_OK)
strcpy(str + i, FSTR_SENSOROK); //"Sensor OK");
if(thresholds & NODE_PREFAILURE)
strcpy(str + i, FSTR_PREFAILURE); //"Sensor may be inoperational");
if(thresholds & NODE_CLEANING)
{
strcpy(str + i, FSTR_CLEAN_SENSOR); //"Clean the sensor!");
zoneSet(zone_idx, SYS_STATE_FAILURE, SYS_STATE_FAILURE);
if(!systemState(SYS_STATE_FIRE | SYS_STATE_ALARM))
addMsg = 1;
}
if(thresholds & NODE_FAILURE)
{
strcpy(str + i, FSTR_FAILURE); //"Sensor failure!");
zoneSet(zone_idx, SYS_STATE_FAILURE, SYS_STATE_FAILURE);
if(!systemState(SYS_STATE_FIRE | SYS_STATE_ALARM))
addMsg = 1;
}
if(!thresholds)
{
strcpy(str + i, FSTR_NOTHRESHOLDS); // "No thresholds");
//addMsg = 1;
}
}
else if(attr_num == 0xe2)
{
//if(minfo.src_ext_addr == 0x67ULL) // patch for testing; something has to be done to distiguish between different profile attributes
// First attempt to distiguish between different profiles
if((minfo.src_ext_addr >= GERK_FIRST_ADDR) && (minfo.src_ext_addr <= GERK_FIRST_ADDR + GERK_SENSOR_NUM))
{
if(!(thresholds & 0x01))
{
strcpy(str + i, FSTR_ALARM);
setNodeState(node_idx, NODE_STATE_ALARM, NODE_STATE_ALARM);
zoneSet(zone_idx, SYS_STATE_ALARM, SYS_STATE_ALARM);
if(!systemState(SYS_STATE_FIRE))
addMsg = 1;
}
else if(thresholds & 0x01)
{
strcpy(str + i, FSTR_NOALARM);
setNodeState(node_idx, ~NODE_STATE_ALARM, NODE_STATE_ALARM);
compileZoneState(zone_idx, zcfg.zone);
}
}
}
else
{
//strcpy(str, "Attr num: ");
//itoa(attr_num, str + strlen(str), 16);
format_str(str, 64, "Attr num: 0x%x, val: 0x%x", attr_num, thresholds);
//addMsg = 1;
}
compileSysState();
entry.msg_len = strlen(str);
if(addMsg)
addEmergencyMsg(&entry, str);
}
break;
#ifdef DEMO_MESSAGES
case 0x41:
case 0x42:
case 0x43:
#define APP_MSGS
#ifdef APP_MSGS
{
uint16_t val;
uint64_t node_extaddr = 0;
uint16_t zone_num = 0xffff;
uint16_t node_num = 0;
msg_size = 2;
val = ((uint8_t *)minfo.body_ptr)[1];
val <<= 8;
val |= ((uint8_t *)minfo.body_ptr)[0];
for(i = 0; i < MAX_CHILDREN; i++)
{
if(children[i].net_addr == minfo.src_addr)
{
node_extaddr = children[i].ext_addr;
break;
}
}
if(node_extaddr && (strgFindNode(node_extaddr, &ncfg) >= 0))
{
zone_num = ncfg.zone;
node_num = ncfg.node_num;
//OLED_puts(0, 6*9, 0xff, font6x9, "Node found");
/*
for(i = 0; i < sizeof(zones)/sizeof(struct node_cfg); i++)
{
if(strgGetNode(i, &ncfg) != STRG_SUCCESS)
break;
if(ncfg.ext_addr == node_extaddr)
{
// OLED_puts(11 * 6, 6*9, 0xff, font6x9, "Zone found");
zone_num = ncfg.zone;
node_num = ncfg.node_num;
break;
}
}
*/
}
/*
else
OLED_puts(0, 6*9, 0xff, font6x9, "Node not found");
*/
if(minfo.msg_type == 0x41)
{
//sprintf(str, /*"Fire (%u), "*/ MSG_STR_FIRE, node_num, val);
strcpy(str, "Fire n");
itoa(node_num, str + strlen(str), 10);
strcat(str, " (");
itoa(val, str + strlen(str), 10);
strcat(str, ") ");
}
else if(minfo.msg_type == 0x42)
{
//sprintf(str, /*"Attention (%u), "*/MSG_STR_ALARM, node_num, val);
strcpy(str, "Attention n");
itoa(node_num, str + strlen(str), 10);
strcat(str, " (");
itoa(val, str + strlen(str), 10);
strcat(str, ") ");
}
else
{
//sprintf(str, MSG_STR_BATT, node_num, val);
strcpy(str, "Batt.");
itoa(node_num, str + strlen(str), 10);
strcat(str, " ");
itoa(val, str + strlen(str), 10);
strcat(str, " mV ");
}
//sprintf(str + strlen(str), (zone_num < 0xffff) ? zone_names[zone_num] : /*"Unknown zone"*/ ZONE_NAME_UNKNOWN);
//strcat(str, (zone_num < 0xffff) ? zone_names[zone_num] : /*"Unknown zone"*/ ZONE_NAME_UNKNOWN);
if((i = strgFindZone(zone_num, &zcfg)) >= 0)
strgGetZoneName(i, str + strlen(str), 64 - strlen(str));
else
strcat(str, ZONE_NAME_UNKNOWN);
}
#else // APP_MSGS
{
struct LogEntry entry;
uint16_t val;
uint64_t node_extaddr = 0;
uint16_t zone_num = 0xffff;
uint16_t node_num = 0;
/*
entry.timestamp = 0;
entry.entry_type = 1;
*/
entry.msg_len = sizeof(struct msginfo) + 2 + 8; // Msg Info + Body + Ext. address
msg_size = 2;
val = ((uint8_t *)minfo.body_ptr)[1];
val <<= 8;
val |= ((uint8_t *)minfo.body_ptr)[0];
for(i = 0; i < MAX_CHILDREN; i++)
{
if(children[i].net_addr == minfo.src_addr)
{
node_extaddr = children[i].ext_addr;
break;
}
}
logAddNwkMsgEntry(&entry, &minfo, node_extaddr);
}
#endif // APP_MSGS
break;
#endif // DEMO_MESSAGES
case MSG_EVENT:
{
net_addr_t node_addr;
//uint64_t timestamp;
uint64_t node_extaddr;
unsigned char type;
// Both messages (JOIN and LEAVE are supposed to have same overall size and field sizes and sequence (though they may
// have different meaning). If it's not true anymore or another event with
// different body size is supported correct the next line as required
msg_size = deserialize(minfo.body_ptr, MSG_JOIN_MSG_SIZE, "8:1:2:8:", &entry.timestamp, &type, &node_addr, &node_extaddr);
//if(((uint8_t *)minfo.body_ptr)[8] == MSG_JOIN)
if(type == MSG_JOIN)
{
/*
msg_size = MSG_JOIN_MSG_SIZE;
node_addr = ((uint8_t *)minfo.body_ptr)[10];
node_addr <<= 8;
node_addr |= ((uint8_t *)minfo.body_ptr)[9];
//sprintf(str, MSG_STR_ADD_NODE, node_addr);
//strcpy(str, "+Node(0x");
//itoa(node_addr, str + strlen(str), 16);
//strcat(str, ", 0x");
for(i = 10 + 8; i > 10; i--)
{
//sprintf(str + strlen(str), "%02X", (uint16_t)(((uint8_t *)minfo.body_ptr)[i]));
//itoa((uint16_t)(((uint8_t *)minfo.body_ptr)[i]), str + strlen(str), 16);
node_extaddr <<= 8;
node_extaddr |= ((uint8_t *)minfo.body_ptr)[i];
}
*/
//sprintf(str + strlen(str), ")");
//strcat(str, ")");
//format_str(str, 64, "Join: 0x%llx 0x%04x", node_extaddr, node_addr);
//OLED_puts(0, 50, 0xff, font6x9, str);
if((err = strgFindNode(node_extaddr, &ncfg)) >= 0)
{
//format_str(str, 64, "+Node(0x%x, 0x%llx) (N%u)", node_addr, node_extaddr, ncfg.node_num);
format_str(str, 64, FSTR_NODE_ADD_FMT, node_addr, node_extaddr, ncfg.node_num);
if(err < MAX_CHILDREN)
{
children[err].net_addr = node_addr;
children[err].ext_addr = node_extaddr;
children[err].node_state = 0;
children[err].zone_num = ncfg.zone;
children[err].node_num = ncfg.node_num;
}
else
format_str(str, 64, FSTR_NODE_ATCAP_FMT, node_addr, node_extaddr);
//format_str(str, 64, "Fail: +Node(0x%x, 0x%llx) - at capacity", node_addr, node_extaddr);
//i = strlen(str);
//format_str(str + i, sizeof(str) - i, " (n%u)", ncfg.node_num);
//strcat(str, " (n");
//itoa(ncfg.node_num, str + strlen(str), 10);
//strcat(str, ")");
}
else
format_str(str, 64, FSTR_NODE_NOTFOUND_FMT, node_addr, node_extaddr);
update_disp_nwk();
update_disp_zones_cfg();
//format_str(str, 64, "Fail: +Node(0x%x, 0x%llx) - not found", node_addr, node_extaddr);
/*
for(i = 0; i < MAX_CHILDREN; i++)
{
if((children[i].net_addr == BAD_ADDRESS) || (children[i].ext_addr == node_extaddr))
{
children[i].net_addr = node_addr;
children[i].ext_addr = node_extaddr;
break;
}
}
if(i == MAX_CHILDREN)
{
//sprintf(str, "+Node failed: at capacity");
strcpy(str, "+Node failed: at capacity");
}
else
{
if(strgFindNode(node_extaddr, &ncfg) >= 0)
{
//sprintf(str + strlen(str), " (n%u)", zones[i].node_num);
strcat(str, " (n");
itoa(ncfg.node_num, str + strlen(str), 10);
strcat(str, ")");
}
}
*/
//OLED_puts(0, 50, 0xff, font6x9, str); // for debugging
}
else if(type == MSG_LEAVE)
{
/*
net_addr_t node_addr;
uint64_t node_extaddr = 0;
msg_size = MSG_LEAVE_MSG_SIZE;
node_addr = ((uint8_t *)minfo.body_ptr)[10];
node_addr <<= 8;
node_addr |= ((uint8_t *)minfo.body_ptr)[9];
//sprintf(str, MSG_STR_DEL_NODE, node_addr);
//strcpy(str, "-Node(0x");
// Now the reason of disconnection is placed instead of short address
// So we'll not print the short address at all
// itoa(node_addr, str + strlen(str), 16);
// strcat(str, ", 0x");
for(i = 10 + 8; i > 10; i--)
{
//sprintf(str + strlen(str), "%02X", (uint16_t)(((uint8_t *)minfo.body_ptr)[i]));
//itoa((uint16_t)(((uint8_t *)minfo.body_ptr)[i]), str + strlen(str), 16);
node_extaddr <<= 8;
node_extaddr |= ((uint8_t *)minfo.body_ptr)[i];
}
*/
if((err = strgFindNode(node_extaddr, &ncfg)) >= 0)
{
if(err < MAX_CHILDREN)
{
//format_str(str, 64, "-Node(0x%x, 0x%llx) (N%u)", children[err].net_addr, node_extaddr, ncfg.node_num);
format_str(str, 64,FSTR_NODE_DEL_FMT, children[err].net_addr, node_extaddr, ncfg.node_num);
children[err].net_addr = BAD_ADDRESS;
}
else
format_str(str, 64, FSTR_NODE_DEL_FMT1, node_extaddr);
//format_str(str, 64, "-Node(0x%llx)", node_extaddr);
//strcat(str, " (n");
//ultoa((unsigned long)ncfg.node_num, str + strlen(str), 10);
//strcat(str, ")");
}
update_disp_nwk();
update_disp_zones_cfg();
//sprintf(str + strlen(str), ")");
//strcat(str, ")");
/*
for(i = 0; i < MAX_CHILDREN; i++)
{
if(children[i].ext_addr == node_extaddr)
children[i].net_addr = BAD_ADDRESS;
}
if(strgFindNode(node_extaddr, &ncfg) >= 0)
{
strcat(str, " (n");
ultoa((unsigned long)ncfg.node_num, str + strlen(str), 10);
strcat(str, ")");
}
*/
}
else
{
format_str(str, 64, "MsgEvt %u", type);
//strcpy(str, "Msg evt ");
//itoa(((uint8_t *)minfo.body_ptr)[8], str + 8, 10);
}
}
break;
default:
//sprintf(str, "Msg type %u", minfo.msg_type);
strcpy(str, "Msg type ");
itoa(minfo.msg_type, str + 9, 10);
}
finish_parse_msg:
if(*str)
{
logAddEntry(&entry, str);
on_msg_add();
//memcpy(&message_table[last_msg_pos], str, strlen(str) + 1);
//memcopy(&message_table[last_msg_pos], str, strlen(str) + 1);
//if(msg_count < MSG_TABLE_SIZE)
// msg_count++;
}
return msg_size;
}
