File content as of revision 52:94688f414dcd:

// by Matthias Grob & Manuel Stalder - ETH Zürich - 2015
#include "mbed.h"
#include "PC.h"                                     // Serial Port via USB for debugging with Terminal
#include "DW1000.h"                                 // our DW1000 device driver
#include "MM2WayRanging.h"                          // our self developed ranging application
#include "InterruptMultiplexer.h"

#include "mavlink_bridge/mavlink_bridge.h"

#define ANCHOR false

using namespace ait;

const int SPI_FREQUENCY = 1000000;
const int NUM_OF_ANCHORS = 1;
const int ANCHOR_ADDRESS_OFFSET = 10;
const bool USE_NLOS_SETTINGS = true;
const PinName DW_RESET_PIN = D15;
const PinName DW_IRQ_PIN = D14;
const PinName DW_MOSI_PIN = D11;
const PinName DW_MISO_PIN = D12;
const PinName DW_SCLK_PIN = D13;
#if ANCHOR
    const bool MAVLINK_COMM = false;
    const int NUM_OF_DW_UNITS = 1;
    const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
#else
//    const bool MAVLINK_COMM = true;
    const bool MAVLINK_COMM = false;
    const int NUM_OF_DW_UNITS = 3;
    const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10, D9, D8};
#endif

PC pc(USBTX, USBRX, 115200);           // USB UART Terminal

void rangeAndDisplayAll(MM2WayRanging& node, MAVLinkBridge& mb, Timer& timer) {
    for (int i = 0; i < NUM_OF_ANCHORS; i++) {
        uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + i;
        node.requestRanging(remote_address);

        if (MAVLINK_COMM) {
            uint8_t address = node.address;
            uint32_t stamp_us = timer.read_us();
            float round_trip_time = node.roundtriptimes[remote_address];
            float range = node.distances[remote_address];
            // Initialize the required buffers
            mavlink_message_t msg;
            // Pack the message
            mavlink_msg_uwb_range_pack(mb.getSysId(), mb.getCompId(), &msg, address, remote_address, stamp_us, round_trip_time, range);
            mb.sendMessage(msg);

            uint16_t std_noise_1 = node.reception_stats[remote_address][0].std_noise;
            uint16_t std_noise_2 = node.reception_stats[remote_address][1].std_noise;
            uint16_t std_noise_3 = node.reception_stats[remote_address][2].std_noise;
            uint16_t preamble_acc_count_1 = node.reception_stats[remote_address][0].preamble_acc_count;
            uint16_t preamble_acc_count_2 = node.reception_stats[remote_address][1].preamble_acc_count;
            uint16_t preamble_acc_count_3 = node.reception_stats[remote_address][2].preamble_acc_count;
            uint16_t first_path_index_1 = node.reception_stats[remote_address][0].first_path_index;
            uint16_t first_path_index_2 = node.reception_stats[remote_address][1].first_path_index;
            uint16_t first_path_index_3 = node.reception_stats[remote_address][2].first_path_index;
            uint16_t first_path_amp_1_1 = node.reception_stats[remote_address][0].first_path_amp_1;
            uint16_t first_path_amp_1_2 = node.reception_stats[remote_address][1].first_path_amp_1;
            uint16_t first_path_amp_1_3 = node.reception_stats[remote_address][2].first_path_amp_1;
            uint16_t first_path_amp_2_1 = node.reception_stats[remote_address][0].first_path_amp_2;
            uint16_t first_path_amp_2_2 = node.reception_stats[remote_address][1].first_path_amp_2;
            uint16_t first_path_amp_2_3 = node.reception_stats[remote_address][2].first_path_amp_2;
            uint16_t first_path_amp_3_1 = node.reception_stats[remote_address][0].first_path_amp_3;
            uint16_t first_path_amp_3_2 = node.reception_stats[remote_address][1].first_path_amp_3;
            uint16_t first_path_amp_3_3 = node.reception_stats[remote_address][2].first_path_amp_3;
            uint16_t channel_impulse_response_power_1 = node.reception_stats[remote_address][0].channel_impulse_response_power;
            uint16_t channel_impulse_response_power_2 = node.reception_stats[remote_address][1].channel_impulse_response_power;
            uint16_t channel_impulse_response_power_3 = node.reception_stats[remote_address][2].channel_impulse_response_power;
            uint8_t prf_1 = node.reception_stats[remote_address][0].prf;
            uint8_t prf_2 = node.reception_stats[remote_address][1].prf;
            uint8_t prf_3 = node.reception_stats[remote_address][2].prf;
            // Pack the message
            mavlink_msg_uwb_range_stats_pack(
                mb.getSysId(), mb.getCompId(), &msg,
                address, remote_address, stamp_us,
                round_trip_time, range,
                std_noise_1, std_noise_2, std_noise_2,
                preamble_acc_count_1, preamble_acc_count_2, preamble_acc_count_3,
                first_path_index_1, first_path_index_2, first_path_index_3,
                first_path_amp_1_1, first_path_amp_1_2, first_path_amp_1_3,
                first_path_amp_2_1, first_path_amp_2_2, first_path_amp_2_3,
                first_path_amp_3_1, first_path_amp_3_2, first_path_amp_3_3,
                channel_impulse_response_power_1, channel_impulse_response_power_2, channel_impulse_response_power_3,
                prf_1, prf_2, prf_3
            );
            mb.sendMessage(msg);

            /*mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise1", std_noise1);
            mb.sendMessage(msg);
            mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise2", std_noise2);
            mb.sendMessage(msg);
            mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise3", std_noise3);
            mb.sendMessage(msg);*/
        } else {
            pc.printf("%d - %d> dist = %.4f\r\n", node.address, remote_address, node.distances[remote_address]);
        }
    }
}

