File content as of revision 67:bd0f0580af5a:

// 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
// HACK
//#include "InterruptMultiplexer.h"
#include "InterruptHandler.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 = false;
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 = 4;
//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D9, D8,D10};
//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10,D9,D8};
const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7,D9,D10,D8};
//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10,D9};
//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
#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);


        pc.printf("%d - %d> dist = %.4f\r\n", node.address, remote_address, node.distances[remote_address]);

    }
}
//continue;


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


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

int main()
{
    //first set the CS to high
    DigitalOut test1(D7);
    DigitalOut test2(D8);
    DigitalOut test3(D9);
    DigitalOut test4(D10);
    test1=1;
    test2=1;
    test3=1;
    test4=1;

    wait(0.1);
    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
    // HACK
    //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();



    DW1000* dw_array[NUM_OF_DW_UNITS + 0];
    MM2WayRanging* node_array[NUM_OF_DW_UNITS + 0];                           // Instance of the two way ranging algorithm
    //InterruptHandler* irq_mp_ptr_array[NUM_OF_DW_UNITS];
    InterruptIn irq(DW_IRQ_PIN);

    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 < 4; ++i) {

        // HACK
        /*irq_mp_ptr_array[i] = new InterruptHandler(NC);
        InterruptHandler& irq_mp = *irq_mp_ptr_array[i];
        if (i == 0)
        {
            irq_mp.getIRQ().rise(&irq_mp, &InterruptHandler::trigger);  // attach interrupt handler to rising edge of interrupt pin from DW1000
        }*/
        dw_array[i] = new DW1000(spi, irq, DW_CS_PINS[i]);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)

        // dw_array[1] = new DW1000(spi, irq_mp, DW_CS_PINS[1]);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)

        DW1000& dw = *dw_array[i];

        wait(0.1)  ;

        dw.setEUI(0xFAEDCD01FAEDCD01 + i);
        wait(0.1)  ;
        // basic methods called to check if we have a working SPI connection
        node_array[i] = new MM2WayRanging(*dw_array[i]);

        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());


        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");
        }
        node.setInterrupt(false);

        /*uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + 0;
        node.requestRanging(remote_address);
        rangeAndDisplayAll(node, mb, timer);
        irq_mp.clear();
        wait(0.5);*/
    }

    wait(1);

    uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + 0;
    for (int i=0; i<2000; i++) {
        for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
            MM2WayRanging& node = *node_array[j];
            DW1000& dw = *dw_array[j];
            irq.rise(&dw, &DW1000::ISR);  // attach interrupt handler to rising edge of interrupt pin from DW1000
            node.setInterrupt(true);
            node.requestRanging(remote_address);
            rangeAndDisplayAll(node, mb, timer);
            node.setInterrupt(false);

        }
    }
//
//    MM2WayRanging& node = *node_array[0];
//    node.requestRanging(remote_address);
    // pc.printf("%d - %d> dist = %.4f\r\n", node.address, remote_address, node.distances[remote_address]);
    //rangeAndDisplayAll(node, mb, timer);

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

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

    while (true) {
    }
}