Diff: main_multi_range.cpp

Revision:

10:6e95128c2efa

Parent:

9:3844cf7e5c00

Child:

12:152a51255a28

--- a/main_multi_range.cpp	Tue Apr 05 12:20:29 2016 +0000
+++ b/main_multi_range.cpp	Wed Apr 06 08:23:26 2016 +0000
@@ -6,15 +6,13 @@
 #include <mbed.h>
 #include <DW1000.h>
 #include <DW1000Utils.h>
-#include <mavlink_bridge/mavlink_bridge.h>
-#include "uwb_link_mbed.h"
+#include <ait_link/uwb_link/uwb_link_mbed.h>
 
-#include "BufferedSerial.h"
+#include <BufferedSerial/BufferedSerial.h>
 #include "UWB2WayMultiRange.h"
 
 enum CommMode {
     ASCII,
-    MAVLINK,
     UWBLINK
 };
 
@@ -27,8 +25,6 @@
 using ait::UWBMessageString;
 using ait::UWBMessageMultiRange;
 using ait::UWBMessage;
-using ait::MAVLinkBridge;
-using ait::UART_Mbed;
 
 // Overall reset rate (if no ranging is performed within the timeout)
 const float WATCHDOG_TIMEOUT = 1.0f;
@@ -54,40 +50,26 @@
 // These CS pins are used on the pyramid
 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7, D8, D9, D10};
 
-const int MAVLINK_MAX_NUM_OF_DW_UNITS = 4;
 const int MAX_UWB_LINK_FRAME_LENGTH = 1024;
 #if _DEBUG
     CommMode comm_mode = ASCII;
 #else
-//    CommMode comm_mode = MAVLINK;
     CommMode comm_mode = UWBLINK;
 #endif
 
 
 BufferedSerial pc(USBTX, USBRX, 115200, 8 * 1024);           // USB UART Terminal
 
-void send_status_message(MAVLinkBridge& mb, UWBLink& ul, char* str, ...)
+void send_status_message(UWBLink& ul, char* str, ...)
 {
     va_list args;
     va_start(args, str);
-    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)) {
-            // 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, 0xFF, buffer);
-            mb.sendMessage(msg);
-        }
-    } else if (comm_mode == UWBLINK) {
+    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);
+            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);
@@ -102,7 +84,7 @@
     va_end(args);
 }
 
-bool measureTimesOfFlight(UWB2WayMultiRange& tracker, MAVLinkBridge& mb, UWBLink& ul, Timer& timer, float ranging_timeout = 0.1f)
+bool measureTimesOfFlight(UWB2WayMultiRange& tracker, UWBLink& ul, Timer& timer, float ranging_timeout = 0.1f)
 {
 #if _DEBUG
     int time_begin_us = timer.read_us();
@@ -119,37 +101,7 @@
         const UWB2WayMultiRange::RawRangingResult& raw_result = tracker.measureTimesOfFlight(remote_address, ranging_timeout);
         if (raw_result.status == UWB2WayMultiRange::SUCCESS) {
             any_success = true;
-            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) {
-                    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) {
-                    timestamp_master_request_1[j] = 0;
-                    timestamp_slave_reply[j] = 0;
-                    timestamp_master_request_2[j] = 0;
-                }
-
-                // Pack and send message
-                mavlink_message_t msg;
-                mavlink_msg_uwb_2way_multi_range_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 if (comm_mode == UWBLINK) {
+            if (comm_mode == UWBLINK) {
                 UWBMessageMultiRange msg_multi_range(
                     tracker.getAddress(),
                     remote_address
@@ -172,11 +124,11 @@
                     float tof = tracker.convertDWTimeunitsToMicroseconds(timediff) / 4.0f;
                     float range = tracker.convertTimeOfFlightToDistance(tof);
 
-                    send_status_message(mb, ul, "%d.%d - %d> range = %.2f, tof = %.2e", tracker.getAddress(), j, remote_address, range, tof);
+                    send_status_message(ul, "%d.%d - %d> range = %.2f, tof = %.2e", tracker.getAddress(), j, remote_address, range, tof);
                 }
             }
         } else {
-            send_status_message(mb, ul, "Ranging failed: %s - %s", UWB2WayMultiRange::RANGING_STATUS_MESSAGES[raw_result.status], raw_result.status_description);
+            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;
@@ -189,8 +141,8 @@
     if (dt_us > 2 * 1000 * 1000)
     {
         float rate = 1000 * 1000 * range_counter / ((float)dt_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);
+        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;
     }
@@ -221,8 +173,6 @@
             out = 1;
         }
     
-        UART_Mbed uart(&pc);
-        MAVLinkBridge mb(&uart);
         UWBLinkMbed ul(&pc, MAX_UWB_LINK_FRAME_LENGTH);
  
 //        while (true) {
@@ -244,7 +194,7 @@
         bool dw_init_success[NUM_OF_DW_UNITS];
         InterruptIn* irq = NULL;
     
-        send_status_message(mb, ul, "Performing hardware reset of UWB modules\r\n");
+        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);
 
@@ -260,27 +210,27 @@
             float query_voltage = dw.getVoltage();
             uint32_t query_device_id = dw.getDeviceID();
     
-            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);
+            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(mb, ul, "ERROR: Queried EUI is not equal to the configured 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(mb, ul, "ERROR: Queried voltage is below minimum value");
+                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(mb, ul, "Setting NLOS configuration for Unit %d", i);
+                send_status_message(ul, "Setting NLOS configuration for Unit %d", i);
                 DW1000Utils::setNLOSSettings(&dw, DATA_RATE_SETTING, PRF_SETTING, PREAMBLE_SETTING);
             }
         }
@@ -294,14 +244,14 @@
         }
 
         if (all_dw_initialized) {
-            send_status_message(mb, ul, "Initializing tracker with address %d", TRACKER_ADDRESS);
+            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(mb, ul, "Resetting all modules and waiting 100ms");
+            send_status_message(ul, "Resetting all modules and waiting 100ms");
             for (int i = 0; i < tracker.getNumOfModules(); ++i) {
                 tracker.getModule(i)->softwareReset();
             }
@@ -312,9 +262,9 @@
             float watchdog_last_time = watchdog_timer.read();
             bool watchdog_timeout_flag = false;
     
-            send_status_message(mb, ul, "Entering main loop");
+            send_status_message(ul, "Entering main loop");
             while (!watchdog_timeout_flag) {
-                bool success = measureTimesOfFlight(tracker, mb, ul, timer);
+                bool success = measureTimesOfFlight(tracker, ul, timer);
                 float current_time = watchdog_timer.read();
                 if (success) {
                     watchdog_last_time = current_time;
@@ -325,18 +275,18 @@
                     }
                 }
             }
-            send_status_message(mb, ul, "");
-            send_status_message(mb, ul, "Watchdog timed out ...");
-            send_status_message(mb, ul, "Restarting ...");
-            send_status_message(mb, ul, "");
+            send_status_message(ul, "");
+            send_status_message(ul, "Watchdog timed out ...");
+            send_status_message(ul, "Restarting ...");
+            send_status_message(ul, "");
         } else {
-            send_status_message(mb, ul, "\r\nNot all DW units could be initialized.");
+            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(mb, ul, "Failed to initialize DW unit %d", i);
+                    send_status_message(ul, "Failed to initialize DW unit %d", i);
                 }
             }
-            send_status_message(mb, ul, "Trying again ...");
+            send_status_message(ul, "Trying again ...");
             wait_ms(1000);
         }