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