Diff: main_multi_range.cpp

Revision:

1:c070ca30da80

Child:

2:5af0f0006f40

diff -r 6d63b6992cbf -r c070ca30da80 main_multi_range.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_multi_range.cpp	Thu Feb 11 10:49:49 2016 +0000
@@ -0,0 +1,228 @@
+#include "settings.h"
+
+#if not BUILD_SLAVE
+
+#include <mbed.h>
+#include <DW1000.h>
+#include <DW1000Utils.h>
+#include <mavlink_bridge/mavlink_bridge.h>
+
+#include "PC.h"
+#include "UWBMultiRange.h"
+
+using ait::UWBMultiRange;
+
+//#define MEASURE_UWB_RANGING_RATE 1
+
+using ait::MAVLinkBridge;
+using ait::UART_Mbed;
+
+const int SPI_FREQUENCY = 1000000;
+
+const int TRACKER_ADDRESS = 20;
+const int NUM_OF_SLAVES = 1;
+const int SLAVE_ADDRESS_OFFSET = 10;
+
+const bool USE_NLOS_SETTINGS = true;
+
+const PinName DW_RESET_PIN = D15;
+const PinName DW_MOSI_PIN = D11;
+const PinName DW_MISO_PIN = D12;
+const PinName DW_SCLK_PIN = D13;
+
+const int MAX_NUM_OF_DW_UNITS = 4;
+#if _DEBUG
+    const bool MAVLINK_COMM = false;
+//    const int NUM_OF_DW_UNITS = 1;
+//    const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
+    const int NUM_OF_DW_UNITS = 4;
+    const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7, D8, D9, D10};
+#else
+    const bool MAVLINK_COMM = false;
+    const int NUM_OF_DW_UNITS = 4;
+    const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7, D8, D9, D10};
+#endif
+
+
+PC pc(USBTX, USBRX, 115200);           // USB UART Terminal
+
+
+void measureTimesOfFlight(UWBMultiRange& tracker, MAVLinkBridge& mb, Timer& timer, float ranging_timeout = 0.2f)
+{
+#if MEASURE_UWB_RANGING_RATE
+    static int32_t range_counter = 0;
+    static uint32_t last_stamp_us = timer.read_us();
+#endif
+
+    for (int i = 0; i < NUM_OF_SLAVES; i++)
+    {
+        uint8_t remote_address = SLAVE_ADDRESS_OFFSET + i;
+        const UWBMultiRange::RawRangingResult& raw_result = tracker.measureTimesOfFlight(remote_address, ranging_timeout);
+        if (raw_result.status == UWBMultiRange::SUCCESS)
+        {
+            if (MAVLINK_COMM)
+            {
+                uint64_t timestamp_master_request_1[MAX_NUM_OF_DW_UNITS];
+                uint64_t timestamp_slave_reply[MAX_NUM_OF_DW_UNITS];
+                uint64_t timestamp_master_request_2[MAX_NUM_OF_DW_UNITS];
+                for (int j = 0; j < tracker.getNumOfModules(); ++j)
+                {
+                    timestamp_master_request_1[j] = raw_result.timestamp_master_request_1[j];
+                    timestamp_slave_reply[j] = raw_result.timestamp_slave_reply[j];
+                    timestamp_master_request_2[j] = raw_result.timestamp_master_request_2[j];
+                }
+                for (int j = tracker.getNumOfModules(); j < MAX_NUM_OF_DW_UNITS; ++j)
+                {
+                    timestamp_master_request_1[j] = 0;
+                    timestamp_slave_reply[j] = 0;
+                    timestamp_master_request_2[j] = 0;
+                }
+
+                // Initialize the required buffers
+                mavlink_message_t msg;
+                // Pack the message
+                mavlink_msg_uwb_tracker_raw_4_pack(
+                    mb.getSysId(), mb.getCompId(), &msg,
+                    tracker.getNumOfModules(),
+                    tracker.getAddress(),
+                    remote_address,
+                    raw_result.timestamp_master_request_1_recv,
+                    raw_result.timestamp_slave_reply_send,
+                    raw_result.timestamp_master_request_2_recv,
+                    timestamp_master_request_1,
+                    timestamp_slave_reply,
+                    timestamp_master_request_2
+                );
+                mb.sendMessage(msg);
+            }
+            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);
+
+                    pc.printf("%d.%d - %d> range = %.2f, tof = %.2e\r\n", tracker.getAddress(), j, remote_address, range, tof);
+                }
+            }
+        }
+        else
+        {
+            if (!MAVLINK_COMM)
+            {
+                pc.printf("Ranging failed: %s\r\n", UWBMultiRange::RANGING_STATUS_MESSAGES[raw_result.status]);
+            }
+        }
+#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);
+        pc.printf("Rate = %f.2Hz\r\n", rate);
+        pc.printf("range_counter = %d, stamp_us = %u, last_stamp_us = %u\r\n", range_counter, now_stamp_us, last_stamp_us);
+        range_counter = 0;
+        last_stamp_us = now_stamp_us;
+    }
+#endif
+}
+
+int main()
+{
+    UART_Mbed uart(USBTX, USBRX, 115200);
+    MAVLinkBridge mb(&uart);
+
+    if (!MAVLINK_COMM)
+    {
+        pc.printf("==== AIT UWB Multi 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)
+
+    Timer timer;
+    timer.start();
+
+    DW1000* dw_array[NUM_OF_DW_UNITS];
+    PinName irq_pin = NC;
+    InterruptIn irq(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);
+
+    // Now we can initialize the DW modules
+    for (int i = 0; i < NUM_OF_DW_UNITS; ++i)
+    {
+        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];
+        dw.setEUI(0xFAEDCD01FAEDCD01 + i);                                  // basic methods called to check if we have a working SPI connection
+
+        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());
+        }
+
+        // Set NLOS settings (According to DecaWave Application Note APS006)
+        if (USE_NLOS_SETTINGS)
+        {
+            if (!MAVLINK_COMM)
+            {
+                pc.printf("Setting NLOS configuration for Unit %d\r\n", i);
+            }
+            DW1000Utils::setNLOSSettings(&dw, DATA_RATE_SETTING, PRF_SETTING, PREAMBLE_SETTING);
+        }
+    }
+
+    pc.printf("Initializing tracker with address %d\r\n", TRACKER_ADDRESS);
+    UWBMultiRange tracker(TRACKER_ADDRESS);
+
+    for (int i = 0; i < NUM_OF_DW_UNITS; ++i)
+    {
+        tracker.addModule(dw_array[i]);
+    }
+
+    if (!MAVLINK_COMM)
+    {
+        pc.printf("Entering main loop\r\n");
+    }
+
+    while (true)
+    {
+        for (int j = 0; j < NUM_OF_DW_UNITS; ++j)
+        {
+            measureTimesOfFlight(tracker, mb, timer);
+        }
+    }
+
+//    for (int i = 0; i < NUM_OF_DW_UNITS; ++i)
+//    {
+//        delete node_array[i];
+//        delete dw_array[i];
+//    }
+}
+#endif