File content as of revision 12:152a51255a28:

#include "settings.h"

#if not BUILD_SLAVE

#include <stdarg.h>
#include <mbed.h>
#include <DW1000.h>
#include <DW1000Utils.h>
#include <ait_link/uwb_link/uwb_link_mbed.h>

#include <BufferedSerial/BufferedSerial.h>
#include "UWB2WayMultiRange.h"

enum CommMode {
    ASCII,
    UWBLINK
};

using ait::UWB2WayMultiRange;

//#define MEASURE_UWB_RANGING_RATE 1

using ait::UWBLink;
using ait::UWBLinkMbed;
using ait::UWBMessageString;
using ait::UWBMessageMultiRange;
using ait::UWBMessage;

// Overall reset rate (if no ranging is performed within the timeout)
const float WATCHDOG_TIMEOUT = 1.0f;

const int SPI_FREQUENCY = 5000000;
const float MIN_DW_VOLTAGE = 2.0f;

const int TRACKER_ADDRESS = 0;
const int NUM_OF_SLAVES = 1;
const int SLAVE_ADDRESS_OFFSET = 20;

const bool USE_NLOS_SETTINGS = true;

//const PinName DW_RESET_PIN = D15;
const PinName DW_RESET_PIN = NC;
const PinName DW_MOSI_PIN = D11;
const PinName DW_MISO_PIN = D12;
const PinName DW_SCLK_PIN = D13;

const int NUM_OF_DW_UNITS = 4;
// These CS pins are used on the Solo
const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D8, D9, D15, D14};
// These CS pins are used on the pyramid
//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7, D8, D9, D10};

const int MAX_UWB_LINK_FRAME_LENGTH = 1024;
#if _DEBUG
    CommMode comm_mode = ASCII;
#else
    CommMode comm_mode = UWBLINK;
#endif


BufferedSerial pc(USBTX, USBRX, 115200, 8 * 1024);           // USB UART Terminal

void send_status_message(UWBLink& ul, char* str, ...)
{
    va_list args;
    va_start(args, str);
    if (comm_mode == UWBLINK) {
        char buffer[MAX_UWB_LINK_FRAME_LENGTH];
        int n = vsnprintf(buffer, sizeof(buffer), str, args);
        if(n > sizeof(buffer)) {
            // Dangerous: Could lead to infinite recursion
            send_status_message(ul, "%s %d buffer to small (buf_size: %d, required: %d)!\r\n", __FILE__, __LINE__, sizeof(buffer), n);
        } else {
            UWBMessageString msg_str(buffer);
            UWBMessage msg(UWBMessage::UWB_MESSAGE_TYPE_STATUS, &msg_str);
            if (!ul.sendMessage(msg)) {
                DEBUG_PRINTF("\r\nSending UWBLink message failed\r\n");
            }
        }
    } else {
        pc.printf(str, args);
        pc.printf("\r\n");
    }
    va_end(args);
}

bool measureTimesOfFlight(UWB2WayMultiRange& tracker, UWBLink& ul, Timer& timer, float ranging_timeout = 0.1f)
{
#if _DEBUG
    int time_begin_us = timer.read_us();
#endif

#if MEASURE_UWB_RANGING_RATE
    static int32_t range_counter = 0;
    static uint32_t last_stamp_us = timer.read_us();
#endif

    bool any_success = false;
    for (int i = 0; i < NUM_OF_SLAVES; i++) {
        uint8_t remote_address = SLAVE_ADDRESS_OFFSET + i;
        const UWB2WayMultiRange::RawRangingResult& raw_result = tracker.measureTimesOfFlight(remote_address, ranging_timeout);
        if (raw_result.status == UWB2WayMultiRange::SUCCESS) {
            any_success = true;
            if (comm_mode == UWBLINK) {
                UWBMessageMultiRange msg_multi_range(
                    tracker.getAddress(),
                    remote_address
                );
                for (int j = 0; j < tracker.getNumOfModules(); ++j) {
                    msg_multi_range.addModuleMeasurement(raw_result.timestamp_master_request_1[j], raw_result.timestamp_slave_reply[j], raw_result.timestamp_master_request_2[j]);
                }
                msg_multi_range.setSlaveMeasurement(raw_result.timestamp_master_request_1_recv, raw_result.timestamp_slave_reply_send, raw_result.timestamp_master_request_2_recv);
                UWBMessage msg(UWBMessage::UWB_MESSAGE_TYPE_MULTI_RANGE, &msg_multi_range);
                if (!ul.sendMessage(msg)) {
                    ERROR_PRINTF("\r\nSending UWBLink message failed\r\n");
                }
            } else {
                for (int j = 0; j < tracker.getNumOfModules(); ++j) {
                    int64_t timediff_slave = raw_result.timestamp_master_request_1_recv + raw_result.timestamp_master_request_2_recv - 2 * raw_result.timestamp_slave_reply_send;
                    // Calculation of the summand on the sending node/beacon
                    int64_t timediff_master = 2 * raw_result.timestamp_slave_reply[j] - raw_result.timestamp_master_request_1[j] - raw_result.timestamp_master_request_2[j];
                    // Calculation of the resulting sum of all four ToFs.
                    int64_t timediff = timediff_master + timediff_slave;
                    float tof = tracker.convertDWTimeunitsToMicroseconds(timediff) / 4.0f;
                    float range = tracker.convertTimeOfFlightToDistance(tof);

                    send_status_message(ul, "%d.%d - %d> range = %.2f, tof = %.2e", tracker.getAddress(), j, remote_address, range, tof);
                }
            }
        } else {
            send_status_message(ul, "Ranging failed: %s - %s", UWB2WayMultiRange::RANGING_STATUS_MESSAGES[raw_result.status], raw_result.status_description);
        }
#if MEASURE_UWB_RANGING_RATE
        ++range_counter;
#endif
    }

#if MEASURE_UWB_RANGING_RATE
    uint32_t now_stamp_us = timer.read_us();
    uint32_t dt_us = now_stamp_us - last_stamp_us;
    if (dt_us > 2 * 1000 * 1000)
    {
        float rate = 1000 * 1000 * range_counter / ((float)dt_us);
        send_status_message(ul, "Rate = %f.2Hz", rate);
        send_status_message(ul, "range_counter = %d, stamp_us = %u, last_stamp_us = %u", range_counter, now_stamp_us, last_stamp_us);
        range_counter = 0;
        last_stamp_us = now_stamp_us;
    }
#endif

#if _DEBUG
    int time_end_us = timer.read_us();
    int time_elapsed_us = time_end_us - time_begin_us;
    int time_elapsed_ms = time_elapsed_us / 1000;
    DEBUG_PRINTF_VA("Time elapsed for ranging and output: %d ms (%d microseconds)\r\n", time_elapsed_ms, time_elapsed_us);
    DEBUG_PRINTF("\r\n\r\n");
#endif

#if _DEBUG
    ////wait_ms(500);
#endif

    return any_success;
}

