File content as of revision 31:6f76f3d518ac:

#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();
    
    dw.receiveString(messageRX);
    
    rangingframe* RX = (rangingframe*)messageRX;
    switch (RX->type) {
        case 1:
            rangingtimingsReceiver[RX->sequence_number][0] = dw.getRXTimestamp();
            TX.type = 2;
            TX.sequence_number = RX->sequence_number;
            TX.time_difference_receiver = 0;
            dw.sendFrame((uint8_t*)&TX, 10);
            break;
        case 2:
            rangingtimingsSender[RX->sequence_number][1] = dw.getRXTimestamp();
            break;
        case 3:
            TX.type = 4;
            TX.time_difference_receiver = rangingtimingsReceiver[TX.sequence_number][1] - rangingtimingsReceiver[TX.sequence_number][0];
            dw.sendFrame((uint8_t*)&TX, 10);
            break;
        case 4:
            tofs[RX->sequence_number] = rangingtimingsSender[RX->sequence_number][1] - rangingtimingsSender[RX->sequence_number][0] - RX->time_difference_receiver;
            //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] = ' ';
    for(int i = 0; i < 10; i++)
        event[event_i][i+3] = messageRX[i];
    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.sequence_number][0] = dw.getTXTimestamp();
            break;
        case 2:
            rangingtimingsReceiver[TX.sequence_number][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);
    //for(int i = 0; i < 10; i++)
    //    event[event_i][i+3] = message[i];
    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, 10);
}

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