File content as of revision 34:f56962030c5c:

#include "MMRanging.h"

MMRanging::MMRanging(DW1000& DW) : dw(DW) {
    MMRanging::dw = dw;
    
    ranging_i = 0;
    
    message[0] = '\0';
    messageRX[0] = '\0';
    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
    uint8_t Index = RX.sequence_number % 10;

    switch (RX.type) {
        case 1:
            rangingtimingsReceiver[Index][0] = dw.getRXTimestamp();
            TX.type = 2;
            TX.sequence_number = RX.sequence_number;
            TX.time_difference_receiver = 0;
            dw.sendFrame((uint8_t*)&TX, sizeof(TX));
            break;
        case 2:
            rangingtimingsSender[Index][1] = dw.getRXTimestamp();
            break;
        case 3:
            TX.type = 4;
            TX.time_difference_receiver = rangingtimingsReceiver[Index][1] - rangingtimingsReceiver[Index][0];
            dw.sendFrame((uint8_t*)&TX, sizeof(TX));
            break;
        case 4:
            //tofs[Index] = RX.time_difference_receiver;
            tofs[Index] = (rangingtimingsSender[Index][1] - rangingtimingsSender[Index][0] - RX.time_difference_receiver);// - 65536;
            //tofs[RX->sequence_number] = (rangingtimingsSender[RX->sequence_number][1] - rangingtimingsSender[RX->sequence_number][0] - RX->time_difference_receiver) >> 1;
            break;
        default : break;
    }
    
    eventtimes[event_i] = RX_timestamp - TX_timestamp;                      // TODO: can give some wrong values because of timer reset after 17 seconds
    event[event_i][0] = '!';
    event[event_i][1] = 'R';
    event[event_i][2] = ' ';
    sprintf(&event[event_i][3], "%d %d", RX.sequence_number, RX.type);
    if (event_i == 8)
        event_i = 0;
    else
        event_i++;
    
    dw.startRX();
}

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

void MMRanging::requestRanging() {
    TX.type = 1;
    TX.sequence_number = counter;
    TX.time_difference_receiver = 0;
    dw.sendFrame((uint8_t*)&TX, sizeof(TX));
}

void MMRanging::requestTime() {
    TX.type = 3;
    TX.sequence_number = counter;
    TX.time_difference_receiver = 0;
    dw.sendFrame((uint8_t*)&TX, sizeof(TX));
    counter++;
}

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