int main()
{
    while (true) {
        // Ensure that all DW units are deactiviated at the beginning
        for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
            DigitalOut out(DW_CS_PINS[i]);
            out = 1;
        }
    
        UWBLinkMbed ul(&pc, MAX_UWB_LINK_FRAME_LENGTH);
 
        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)
    
        Timer timer;
        timer.start();
    
        DW1000* dw_array[NUM_OF_DW_UNITS];
        bool dw_init_success[NUM_OF_DW_UNITS];
        InterruptIn* irq = NULL;
    
        send_status_message(ul, "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);

        // Now we can initialize the DW modules
        for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
            dw_init_success[i] = true;
            dw_array[i] = new DW1000(spi, irq, DW_CS_PINS[i]);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
    
            DW1000& dw = *dw_array[i];
            uint64_t eui = 0xFAEDCD01FAEDCD01 + i;
            dw.setEUI(eui);                                  // basic methods called to check if we have a working SPI connection
            uint64_t query_eui = dw.getEUI();
            float query_voltage = dw.getVoltage();
            uint32_t query_device_id = dw.getDeviceID();
    
            send_status_message(ul, "\r\nUnit %d", i);
            send_status_message(ul, "\r\nDecaWave 1.0 up and running!");            // Splashscreen
            send_status_message(ul, "DEVICE_ID register: 0x%X", query_device_id);
            send_status_message(ul, "EUI register: %016llX", query_eui);
            send_status_message(ul, "Voltage: %.2fV", query_voltage);

            if (query_eui != eui) {
                send_status_message(ul, "ERROR: Queried EUI is not equal to the configured EUI");
                dw_init_success[i] = false;
                continue;
            }
    
            if (query_voltage < MIN_DW_VOLTAGE) {
                send_status_message(ul, "ERROR: Queried voltage is below minimum value");
                dw_init_success[i] = false;
                continue;
            }
    
            // Set NLOS settings (According to DecaWave Application Note APS006)
            if (USE_NLOS_SETTINGS) {
                send_status_message(ul, "Setting NLOS configuration for Unit %d", i);
                DW1000Utils::setNLOSSettings(&dw, DATA_RATE_SETTING, PRF_SETTING, PREAMBLE_SETTING);
            }
        }

        bool all_dw_initialized = true;
        for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
            if (!dw_init_success[i]) {
                all_dw_initialized = false;
                break;
            }
        }

        if (all_dw_initialized) {
            send_status_message(ul, "Initializing tracker with address %d", TRACKER_ADDRESS);
            UWB2WayMultiRange tracker(TRACKER_ADDRESS);
        
            for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
                tracker.addModule(dw_array[i]);
            }
        
            send_status_message(ul, "Resetting all modules and waiting 100ms");
            for (int i = 0; i < tracker.getNumOfModules(); ++i) {
                tracker.getModule(i)->softwareReset();
            }
            wait_us(100);       // This helps sometimes
        
            Timer watchdog_timer;
            watchdog_timer.start();
            float watchdog_last_time = watchdog_timer.read();
            bool watchdog_timeout_flag = false;
    
            send_status_message(ul, "Entering main loop");
            while (!watchdog_timeout_flag) {
                bool success = measureTimesOfFlight(tracker, ul, timer);
                float current_time = watchdog_timer.read();
                if (success) {
                    watchdog_last_time = current_time;
                } else {
                    float watchdog_time = current_time - watchdog_last_time;
                    if (watchdog_time > WATCHDOG_TIMEOUT) {
                        watchdog_timeout_flag = true;
                    }
                }
            }
            send_status_message(ul, "");
            send_status_message(ul, "Watchdog timed out ...");
            send_status_message(ul, "Restarting ...");
            send_status_message(ul, "");
        } else {
            send_status_message(ul, "\r\nNot all DW units could be initialized.");
            for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
                if (!dw_init_success[i]) {
                    send_status_message(ul, "Failed to initialize DW unit %d", i);
                }
            }
            send_status_message(ul, "Trying again ...");
            wait_ms(1000);
        }

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