File content as of revision 50:50b8aea54a51:

#include "MM2WayRanging.h"

//#include "PC.h"
//static PC pc(USBTX, USBRX, 115200);           // USB UART Terminal

MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
    for (int i = 0; i < 3; ++i) {
        memset(reception_stats[i], 0, sizeof(ReceptionStats));
    }
    memset(roundtriptimes, 0, sizeof(roundtriptimes));
    memset(distances, 0, sizeof(distances));

    isAnchor = true;
    overflow = false;
    address = 0;
    rxTimestamp = 0;
    timediffRec = 0;
    timediffSend = 0;
    for (int i = 0; i < 10; i++)
        acknowledgement[i] = true;

    dw.setCallbacks(this, &MM2WayRanging::callbackRX, &MM2WayRanging::callbackTX);

    LocalTimer.start();

    dw.startRX();
}

void MM2WayRanging::callbackRX() {
    //pc.printf("Received frame\r\n");
    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());

    if (receivedFrame.remote_address == address) {
        uint16_t std_noise = dw.getStdNoise();
        uint16_t preamble_acc_count = dw.getPACC();
        uint16_t first_path_index = dw.getFPINDEX();
        uint16_t first_path_amp_1 = dw.getFPAMPL1();
        uint16_t first_path_amp_2 = dw.getFPAMPL2();
        uint16_t first_path_amp_3 = dw.getFPAMPL3();
        uint16_t channel_impulse_response_power = dw.getCIRPWR();
        uint8_t prf = dw.getPRF();
        switch (receivedFrame.type) {
            case PING:
                rxTimestamp = dw.getRXTimestamp();
                receiverTimestamps[receivedFrame.address][0] = rxTimestamp;      //Save the first timestamp on the receiving node/anchor (T_rp)
                sendDelayedAnswer(receivedFrame.address, ANCHOR_RESPONSE, rxTimestamp);
                reception_stats[receivedFrame.address][0].std_noise = std_noise;
                reception_stats[receivedFrame.address][0].preamble_acc_count = preamble_acc_count;
                reception_stats[receivedFrame.address][0].first_path_index = first_path_index;
                reception_stats[receivedFrame.address][0].first_path_amp_1 = first_path_amp_1;
                reception_stats[receivedFrame.address][0].first_path_amp_2 = first_path_amp_2;
                reception_stats[receivedFrame.address][0].first_path_amp_3 = first_path_amp_3;
                reception_stats[receivedFrame.address][0].channel_impulse_response_power = channel_impulse_response_power;
                reception_stats[receivedFrame.address][0].prf = prf;
                //pc.printf("Received ping. Noise 0x%0x %d\r\n", std_noise, prf);
                break;
            case ANCHOR_RESPONSE:
                rxTimestamp = dw.getRXTimestamp();
                senderTimestamps[receivedFrame.address][1] = rxTimestamp;        //Save the second timestamp on the sending node/beacon (T_rr)
                sendDelayedAnswer(receivedFrame.address, 3, rxTimestamp);
                //pc.printf("Received anchor response\r\n");
                reception_stats[receivedFrame.address][1].std_noise = std_noise;
                reception_stats[receivedFrame.address][1].preamble_acc_count = preamble_acc_count;
                reception_stats[receivedFrame.address][1].first_path_index = first_path_index;
                reception_stats[receivedFrame.address][1].first_path_amp_1 = first_path_amp_1;
                reception_stats[receivedFrame.address][1].first_path_amp_2 = first_path_amp_2;
                reception_stats[receivedFrame.address][1].first_path_amp_3 = first_path_amp_3;
                reception_stats[receivedFrame.address][1].channel_impulse_response_power = channel_impulse_response_power;
                reception_stats[receivedFrame.address][1].prf = prf;
                break;
            case BEACON_RESPONSE:
                rxTimestamp = dw.getRXTimestamp();
                receiverTimestamps[receivedFrame.address][2] = rxTimestamp;      //Save the third timestamp on the receiving node/anchor (T_rf)

                correctReceiverTimestamps(receivedFrame.address);                //Correct the timestamps for the case of a counter overflow
                //calculation of the summand on the receiving node/anchor
                timediffRec = - 2*receiverTimestamps[receivedFrame.address][1] + receiverTimestamps[receivedFrame.address][0] + receiverTimestamps[receivedFrame.address][2];
                reception_stats[receivedFrame.address][2].std_noise = std_noise;
                reception_stats[receivedFrame.address][2].preamble_acc_count = preamble_acc_count;
                reception_stats[receivedFrame.address][2].first_path_index = first_path_index;
                reception_stats[receivedFrame.address][2].first_path_amp_1 = first_path_amp_1;
                reception_stats[receivedFrame.address][2].first_path_amp_2 = first_path_amp_2;
                reception_stats[receivedFrame.address][2].first_path_amp_3 = first_path_amp_3;
                reception_stats[receivedFrame.address][2].channel_impulse_response_power = channel_impulse_response_power;
                reception_stats[receivedFrame.address][2].prf = prf;
                sendTransferFrame(receivedFrame.address, timediffRec);
                //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][0]);
                //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][1]);
                //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][2]);
                //pc.printf("Received beacon response. Noise %d\r\n", noise_level);
                break;
            case TRANSFER_FRAME:
                //calculation of the summand on the sending node/beacon
                timediffSend = 2 * senderTimestamps[receivedFrame.address][1] - senderTimestamps[receivedFrame.address][0] - senderTimestamps[receivedFrame.address][2];
                //calculation of the resulting sum of all four ToFs.
                tofs[receivedFrame.address] = receivedFrame.signedTime + timediffSend;
                acknowledgement[receivedFrame.address] = true;
                memcpy(&reception_stats[receivedFrame.address][0], &receivedFrame.stats1, sizeof(ReceptionStats));
                memcpy(&reception_stats[receivedFrame.address][2], &receivedFrame.stats2, sizeof(ReceptionStats));
                //reception_stats[receivedFrame.address][0].std_noise = receivedFrame.stats1.std_noise;
                //reception_stats[receivedFrame.address][2].std_noise = receivedFrame.stats2.std_noise;
                //noise_levels[receivedFrame.address][0] = receivedFrame.std_noise1;
                //noise_levels[receivedFrame.address][2] = receivedFrame.std_noise2;
                //pc.printf("Received transfer frame\r\n");
                break;
            default : break;
        }
    }

    dw.startRX();
}

