Esempio n. 1
0
void DW1000RangingClass::loop() {
	//we check if needed to reset !
	checkForReset();
	long time = millis();
	if(time-timer > _timerDelay) {
		timer = time;
		timerTick();
	}
	
	if(_sentAck) {
		_sentAck = false;
		
		
		int messageType = detectMessageType(data);
		
		if(messageType != POLL_ACK && messageType != POLL && messageType != RANGE)
			return;
		
		
		//A msg was sent. We launch the ranging protocole when a message was sent
		if(_type == ANCHOR) {
			if(messageType == POLL_ACK) {
				DW1000Device* myDistantDevice = searchDistantDevice(_lastSentToShortAddress);
				
				DW1000.getTransmitTimestamp(myDistantDevice->timePollAckSent);
			}
		}
		else if(_type == TAG) {
			if(messageType == POLL) {
				DW1000Time timePollSent;
				DW1000.getTransmitTimestamp(timePollSent);
				//if the last device we send the POLL is broadcast:
				if(_lastSentToShortAddress[0] == 0xFF && _lastSentToShortAddress[1] == 0xFF) {
					//we save the value for all the devices !
					for(short i = 0; i < _networkDevicesNumber; i++) {
						_networkDevices[i].timePollSent = timePollSent;
					}
				}
				else {
					//we search the device associated with the last send address
					DW1000Device* myDistantDevice = searchDistantDevice(_lastSentToShortAddress);
					//we save the value just for one device
					myDistantDevice->timePollSent = timePollSent;
				}
			}
			else if(messageType == RANGE) {
				DW1000Time timeRangeSent;
				DW1000.getTransmitTimestamp(timeRangeSent);
				//if the last device we send the POLL is broadcast:
				if(_lastSentToShortAddress[0] == 0xFF && _lastSentToShortAddress[1] == 0xFF) {
					//we save the value for all the devices !
					for(short i = 0; i < _networkDevicesNumber; i++) {
						_networkDevices[i].timeRangeSent = timeRangeSent;
					}
				}
				else {
					//we search the device associated with the last send address
					DW1000Device* myDistantDevice = searchDistantDevice(_lastSentToShortAddress);
					//we save the value just for one device
					myDistantDevice->timeRangeSent = timeRangeSent;
				}
				
			}
		}
		
	}
	
	//check for new received message
	if(_receivedAck) {
		_receivedAck = false;
		
		
		//we read the datas from the modules:
		// get message and parse
		DW1000.getData(data, LEN_DATA);
		
		
		int messageType = detectMessageType(data);
		
		//we have just received a BLINK message from tag
		if(messageType == BLINK && _type == ANCHOR) {
			byte address[8];
			byte shortAddress[2];
			_globalMac.decodeBlinkFrame(data, address, shortAddress);
			//we crate a new device with th tag
			DW1000Device myTag(address, shortAddress);
			
			if(addNetworkDevices(&myTag)) {
				Serial.print("blink; 1 device added ! -> ");
				Serial.print(" short:");
				Serial.println(myTag.getShortAddress(), HEX);
				
				//we relpy by the transmit ranging init message
				transmitRangingInit(&myTag);
				noteActivity();
			}
			_expectedMsgId = POLL;
		}
		else if(messageType == RANGING_INIT && _type == TAG) {
			
			byte address[2];
			_globalMac.decodeLongMACFrame(data, address);
			//we crate a new device with the anchor
			DW1000Device myAnchor(address, true);
			
			if(addNetworkDevices(&myAnchor, true)) {
				Serial.print("ranging init; 1 device added ! -> ");
				Serial.print(" short:");
				Serial.println(myAnchor.getShortAddress(), HEX);
			}
			
			noteActivity();
			
		}
		else {
			//we have a short mac layer frame !
			byte address[2];
			_globalMac.decodeShortMACFrame(data, address);
			
			
			
			//we get the device which correspond to the message which was sent (need to be filtered by MAC address)
			DW1000Device* myDistantDevice = searchDistantDevice(address);
			
			
			if(myDistantDevice == NULL) {
				Serial.println("Not found");
				//we don't have the short address of the device in memory
				/*
				Serial.print("unknown: ");
				Serial.print(address[0], HEX);
				Serial.print(":");
				Serial.println(address[1], HEX);
				*/
				return;
			}
			
			
			//then we proceed to range protocole
			if(_type == ANCHOR) {
				if(messageType != _expectedMsgId) {
					// unexpected message, start over again (except if already POLL)
					_protocolFailed = true;
				}
				if(messageType == POLL) {
					//we receive a POLL which is a broacast message
					//we need to grab info about it
					short numberDevices = 0;
					memcpy(&numberDevices, data+SHORT_MAC_LEN+1, 1);
					
					for(short i = 0; i < numberDevices; i++) {
						//we need to test if this value is for us:
						//we grab the mac address of each devices:
						byte shortAddress[2];
						memcpy(shortAddress, data+SHORT_MAC_LEN+2+i*4, 2);
						
						//we test if the short address is our address
						if(shortAddress[0] == _currentShortAddress[0] && shortAddress[1] == _currentShortAddress[1]) {
							//we grab the replytime wich is for us
							unsigned int replyTime = 0;
							memcpy(&replyTime, data+SHORT_MAC_LEN+2+i*4+2, 2);
							//we configure our replyTime;
							_replyDelayTimeUS = replyTime;
							
							// on POLL we (re-)start, so no protocol failure
							_protocolFailed = false;
							
							DW1000.getReceiveTimestamp(myDistantDevice->timePollReceived);
							//we note activity for our device:
							myDistantDevice->noteActivity();
							//we indicate our next receive message for our ranging protocole
							_expectedMsgId = RANGE;
							transmitPollAck(myDistantDevice);
							noteActivity();
							
							return;
						}
						
					}
					
					
				}
				else if(messageType == RANGE) {
					//we receive a RANGE which is a broacast message
					//we need to grab info about it
					short numberDevices = 0;
					memcpy(&numberDevices, data+SHORT_MAC_LEN+1, 1);
					
					
					for(short i = 0; i < numberDevices; i++) {
						//we need to test if this value is for us:
						//we grab the mac address of each devices:
						byte shortAddress[2];
						memcpy(shortAddress, data+SHORT_MAC_LEN+2+i*17, 2);
						
						//we test if the short address is our address
						if(shortAddress[0] == _currentShortAddress[0] && shortAddress[1] == _currentShortAddress[1]) {
							//we grab the replytime wich is for us
							DW1000.getReceiveTimestamp(myDistantDevice->timeRangeReceived);
							noteActivity();
							_expectedMsgId = POLL;
							
							if(!_protocolFailed) {
								
								myDistantDevice->timePollSent.setTimestamp(data+SHORT_MAC_LEN+4+17*i);
								myDistantDevice->timePollAckReceived.setTimestamp(data+SHORT_MAC_LEN+9+17*i);
								myDistantDevice->timeRangeSent.setTimestamp(data+SHORT_MAC_LEN+14+17*i);
								
								// (re-)compute range as two-way ranging is done
								DW1000Time myTOF;
								computeRangeAsymmetric(myDistantDevice, &myTOF); // CHOSEN RANGING ALGORITHM
								
								float distance = myTOF.getAsMeters();
								
								myDistantDevice->setRXPower(DW1000.getReceivePower());
								myDistantDevice->setRange(distance);
								
								myDistantDevice->setFPPower(DW1000.getFirstPathPower());
								myDistantDevice->setQuality(DW1000.getReceiveQuality());
								
								//we send the range to TAG
								transmitRangeReport(myDistantDevice);
								
								//we have finished our range computation. We send the corresponding handler
								_lastDistantDevice = myDistantDevice->getIndex();
								if(_handleNewRange != 0) {
									(*_handleNewRange)();
								}
								
							}
							else {
								transmitRangeFailed(myDistantDevice);
							}
							
							
							return;
						}
						
					}
					
					
				}
			}
			else if(_type == TAG) {
				// get message and parse
				if(messageType != _expectedMsgId) {
					// unexpected message, start over again
					//not needed ?
					return;
					_expectedMsgId = POLL_ACK;
					return;
				}
				if(messageType == POLL_ACK) {
					DW1000.getReceiveTimestamp(myDistantDevice->timePollAckReceived);
					//we note activity for our device:
					myDistantDevice->noteActivity();
					
					
					
					
					//in the case the message come from our last device:
					if(myDistantDevice->getIndex() == _networkDevicesNumber-1) {
						_expectedMsgId = RANGE_REPORT;
						//and transmit the next message (range) of the ranging protocole (in broadcast)
						transmitRange(NULL);
					}
				}
				else if(messageType == RANGE_REPORT) {
					
					float curRange;
					memcpy(&curRange, data+1+SHORT_MAC_LEN, 4);
					float curRXPower;
					memcpy(&curRXPower, data+5+SHORT_MAC_LEN, 4);
					//we have a new range to save !
					myDistantDevice->setRange(curRange);
					myDistantDevice->setRXPower(curRXPower);
					
					
					//We can call our handler !
					//we have finished our range computation. We send the corresponding handler
					_lastDistantDevice = myDistantDevice->getIndex();
					if(_handleNewRange != 0) {
						(*_handleNewRange)();
					}
				}
				else if(messageType == RANGE_FAILED) {
					//not needed as we have a timer;
					return;
					_expectedMsgId = POLL_ACK;
				}
			}
		}
		
	}
}
Esempio n. 2
0
void DW1000RangingClass::loop(){
    //we check if needed to reset !
    checkForReset();
    long time=millis(); 
    if(time-timer>DEFAULT_TIMER_DELAY){
        timer=time;
        timerTick();
    }
    
    if(_sentAck){
        _sentAck = false;
        
        
        int messageType=detectMessageType(data);
        
        if(messageType!=POLL_ACK && messageType!= POLL && messageType!=RANGE)
            return;
        
        
        DW1000Device *myDistantDevice=searchDistantDevice(_lastSentToShortAddress);
        
        if(myDistantDevice==0)
        {
            //we don't have the short address of the device in memory
            /*
            Serial.print("unknown sent: ");
            Serial.print(_lastSentToShortAddress[0], HEX);
            Serial.print(":");
            Serial.println(_lastSentToShortAddress[1], HEX);
             */
        }
        
        
        //A msg was sent. We launch the ranging protocole when a message was sent
        if(_type==ANCHOR){
            if(messageType == POLL_ACK) {
                DW1000.getTransmitTimestamp(myDistantDevice->timePollAckSent);
            }
        }
        else if(_type==TAG){
            if(messageType == POLL) {
                DW1000.getTransmitTimestamp(myDistantDevice->timePollSent);
                //Serial.print("Sent POLL @ "); Serial.println(timePollSent.getAsFloat());
            } else if(messageType == RANGE) { 
                
                DW1000.getTransmitTimestamp(myDistantDevice->timeRangeSent);
            }
        }
        
    }
    
    //check for new received message
    if(_receivedAck){
        _receivedAck=false;
        
        //we read the datas from the modules:
        // get message and parse
        DW1000.getData(data, LEN_DATA);
        
        
        int messageType=detectMessageType(data);
        
        //we have just received a BLINK message from tag
        if(messageType==BLINK && _type==ANCHOR){ 
            byte address[8];
            byte shortAddress[2];
            _globalMac.decodeBlinkFrame(data, address, shortAddress);
            //we crate a new device with th tag
            DW1000Device myTag(address, shortAddress);
            
            if(addNetworkDevices(&myTag))
            {
                Serial.print("blink; 1 device added ! -> "); 
                Serial.print(" short:");
                Serial.println(myTag.getShortAddress(), HEX);
                
                //we relpy by the transmit ranging init message
                transmitRangingInit(&myTag);
                noteActivity();
            }
            _expectedMsgId=POLL;
        }
        else if(messageType==RANGING_INIT && _type==TAG){
            
            byte address[2];
            _globalMac.decodeLongMACFrame(data, address);
            //we crate a new device with the anchor
            DW1000Device myAnchor(address, true);
            
            if(addNetworkDevices(&myAnchor, true))
            {
                Serial.print("ranging init; 1 device added ! -> ");
                Serial.print(" short:");
                Serial.println(myAnchor.getShortAddress(), HEX);
            }
            //we relpy by the transmit ranging init message
            //transmitPoll(&myAnchor);
        
            noteActivity();
            
        }
        else
        {
            //we have a short mac layer frame !
            byte address[2];
            _globalMac.decodeShortMACFrame(data, address);
            
             
            
            //we get the device which correspond to the message which was sent (need to be filtered by MAC address)
            DW1000Device *myDistantDevice=searchDistantDevice(address);
            
            if(myDistantDevice==0)
            {
                //we don't have the short address of the device in memory
                /*
                Serial.print("unknown: ");
                Serial.print(address[0], HEX);
                Serial.print(":");
                Serial.println(address[1], HEX);
                */
                return;
            }
            
            
            
        
            //then we proceed to range protocole
            if(_type==ANCHOR){
                if(messageType != _expectedMsgId) {
                    // unexpected message, start over again (except if already POLL)
                    _protocolFailed = true;
                }
                if(messageType == POLL) {
                    //Serial.println("receive poll");
                    // on POLL we (re-)start, so no protocol failure
                    _protocolFailed = false;
                    
                    DW1000.getReceiveTimestamp(myDistantDevice->timePollReceived);
                    //we note activity for our device:
                    myDistantDevice->noteActivity();
                    //we indicate our next receive message for our ranging protocole
                    _expectedMsgId = RANGE;
                    transmitPollAck(myDistantDevice);
                    noteActivity();
                }
                else if(messageType == RANGE) {
                    DW1000.getReceiveTimestamp(myDistantDevice->timeRangeReceived);
                    noteActivity();
                    _expectedMsgId = POLL;
                    
                    if(!_protocolFailed) {
                        
                        myDistantDevice->timePollSent.setTimestamp(data+1+SHORT_MAC_LEN);
                        myDistantDevice->timePollAckReceived.setTimestamp(data+6+SHORT_MAC_LEN);
                        myDistantDevice->timeRangeSent.setTimestamp(data+11+SHORT_MAC_LEN);
                        
                        // (re-)compute range as two-way ranging is done
                        DW1000Time myTOF;
                        computeRangeAsymmetric(myDistantDevice, &myTOF); // CHOSEN RANGING ALGORITHM
                        
                        float distance=myTOF.getAsMeters();
                        
                        myDistantDevice->setRXPower(DW1000.getReceivePower());
                        myDistantDevice->setRange(distance);
                         
                        myDistantDevice->setFPPower(DW1000.getFirstPathPower());
                        myDistantDevice->setQuality(DW1000.getReceiveQuality());
                        
                        //we send the range to TAG
                        transmitRangeReport(myDistantDevice);
                        
                        //we have finished our range computation. We send the corresponding handler
                        
                        if(_handleNewRange != 0) {
                            (*_handleNewRange)();
                        }
                        
                    }
                    else {
                        transmitRangeFailed(myDistantDevice);
                    }
                    
                    noteActivity();
                }
            }
            else if(_type==TAG){
                // get message and parse
                if(messageType != _expectedMsgId) {
                    // unexpected message, start over again
                    _expectedMsgId = POLL_ACK;
                    //transmitPoll(myDistantDevice);
                    return;
                }
                if(messageType == POLL_ACK) {
                    DW1000.getReceiveTimestamp(myDistantDevice->timePollAckReceived);
                    _expectedMsgId = RANGE_REPORT;
                    //we note activity for our device:
                    myDistantDevice->noteActivity();
                    //and transmit the next message (range) of the ranging protocole
                    transmitRange(myDistantDevice);
                }
                else if(messageType == RANGE_REPORT) {
                    _expectedMsgId = POLL_ACK;
                    float curRange;
                    memcpy(&curRange, data+1+SHORT_MAC_LEN, 4);
                    float curRXPower;
                    memcpy(&curRXPower, data+5+SHORT_MAC_LEN, 4);
                    //we have a new range to save !
                    myDistantDevice->setRange(curRange);
                    myDistantDevice->setRXPower(curRXPower);
                    
                    
                    //We can call our handler !
                    if(_handleNewRange != 0){
                        (*_handleNewRange)();
                    }
                }
                else if(messageType == RANGE_FAILED) {
                    _expectedMsgId = POLL_ACK;
                }
            }
        }
        
    }
}
void DW1000RangingClass::loop(){
    //we check if needed to reset !
    checkForReset();
        
    
    if(_sentAck){
        _sentAck = false;
        
        //we get the device which correspond to the message which was sent (need to be filtered by MAC address)
        DW1000Device *myDistantDevice=&_networkDevices[0];
        
        //A msg was sent. We launch the ranging protocole when a message was sent
        if(_type==ANCHOR){
            if(data[0] == POLL_ACK) {
                DW1000.getTransmitTimestamp((*myDistantDevice).timePollAckSent);
                noteActivity();
            }
        }
        else if(_type==TAG){
            if(data[0] == POLL) {
                DW1000.getTransmitTimestamp((*myDistantDevice).timePollSent);
                //Serial.print("Sent POLL @ "); Serial.println(timePollSent.getAsFloat());
            } else if(data[0] == RANGE) {
                DW1000.getTransmitTimestamp((*myDistantDevice).timeRangeSent);
                noteActivity();
            }
        }
        
    }
    
    //check for new received message
    if(_receivedAck){
        _receivedAck=false;
        
        //we get the device which correspond to the message which was sent (need to be filtered by MAC address)
        DW1000Device *myDistantDevice=&_networkDevices[0];
        
        //we read the datas from the modules:
        // get message and parse
        DW1000.getData(data, LEN_DATA);
        
        //then we proceed to range protocole
        if(_type==ANCHOR){
            if(data[0] != _expectedMsgId) {
                // unexpected message, start over again (except if already POLL)
                _protocolFailed = true;
            }
            if(data[0] == POLL) {
                // on POLL we (re-)start, so no protocol failure
                _protocolFailed = false;
                DW1000.getReceiveTimestamp((*myDistantDevice).timePollReceived);
                _expectedMsgId = RANGE;
                transmitPollAck();
                noteActivity();
            }
            else if(data[0] == RANGE) {
                DW1000.getReceiveTimestamp((*myDistantDevice).timeRangeReceived);
                _expectedMsgId = POLL;
                if(!_protocolFailed) {
                    (*myDistantDevice).timePollSent.setTimestamp(data+1);
                    (*myDistantDevice).timePollAckReceived.setTimestamp(data+6);
                    (*myDistantDevice).timeRangeSent.setTimestamp(data+11);
                    
                    // (re-)compute range as two-way ranging is done
                    computeRangeAsymmetric(myDistantDevice); // CHOSEN RANGING ALGORITHM
                    
                    float distance=(*myDistantDevice).timeComputedRange.getAsMeters();
                    (*myDistantDevice).setRXPower(DW1000.getReceivePower());
                    float rangeBias=rangeRXCorrection((*myDistantDevice).getRXPower());
                    (*myDistantDevice).setRange(distance-rangeBias);
                     
                    (*myDistantDevice).setFPPower(DW1000.getFirstPathPower());
                    (*myDistantDevice).setQuality(DW1000.getReceiveQuality());
                    
                    //we wend the range to TAG
                    transmitRangeReport(myDistantDevice);
                    
                    
                    
                    
                    //we have finished our range computation. We send the corresponding handler
                    
                    if(_handleNewRange != 0) {
                        (*_handleNewRange)();
                    }
                    
                }
                else {
                    transmitRangeFailed();
                }
                
                noteActivity();
            }
        }
        else if(_type==TAG){
            
            // get message and parse
            if(data[0] != _expectedMsgId) {
                // unexpected message, start over again
                //Serial.print("Received wrong message # "); Serial.println(msgId);
                _expectedMsgId = POLL_ACK;
                transmitPoll();
                return;
            }
            if(data[0] == POLL_ACK) {
                DW1000.getReceiveTimestamp((*myDistantDevice).timePollAckReceived);
                _expectedMsgId = RANGE_REPORT;
                transmitRange(myDistantDevice);
                noteActivity();
            }
            else if(data[0] == RANGE_REPORT) {
                _expectedMsgId = POLL_ACK;
                float curRange;
                memcpy(&curRange, data+1, 4);
                float curRXPower;
                memcpy(&curRXPower, data+5, 4);
                //we have a new range to save !
                (*myDistantDevice).setRange(curRange);
                (*myDistantDevice).setRXPower(curRXPower);
                
                //We can call our handler !
                if(_handleNewRange != 0){
                    (*_handleNewRange)();
                }
                
                //we start again ranging
                transmitPoll();
                noteActivity();
            }
            else if(data[0] == RANGE_FAILED) {
                _expectedMsgId = POLL_ACK;
                transmitPoll();
                noteActivity();
            }
        }

    }
}