void calibrationRanging(MM2WayRanging& node, int destination){
    const int numberOfRangings = 100;
    float rangings[numberOfRangings];
    int index = 0;
    float mean = 0;
    float start, stop;

    Timer localTimer;
    localTimer.start();

    start = localTimer.read();

    while (1) {

        node.requestRanging(destination);
        if(node.overflow){
            pc.printf("Overflow! Measured: %f\r\n", node.distances[destination]); // This is the output to see if a timer overflow was corrected by the two way ranging class
        }

        if (node.distances[destination] == -1) {
            pc.printf("Measurement timed out\r\n");
            wait(0.001);
            continue;
        }

        if (node.distances[destination] < 100) {
            rangings[index] = node.distances[destination];
            //pc.printf("%f\r\n", node.distances[destination]);
            index++;

            if (index == numberOfRangings) {
                stop = localTimer.read();

                for (int i = 0; i < numberOfRangings - 1; i++)
                    rangings[numberOfRangings - 1] += rangings[i];

                mean = rangings[numberOfRangings - 1] / numberOfRangings;
                pc.printf("\r\n\r\nMean %i: %f\r\n", destination, mean);
                pc.printf("Elapsed Time for %i: %f\r\n", numberOfRangings, stop - start);

                mean = 0;
                index = 0;

                //wait(2);

                start = localTimer.read();
            }
        }
        else
            pc.printf("%f\r\n", node.distances[destination]);
    }
}

struct __attribute__((packed, aligned(1))) DistancesFrame {
    uint8_t source;
    uint8_t destination;
    uint8_t type;
    float dist[4];
};


// -----------------------------------------------------------------------------------------------
void altCallbackRX();

