Diff: main_multi_range.cpp

Revision:

6:9055fcc2f0cb

Parent:

5:1928eb8c417a

Child:

9:3844cf7e5c00

--- a/main_multi_range.cpp	Thu Mar 31 15:39:52 2016 +0000
+++ b/main_multi_range.cpp	Mon Apr 04 11:20:13 2016 +0000
@@ -7,14 +7,26 @@
 #include <DW1000.h>
 #include <DW1000Utils.h>
 #include <mavlink_bridge/mavlink_bridge.h>
+#include "uwb_link_mbed.h"
 
 #include "BufferedSerial.h"
 #include "UWB2WayMultiRange.h"
 
+enum CommMode {
+    ASCII,
+    MAVLINK,
+    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;
 using ait::MAVLinkBridge;
 using ait::UART_Mbed;
 
@@ -22,6 +34,7 @@
 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;
@@ -35,34 +48,57 @@
 const PinName DW_MISO_PIN = D12;
 const PinName DW_SCLK_PIN = D13;
 
+//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};
 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};
+//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D8, D9, D15, D14};
+const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D8, D9, D7, D10};
+
+const int MAVLINK_MAX_NUM_OF_DW_UNITS = 4;
+const int MAX_UWB_LINK_FRAME_LENGTH = 1024;
 #if _DEBUG
-    const bool MAVLINK_COMM = false;
+//    CommMode comm_mode = ASCII;
+    CommMode comm_mode = UWBLINK;
+//    CommMode comm_mode = MAVLINK;
 #else
-    const bool MAVLINK_COMM = true;
+//    CommMode comm_mode = MAVLINK;
+    CommMode comm_mode = UWBLINK;
 #endif
-const int MAVLINK_MAX_NUM_OF_DW_UNITS = 4;
 
 
 BufferedSerial pc(USBTX, USBRX, 115200, 8 * 1024);           // USB UART Terminal
 
