void DW1000RangingClass::timerTick(){ if(_networkDevicesNumber>0 && counterForBlink!=0) { if(_type==TAG){ _expectedMsgId = POLL_ACK; //need to do a broadcast poll ! transmitPoll(&_networkDevices[0]); } } else if(counterForBlink==0) { if(_type==TAG){ transmitBlink(); } //check for inactive devices if we are a TAG or ANCHOR checkForInactiveDevices(); } counterForBlink++; if(counterForBlink>30){ counterForBlink=0; } }
void DW1000RangingClass::resetInactive() { //if inactive if(_type==ANCHOR){ _expectedMsgId = POLL; receiver(); } else if(_type==TAG){ _expectedMsgId = POLL_ACK; transmitPoll(); } noteActivity(); }
void DW1000RangingClass::startAsTag(DW1000Device myDevice, DW1000Device networkDevices[]){ //we copy our network array into _networkDevices memcpy(_networkDevices, networkDevices, sizeof(DW1000Device)); byte address[8]; myDevice.getAddress(address); DW1000.setEUI(address); generalStart(); //defined type as anchor _type=TAG; if(DEBUG){ Serial.println("### START AS TAG ###"); } //we can start to poll: (this is the TAG which start to poll) transmitPoll(); }
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(); } } } }