int main() {
    UART_Mbed uart(USBTX, USBRX, 115200);
    MAVLinkBridge mb(&uart);

    if (!MAVLINK_COMM) {
        pc.printf("==== AIT UWB Range ====\r\n");
    }

    SPI spi(DW_MOSI_PIN, DW_MISO_PIN, DW_SCLK_PIN);
    spi.format(8, 0);                    // Setup the spi for standard 8 bit data and SPI-Mode 0 (GPIO5, GPIO6 open circuit or ground on DW1000)
    // NOTE: Minimum Frequency 1MHz. Below it is now working. Could be something with the activation and deactivation of interrupts.
    spi.frequency(SPI_FREQUENCY);             // with a 1MHz clock rate (worked up to 49MHz in our Test)

    // Setup interrupt pin
    InterruptMultiplexer irq_mp(DW_IRQ_PIN);
    irq_mp.getIRQ().rise(&irq_mp, &InterruptMultiplexer::trigger);  // attach interrupt handler to rising edge of interrupt pin from DW1000

    Timer timer;
    timer.start();

    while (true) {

    DW1000* dw_array[NUM_OF_DW_UNITS + 2];
    MM2WayRanging* node_array[NUM_OF_DW_UNITS + 2];                           // Instance of the two way ranging algorithm

    if (!MAVLINK_COMM) {
        pc.printf("Performing hardware reset of UWB modules\r\n");
    }
    // == IMPORTANT == Create all DW objects first (this will cause a reset of the DW module)
    DW1000::hardwareReset(DW_RESET_PIN);
    for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
        dw_array[i] = new DW1000(spi, irq_mp, DW_CS_PINS[i]);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
    }

    // Now we can initialize the DW modules
    for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
        DW1000& dw = *dw_array[i];
        dw.setEUI(0xFAEDCD01FAEDCD01 + i);                                  // basic methods called to check if we have a working SPI connection
        node_array[i] = new MM2WayRanging(*dw_array[i]);
        if (MAVLINK_COMM) {
            // TODO
        } else {
            pc.printf("\r\nUnit %d\r\n", i);
            pc.printf("\r\nDecaWave 1.0   up and running!\r\n");            // Splashscreen
            pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
            pc.printf("EUI register: %016llX\r\n", dw.getEUI());
            pc.printf("Voltage: %.2fV\r\n", dw.getVoltage());
        }
    }

    for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
        DW1000& dw = *dw_array[i];
        MM2WayRanging& node = *node_array[i];
        node.isAnchor = ANCHOR;                  // declare as anchor or beacon
        if (ANCHOR) {
            node.address = ANCHOR_ADDRESS_OFFSET + i;
            if (!MAVLINK_COMM)
                pc.printf("This node is Anchor node %d\r\n", node.address);
        } else {
            node.address = i;
            if (!MAVLINK_COMM)
                pc.printf("This node is a Beacon\r\n");
        }
    }

    // Set NLOS settings
    if (USE_NLOS_SETTINGS) {
        for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
            if (!MAVLINK_COMM)
                pc.printf("Setting NLOS configuration for Unit %d\r\n", i);
            DW1000& dw = *dw_array[i];
            dw.writeRegister8(DW1000_LDE_CTRL, 0x0806, 0x07);              //LDE_CFG1
            dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x0003);           //LDE_CFG2
            //dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1603);           //LDE_CFG2
            // Channel control
            uint16_t prf_mask = (0x1 << 19) | (0x1 << 18);
            uint16_t prf = 0x01 << 18; // 16 MHz
            uint16_t v = dw.readRegister16(DW1000_CHAN_CTRL, 0x00);
            v &= ~prf_mask;
            v |= prf;
            dw.writeRegister16(DW1000_CHAN_CTRL, 0x00, v);
        }
    }

    if (!MAVLINK_COMM)
        pc.printf("Entering main loop\r\n");

    //for (int i = 0; i < 10; ++i) {
    while (true) {
        if (ANCHOR) {
            pc.printf("."); // to see if the core and output is working
            wait_ms(500);
        } else {
            for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
                MM2WayRanging& node = *node_array[j];
                rangeAndDisplayAll(node, mb, timer);
            }
        }
    }

    for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
        delete node_array[i];
        delete dw_array[i];
    }

    }
}


void altCallbackRX(DW1000& dw) { // this callback was used in our verification test case for the observer node which only receives and outputs the resulting data
   DistancesFrame distFrame;
   float distances[4];
   dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&distFrame, dw.getFramelength());

    if (distFrame.destination == 5) {
        for (int i = 0; i<4; i++){
            pc.printf("%f, ", distFrame.dist[i]);  
        } 
        pc.printf("\r\n");
    }
    dw.startRX();
}