-void send_status_message(MAVLinkBridge& mb, char* str, ...)
+void send_status_message(MAVLinkBridge& mb, UWBLink& ul, char* str, ...)
 {
     va_list args;
     va_start(args, str);
-    if (MAVLINK_COMM) {
+    if (comm_mode == MAVLINK) {
         char buffer[MAVLINK_MSG_UWB_STATUS_FIELD_DESCRIPTION_LEN];
         int n = vsnprintf(buffer, sizeof(buffer), str, args);
         if(n > sizeof(buffer)) {
-            send_status_message(mb, "%s %d buffer to small (buf_size: %d, required: %d)!\r\n", __FILE__, __LINE__, sizeof(buffer), n);
+            // Dangerous: Could lead to infinite recursion
+            send_status_message(mb, ul, "%s %d buffer to small (buf_size: %d, required: %d)!\r\n", __FILE__, __LINE__, sizeof(buffer), n);
         } else {
             // Pack and send message
             mavlink_message_t msg;
-            mavlink_msg_uwb_status_pack(mb.getSysId(), mb.getCompId(), &msg, -1, buffer);
+            mavlink_msg_uwb_status_pack(mb.getSysId(), mb.getCompId(), &msg, 0xFF, buffer);
             mb.sendMessage(msg);
         }
+    } else 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(mb, 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");
@@ -70,7 +106,7 @@
     va_end(args);
 }
 
-bool measureTimesOfFlight(UWB2WayMultiRange& tracker, MAVLinkBridge& mb, Timer& timer, float ranging_timeout = 0.1f)
+bool measureTimesOfFlight(UWB2WayMultiRange& tracker, MAVLinkBridge& mb, UWBLink& ul, Timer& timer, float ranging_timeout = 0.1f)
 {
 #if _DEBUG
     int time_begin_us = timer.read_us();
@@ -82,26 +118,21 @@
 #endif
 
     bool any_success = false;
-    for (int i = 0; i < NUM_OF_SLAVES; i++)
-    {
+    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)
-        {
+        if (raw_result.status == UWB2WayMultiRange::SUCCESS) {
             any_success = true;
-            if (MAVLINK_COMM)
-            {
+            if (comm_mode == MAVLINK) {
                 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)
-                {
+                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 < MAVLINK_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;
                     timestamp_master_request_2[j] = 0;
@@ -122,11 +153,21 @@
                     timestamp_master_request_2
                 );
                 mb.sendMessage(msg);
-            }
-            else
-            {
-                for (int j = 0; j < tracker.getNumOfModules(); ++j)
-                {
+            } else 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];
@@ -135,13 +176,11 @@
                     float tof = tracker.convertDWTimeunitsToMicroseconds(timediff) / 4.0f;
                     float range = tracker.convertTimeOfFlightToDistance(tof);
 
-                    send_status_message(mb, "%d.%d - %d> range = %.2f, tof = %.2e", tracker.getAddress(), j, remote_address, range, tof);
+                    send_status_message(mb, ul, "%d.%d - %d> range = %.2f, tof = %.2e", tracker.getAddress(), j, remote_address, range, tof);
                 }
             }
-        }
-        else
-        {
-            send_status_message(mb, "Ranging failed: %s - %s", UWB2WayMultiRange::RANGING_STATUS_MESSAGES[raw_result.status], raw_result.status_description);
+        } else {
+            send_status_message(mb, ul, "Ranging failed: %s - %s", UWB2WayMultiRange::RANGING_STATUS_MESSAGES[raw_result.status], raw_result.status_description);
         }
 #if MEASURE_UWB_RANGING_RATE
         ++range_counter;
@@ -154,8 +193,8 @@
     if (dt_us > 2 * 1000 * 1000)
     {
         float rate = 1000 * 1000 * range_counter / ((float)dt_us);
-        send_status_message(mb, "Rate = %f.2Hz", rate);
-        send_status_message(mb, "range_counter = %d, stamp_us = %u, last_stamp_us = %u", range_counter, now_stamp_us, last_stamp_us);
+        send_status_message(mb, ul, "Rate = %f.2Hz", rate);
+        send_status_message(mb, 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;
     }
@@ -170,7 +209,7 @@
 #endif
 
 #if _DEBUG
-    //wait_ms(500);
+    ////wait_ms(500);
 #endif
 
     return any_success;
@@ -188,9 +227,15 @@
     
         UART_Mbed uart(&pc);
         MAVLinkBridge mb(&uart);
-    
-        send_status_message(mb, "==== AIT UWB Multi Range ====");
-    
+        UWBLinkMbed ul(&pc, MAX_UWB_LINK_FRAME_LENGTH);
+ 
+//        while (true) {
+//        UWBMessage msg(UWBMessage::UWB_MESSAGE_TYPE_NOP);
+//        if (!ul.sendMessage(msg)) {
+//            ERROR_PRINTF("\r\nSending UWBLink message failed\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.
@@ -200,74 +245,106 @@
         timer.start();
     
         DW1000* dw_array[NUM_OF_DW_UNITS];
+        bool dw_init_success[NUM_OF_DW_UNITS];
         InterruptIn* irq = NULL;
     
-        send_status_message(mb, "Performing hardware reset of UWB modules\r\n");
+        send_status_message(mb, 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)
-        {
+        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];
-            dw.setEUI(0xFAEDCD01FAEDCD01 + i);                                  // basic methods called to check if we have a working SPI connection
+            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(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, ul, "\r\nUnit %d", i);
+            send_status_message(mb, ul, "\r\nDecaWave 1.0 up and running!");            // Splashscreen
+            send_status_message(mb, ul, "DEVICE_ID register: 0x%X", query_device_id);
+            send_status_message(mb, ul, "EUI register: %016llX", query_eui);
+            send_status_message(mb, ul, "Voltage: %.2fV", query_voltage);
+
+            if (query_eui != eui) {
+                send_status_message(mb, 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(mb, 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(mb, "Setting NLOS configuration for Unit %d", i);
+            if (USE_NLOS_SETTINGS) {
+                send_status_message(mb, ul, "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]);
+
+        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;
+            }
         }
-    
-        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
+
+        if (all_dw_initialized) {
+            send_status_message(mb, 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(mb, 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;
     
-        Timer watchdog_timer;
-        watchdog_timer.start();
-        float watchdog_last_time = watchdog_timer.read();
-        bool watchdog_timeout_flag = false;
-
-        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, ul, "Entering main loop");
+            while (!watchdog_timeout_flag) {
+                bool success = measureTimesOfFlight(tracker, mb, 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(mb, ul, "");
+            send_status_message(mb, ul, "Watchdog timed out ...");
+            send_status_message(mb, ul, "Restarting ...");
+            send_status_message(mb, ul, "");
+        } else {
+            send_status_message(mb, 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(mb, ul, "Failed to initialize DW unit %d", i);
+                }
+            }
+            send_status_message(mb, ul, "Trying again ...");
+            wait_ms(1000);
         }
-        send_status_message(mb, "");
-        send_status_message(mb, "Watchdog timed out ...");
-        send_status_message(mb, "Restarting ...");
-        send_status_message(mb, "");
 
-        for (int i = 0; i < NUM_OF_DW_UNITS; ++i)
-        {
+        for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
             delete dw_array[i];
         }
     }