void LidarProcessOctree::subdivide(LidarProcessOctree::Node& node)
{
{
#if ALLOW_THREADING
Threads::Mutex::Lock lruLock(lruMutex);
#endif
++numSubdivideCalls;
}
#if ALLOW_THREADING
Threads::Mutex::Lock nodeLock(node.mutex);
#endif
/* Bail out if the node's children have magically appeared since the last check: */
if(node.children!=0)
return;
/* Check if the node cache is full: */
{
#if ALLOW_THREADING
Threads::Mutex::Lock lruLock(lruMutex);
#endif
if(numCachedNodes+8>cacheSize)
{
/* Find an unused leaf node parent and remove its children: */
Node* coarsenNode;
#if ALLOW_THREADING
for(coarsenNode=lruHead;coarsenNode!=0;coarsenNode=coarsenNode->lruSucc)
{
/* Check if the leaf parent is currently unused: */
coarsenNode->mutex.lock();
if(coarsenNode->processCounter==0)
break;
coarsenNode->mutex.unlock();
}
#else
for(coarsenNode=lruHead;coarsenNode!=0&&coarsenNode->processCounter!=0;coarsenNode=coarsenNode->lruSucc)
;
#endif
/* Remove the found node's children: */
delete[] coarsenNode->children;
coarsenNode->children=0;
#if ALLOW_THREADING
coarsenNode->mutex.unlock();
#endif
/* Remove the found node from the lru list: */
if(coarsenNode->lruPred!=0)
coarsenNode->lruPred->lruSucc=coarsenNode->lruSucc;
else
lruHead=coarsenNode->lruSucc;
if(coarsenNode->lruSucc!=0)
coarsenNode->lruSucc->lruPred=coarsenNode->lruPred;
else
lruTail=coarsenNode->lruPred;
coarsenNode->lruPred=0;
coarsenNode->lruSucc=0;
/* Update the node cache: */
numCachedNodes-=8;
/* Check if the found node's parent is now a leaf parent node: */
Node* parent=coarsenNode->parent;
if(parent!=0)
{
/* Check if all children of the parent are leaves: */
bool leafParent=true;
for(int childIndex=0;childIndex<8&&leafParent;++childIndex)
leafParent=parent->children[childIndex].children==0;
if(leafParent)
{
/* Add the parent to the end of the leaf parent node list: */
parent->lruPred=lruTail;
parent->lruSucc=0;
if(lruTail!=0)
lruTail->lruSucc=parent;
else
lruHead=parent;
lruTail=parent;
}
}
}
/* Check if the node's parent was a leaf parent: */
Node* parent=node.parent;
if(parent!=0&&(parent->lruPred!=0||parent->lruSucc!=0||lruHead==parent))
{
/* Remove the node's parent from the leaf parent node list: */
if(parent->lruPred!=0)
parent->lruPred->lruSucc=parent->lruSucc;
else
lruHead=parent->lruSucc;
if(parent->lruSucc!=0)
parent->lruSucc->lruPred=parent->lruPred;
else
lruTail=parent->lruPred;
parent->lruPred=0;
parent->lruSucc=0;
}
/* Add the node to the leaf parent node list: */
node.lruPred=lruTail;
node.lruSucc=0;
if(lruTail!=0)
lruTail->lruSucc=&node;
else
lruHead=&node;
lruTail=&node;
/* Update the node cache: */
numCachedNodes+=8U;
}
/* Create and load the node's children: */
{
#if ALLOW_THREADING
Threads::Mutex::Lock fileLock(fileMutex);
#endif
Node* children=new Node[8];
indexFile.setReadPosAbs(node.childrenOffset);
for(int childIndex=0;childIndex<8;++childIndex)
{
Node& child=children[childIndex];
/* Read the child node's structure: */
LidarOctreeFileNode ofn;
ofn.read(indexFile);
child.parent=&node;
child.childrenOffset=ofn.childrenOffset;
child.domain=Cube(node.domain,childIndex);
child.numPoints=ofn.numPoints;
child.dataOffset=ofn.dataOffset;
child.detailSize=ofn.detailSize;
if(child.numPoints>0)
{
/* Load the child node's points: */
child.points=new LidarPoint[maxNumPointsPerNode]; // Always allocate maximum to prevent memory fragmentation
pointsFile.setReadPosAbs(LidarDataFileHeader::getFileSize()+pointsRecordSize*child.dataOffset);
pointsFile.read(child.points,child.numPoints);
}
++numLoadedNodes;
}
/* Install the node's children array: */
node.children=children;
}
}
void _begin() {
#if !defined(MY_DISABLED_SERIAL)
hwInit();
#endif
// Call before() in sketch (if it exists)
if (before)
before();
debug(PSTR("Starting " MY_NODE_TYPE " (" MY_CAPABILITIES ", " LIBRARY_VERSION ")\n"));
signerInit();
#if defined(MY_RADIO_FEATURE)
_failedTransmissions = 0;
// Setup radio
if (!transportInit()) {
debug(PSTR("Radio init failed. Check wiring.\n"));
// Nothing more we can do
_infiniteLoop();
} else {
debug(PSTR("Radio init successful.\n"));
}
#endif
#if defined(MY_GATEWAY_FEATURE)
#if defined(MY_INCLUSION_BUTTON_FEATURE)
inclusionInit();
#endif
// initialize the transport driver
if (!gatewayTransportInit()) {
debug(PSTR("Transport driver init fail\n"));
// Nothing more we can do
_infiniteLoop();
}
#endif
#if defined(MY_LEDS_BLINKING_FEATURE)
ledsInit();
#endif
// Read latest received controller configuration from EEPROM
hwReadConfigBlock((void*)&_cc, (void*)EEPROM_CONTROLLER_CONFIG_ADDRESS, sizeof(ControllerConfig));
if (_cc.isMetric == 0xff) {
// Eeprom empty, set default to metric
_cc.isMetric = 0x01;
}
#if defined(MY_GATEWAY_FEATURE)
// Set configuration for gateway
_nc.parentNodeId = GATEWAY_ADDRESS;
_nc.distance = 0;
_nc.nodeId = GATEWAY_ADDRESS;
#elif defined(MY_RADIO_FEATURE)
// Read settings from eeprom
hwReadConfigBlock((void*)&_nc, (void*)EEPROM_NODE_ID_ADDRESS, sizeof(NodeConfig));
#ifdef MY_OTA_FIRMWARE_FEATURE
// Read firmware config from EEPROM, i.e. type, version, CRC, blocks
hwReadConfigBlock((void*)&_fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS, sizeof(NodeFirmwareConfig));
#endif
_autoFindParent = MY_PARENT_NODE_ID == AUTO;
if (!_autoFindParent) {
_nc.parentNodeId = MY_PARENT_NODE_ID;
// Save static parent id in eeprom (used by bootloader)
hwWriteConfig(EEPROM_PARENT_NODE_ID_ADDRESS, MY_PARENT_NODE_ID);
// We don't actually know the distance to gw here. Let's pretend it is 1.
// If the current node is also repeater, be aware of this.
_nc.distance = 1;
} else if (!isValidParent(_nc.parentNodeId)) {
// Auto find parent, but parent in eeprom is invalid. Try find one.
transportFindParentNode();
}
if (MY_NODE_ID != AUTO) {
// Set static id
_nc.nodeId = MY_NODE_ID;
// Save static id in eeprom
hwWriteConfig(EEPROM_NODE_ID_ADDRESS, MY_NODE_ID);
} else if (_nc.nodeId == AUTO && isValidParent(_nc.parentNodeId)) {
// Try to fetch node-id from gateway
transportRequestNodeId();
}
#endif
#ifdef MY_NODE_LOCK_FEATURE
// Check if node has been locked down
if (hwReadConfig(EEPROM_NODE_LOCK_COUNTER) == 0) {
// Node is locked, check if unlock pin is asserted, else hang the node
pinMode(MY_NODE_UNLOCK_PIN, INPUT_PULLUP);
// Make a short delay so we are sure any large external nets are fully pulled
unsigned long enter = hwMillis();
while (hwMillis() - enter < 2);
if (digitalRead(MY_NODE_UNLOCK_PIN) == 0) {
// Pin is grounded, reset lock counter
hwWriteConfig(EEPROM_NODE_LOCK_COUNTER, MY_NODE_LOCK_COUNTER_MAX);
// Disable pullup
pinMode(MY_NODE_UNLOCK_PIN, INPUT);
debug(PSTR("Node is unlocked.\n"));
} else {
// Disable pullup
pinMode(MY_NODE_UNLOCK_PIN, INPUT);
nodeLock("LDB"); //Locked during boot
}
} else if (hwReadConfig(EEPROM_NODE_LOCK_COUNTER) == 0xFF) {
// Reset walue
hwWriteConfig(EEPROM_NODE_LOCK_COUNTER, MY_NODE_LOCK_COUNTER_MAX);
}
#endif
// Call sketch setup
if (setup)
setup();
#if defined(MY_RADIO_FEATURE)
transportPresentNode();
#endif
if (presentation)
presentation();
debug(PSTR("Init complete, id=%d, parent=%d, distance=%d\n"), _nc.nodeId, _nc.parentNodeId, _nc.distance);
}
