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;
}
}
}
}
}
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();
}
}
}
}