void MM2WayRanging::callbackTX() {
    //pc.printf("Sent frame\r\n");
    switch (rangingFrame.type) {
    case PING:
        senderTimestamps[rangingFrame.remote_address][0] = dw.getTXTimestamp();    //Save the first timestamp on the sending node/beacon (T_sp)
        //pc.printf("Sent ping\r\n");
        break;
    case ANCHOR_RESPONSE:
        receiverTimestamps[rangingFrame.remote_address][1] = dw.getTXTimestamp();  //Save the second timestamp on the receiving node/anchor (T_sr)
        //pc.printf("Sent anchor response\r\n");
        break;
    case BEACON_RESPONSE:
        senderTimestamps[rangingFrame.remote_address][2] = dw.getTXTimestamp();    //Save the third timestamp on the sending node/beacon (T_sr)
        correctSenderTimestamps(rangingFrame.remote_address);                      //Correct the timestamps for the case of a counter overflow
        //pc.printf("Sent beacon response\r\n");
        break;
    default:
        break;
    }

}

/**
 *  Get the distance to the Anchor with address @param remote_address.
 *
 *   @param remote_address The address of the anchor
 */
void MM2WayRanging::requestRanging(uint8_t remote_address) {
//    pc.printf("Request range %d\r\n", remote_address);
    dw.enable_irq();
    dw.startRX();
//    pc.printf("Enabled IRQ %d\r\n", remote_address);
    acknowledgement[remote_address] = false;
    float time_before = LocalTimer.read();

//    pc.printf("Sending ping\r\n");
    sendPingFrame(remote_address);

//    pc.printf("Waiting for ack\r\n");
    while(!acknowledgement[remote_address] && (LocalTimer.read() < time_before + 0.3f)); // wait for succeeding ranging or timeout

    roundtriptimes[remote_address] = LocalTimer.read() - time_before;

    if(acknowledgement[remote_address]){
        distances[remote_address] = calibratedDistance(remote_address);
    } else {
        distances[remote_address] = -1;
    }
    dw.stopTRX();
    dw.disable_irq();
}

inline float MM2WayRanging::calibratedDistance(uint8_t remote_address) {

    float rawDistance = (tofs[remote_address] * 300 * TIMEUNITS_TO_US / 4);



 // Calibration for Nucleo 0 (and 1)

 //   if (this->address == 1) rawDistance+= 10;
//    switch(remote_address){
//        case 2:
//            return  rawDistance * 0.9754 - 0.5004;
//        case 3:
//            return  rawDistance * 0.9759 - 0.4103;
//        case 4:
//            return  rawDistance * 0.9798 - 0.5499;
//        case 5:
//            return  rawDistance * 0.9765 - 0.5169;
//        }

    return rawDistance;

}

void MM2WayRanging::sendPingFrame(uint8_t remote_address) {
    rangingFrame.address = address;
    rangingFrame.remote_address = remote_address;
    rangingFrame.type = PING;
    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
}

void MM2WayRanging::sendTransferFrame(uint8_t remote_address, int timeDiffsReceiver) {
    transferFrame.address = address;
    transferFrame.remote_address = remote_address;
    transferFrame.type = TRANSFER_FRAME;
    transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
    memcpy(&transferFrame.stats1, &reception_stats[receivedFrame.address][0], sizeof(ReceptionStats));
    memcpy(&transferFrame.stats2, &reception_stats[receivedFrame.address][2], sizeof(ReceptionStats));
    dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
}

void MM2WayRanging::sendDelayedAnswer(uint8_t remote_address, uint8_t type, uint64_t rxTimestamp) {

    rangingFrame.address = address;
    rangingFrame.remote_address = remote_address;
    rangingFrame.type = type;

    if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
    else
        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
}

void MM2WayRanging::correctReceiverTimestamps(uint8_t source){

    if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
        receiverTimestamps[source][1] += MMRANGING_2POWER40;
        receiverTimestamps[source][2] += MMRANGING_2POWER40;
    }

    if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
            receiverTimestamps[source][2] += MMRANGING_2POWER40;
        }

}

void MM2WayRanging::correctSenderTimestamps(uint8_t source){

    if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
        senderTimestamps[source][1] += MMRANGING_2POWER40;
        senderTimestamps[source][2] += MMRANGING_2POWER40;
        overflow = true;
    } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
        senderTimestamps[source][2] += MMRANGING_2POWER40;
        overflow = true;
    }else overflow = false;

}