File content as of revision 37:40f94c634c3e:

#include "MMRanging.h"

MMRanging::MMRanging(DW1000& DW) : dw(DW) {
    event_i = 0;
    counter = 0;
    dw.setCallbacks(this, &MMRanging::callbackRX, &MMRanging::callbackTX);
    dw.startRX();
}

void MMRanging::callbackRX() {
    RX_timestamp = dw.getRXTimestamp();
    
    rangingframe RX;
    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&RX, dw.getFramelength());  // get data from buffer
    
    if (RX.destination == address)                                                  // only if received packet is for me
        switch (RX.type) {
            case 1:
                rangingtimingsReceiver[RX.source][0] = dw.getRXTimestamp();
                sendRangingframe(RX.source, RX.sequence_number, 2, 0);
                break;
            case 2:
                rangingtimingsSender[RX.source][1] = dw.getRXTimestamp();
                sendRangingframe(RX.source, counter, 3, 0);
                counter++;
                break;
            case 3:
                sendRangingframe(RX.source, RX.sequence_number, 4, rangingtimingsReceiver[RX.source][1] - rangingtimingsReceiver[RX.source][0]);
                break;
            case 4:
                tofs[RX.source] = (rangingtimingsSender[RX.source][1] - rangingtimingsSender[RX.source][0] - RX.time_difference_receiver);
                break;
            default : break;
        }
    
    event[event_i][0] = '!';
    event[event_i][1] = 'R';
    event[event_i][2] = ' ';
    sprintf(&event[event_i][3], "%d>%d / %d %d", RX.source, RX.destination, RX.sequence_number, RX.type);
    if (event_i == 8)
        event_i = 0;
    else
        event_i++;
    
    dw.startRX();
}

void MMRanging::callbackTX() {
    TX_timestamp = dw.getTXTimestamp();
    switch (TX.type) {
        case 1:
            rangingtimingsSender[TX.destination][0] = dw.getTXTimestamp();
            break;
        case 2:
            rangingtimingsReceiver[TX.destination][1] = dw.getTXTimestamp();
            break;
        default: break;
    }
    
    event[event_i][0] = '!';
    event[event_i][1] = 'S';
    event[event_i][2] = ' ';
    sprintf(&event[event_i][3], "%d>%d / %d %d", TX.source, TX.destination, TX.sequence_number, TX.type);
    if (event_i == 8)
        event_i = 0;
    else
        event_i++;
}

void MMRanging::requestRanging(uint8_t destination) {
    sendRangingframe(destination, counter, 1, 0);
}

void MMRanging::sendRangingframe(uint8_t destination, uint8_t sequence_number, uint8_t type, uint64_t time_difference_receiver) {
    TX.source = address;
    TX.destination = destination;
    TX.sequence_number = sequence_number;
    TX.type = type;
    TX.time_difference_receiver = time_difference_receiver;
    dw.sendFrame((uint8_t*)&TX, sizeof(TX));
}