Diff: main_multi_range.cpp

Revision:

5:1928eb8c417a

Parent:

4:1a2c1e5e5516

Child:

6:9055fcc2f0cb

--- a/main_multi_range.cpp	Tue Mar 29 10:00:43 2016 +0000
+++ b/main_multi_range.cpp	Thu Mar 31 15:39:52 2016 +0000
@@ -18,7 +18,10 @@
 using ait::MAVLinkBridge;
 using ait::UART_Mbed;
 
-const int SPI_FREQUENCY = 1000000;
+// Overall reset rate (if no ranging is performed within the timeout)
+const float WATCHDOG_TIMEOUT = 1.0f;
+
+const int SPI_FREQUENCY = 5000000;
 
 const int TRACKER_ADDRESS = 0;
 const int NUM_OF_SLAVES = 1;
@@ -26,23 +29,21 @@
 
 const bool USE_NLOS_SETTINGS = true;
 
-const PinName DW_RESET_PIN = D15;
+//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 MAX_NUM_OF_DW_UNITS = 4;
+const int NUM_OF_DW_UNITS = 4;
+//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7, D8, D9, D10};
+const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D8, D9, D14, D15};
 #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 = true;
-    const int NUM_OF_DW_UNITS = 4;
-    const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7, D8, D9, D10};
 #endif
+const int MAVLINK_MAX_NUM_OF_DW_UNITS = 4;
 
 
 BufferedSerial pc(USBTX, USBRX, 115200, 8 * 1024);           // USB UART Terminal
@@ -69,7 +70,7 @@
     va_end(args);
 }
 
-void measureTimesOfFlight(UWB2WayMultiRange& tracker, MAVLinkBridge& mb, Timer& timer, float ranging_timeout = 0.1f)
+bool measureTimesOfFlight(UWB2WayMultiRange& tracker, MAVLinkBridge& mb, Timer& timer, float ranging_timeout = 0.1f)
 {
 #if _DEBUG
     int time_begin_us = timer.read_us();
@@ -80,24 +81,26 @@
     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 (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];
+                uint64_t timestamp_master_request_1[MAVLINK_MAX_NUM_OF_DW_UNITS];
+                uint64_t timestamp_slave_reply[MAVLINK_MAX_NUM_OF_DW_UNITS];
+                uint64_t timestamp_master_request_2[MAVLINK_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)
+                for (int j = tracker.getNumOfModules(); j < MAVLINK_MAX_NUM_OF_DW_UNITS; ++j)
                 {
                     timestamp_master_request_1[j] = 0;
                     timestamp_slave_reply[j] = 0;
@@ -167,77 +170,106 @@
 #endif
 
 #if _DEBUG
-    wait_ms(1000);
+    //wait_ms(500);
 #endif
+
+    return any_success;
 }
 
 int main()
 {
-    UART_Mbed uart(&pc);
-    MAVLinkBridge mb(&uart);
-
-    send_status_message(mb, "==== AIT UWB Multi Range ====");
-
-    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);
+    while (true) {
 
-    send_status_message(mb, "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
+        const PinName CS_PINS[5] = {D8, D9, D10, D14, D15};
+        for (int i = 0; i < sizeof(CS_PINS) / sizeof(PinName); ++i) {
+            DigitalOut out(CS_PINS[i]);
+            out = 1;
+        }
+    
+        UART_Mbed uart(&pc);
+        MAVLinkBridge mb(&uart);
+    
+        send_status_message(mb, "==== AIT UWB Multi Range ====");
+    
+        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];
+        InterruptIn* irq = NULL;
+    
+        send_status_message(mb, "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
+    
+            send_status_message(mb, "\r\nUnit %d", i);
+            send_status_message(mb, "\r\nDecaWave 1.0 up and running!");            // Splashscreen
+            send_status_message(mb, "DEVICE_ID register: 0x%X", dw.getDeviceID());
+            send_status_message(mb, "EUI register: %016llX", dw.getEUI());
+            send_status_message(mb, "Voltage: %.2fV", dw.getVoltage());
+    
+            // Set NLOS settings (According to DecaWave Application Note APS006)
+            if (USE_NLOS_SETTINGS)
+            {
+                send_status_message(mb, "Setting NLOS configuration for Unit %d", i);
+                DW1000Utils::setNLOSSettings(&dw, DATA_RATE_SETTING, PRF_SETTING, PREAMBLE_SETTING);
+            }
+        }
+    
+        send_status_message(mb, "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(mb, "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(mb, "\r\nUnit %d", i);
-        send_status_message(mb, "\r\nDecaWave 1.0 up and running!");            // Splashscreen
-        send_status_message(mb, "DEVICE_ID register: 0x%X", dw.getDeviceID());
-        send_status_message(mb, "EUI register: %016llX", dw.getEUI());
-        send_status_message(mb, "Voltage: %.2fV", dw.getVoltage());
+        send_status_message(mb, "Entering main loop");
+        while (!watchdog_timeout_flag)
+        {
+            bool success = measureTimesOfFlight(tracker, mb, 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(mb, "");
+        send_status_message(mb, "Watchdog timed out ...");
+        send_status_message(mb, "Restarting ...");
+        send_status_message(mb, "");
 
-        // Set NLOS settings (According to DecaWave Application Note APS006)
-        if (USE_NLOS_SETTINGS)
+        for (int i = 0; i < NUM_OF_DW_UNITS; ++i)
         {
-            send_status_message(mb, "Setting NLOS configuration for Unit %d", i);
-            DW1000Utils::setNLOSSettings(&dw, DATA_RATE_SETTING, PRF_SETTING, PREAMBLE_SETTING);
+            delete dw_array[i];
         }
     }
-
-    send_status_message(mb, "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(mb, "Entering main loop");
-
-    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