Diff: main_multi_range.cpp

Revision:

2:5af0f0006f40

Parent:

1:c070ca30da80

Child:

4:1a2c1e5e5516

--- a/main_multi_range.cpp	Thu Feb 11 10:49:49 2016 +0000
+++ b/main_multi_range.cpp	Sat Feb 13 17:11:04 2016 +0000
@@ -2,15 +2,16 @@
 
 #if not BUILD_SLAVE
 
+#include <stdarg.h>
 #include <mbed.h>
 #include <DW1000.h>
 #include <DW1000Utils.h>
 #include <mavlink_bridge/mavlink_bridge.h>
 
-#include "PC.h"
-#include "UWBMultiRange.h"
+#include "BufferedSerial.h"
+#include "UWB2WayMultiRange.h"
 
-using ait::UWBMultiRange;
+using ait::UWB2WayMultiRange;
 
 //#define MEASURE_UWB_RANGING_RATE 1
 
@@ -19,7 +20,7 @@
 
 const int SPI_FREQUENCY = 1000000;
 
-const int TRACKER_ADDRESS = 20;
+const int TRACKER_ADDRESS = 0;
 const int NUM_OF_SLAVES = 1;
 const int SLAVE_ADDRESS_OFFSET = 10;
 
@@ -38,17 +39,42 @@
     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 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
 
 
-PC pc(USBTX, USBRX, 115200);           // USB UART Terminal
-
+BufferedSerial pc(USBTX, USBRX, 115200, 8 * 1024);           // USB UART Terminal
 
-void measureTimesOfFlight(UWBMultiRange& tracker, MAVLinkBridge& mb, Timer& timer, float ranging_timeout = 0.2f)
+void send_status_message(MAVLinkBridge& mb, char* str, ...)
 {
+    va_list args;
+    va_start(args, str);
+    if (MAVLINK_COMM) {
+        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);
+        } else {
+            // Pack and send message
+            mavlink_message_t msg;
+            mavlink_msg_uwb_status_pack(mb.getSysId(), mb.getCompId(), &msg, -1, buffer);
+            mb.sendMessage(msg);
+        }
+    } else {
+        pc.printf(str, args);
+        pc.printf("\r\n");
+    }
+    va_end(args);
+}
+
+void measureTimesOfFlight(UWB2WayMultiRange& tracker, MAVLinkBridge& mb, 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();
@@ -57,8 +83,8 @@
     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)
+        const UWB2WayMultiRange::RawRangingResult& raw_result = tracker.measureTimesOfFlight(remote_address, ranging_timeout);
+        if (raw_result.status == UWB2WayMultiRange::SUCCESS)
         {
             if (MAVLINK_COMM)
             {
@@ -78,10 +104,9 @@
                     timestamp_master_request_2[j] = 0;
                 }
 
-                // Initialize the required buffers
+                // Pack and send message
                 mavlink_message_t msg;
-                // Pack the message
-                mavlink_msg_uwb_tracker_raw_4_pack(
+                mavlink_msg_uwb_2way_multi_range_raw_4_pack(
                     mb.getSysId(), mb.getCompId(), &msg,
                     tracker.getNumOfModules(),
                     tracker.getAddress(),
@@ -107,16 +132,13 @@
                     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);
+                    send_status_message(mb, "%d.%d - %d> range = %.2f, tof = %.2e", 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]);
-            }
+            send_status_message(mb, "Ranging failed: %s - %s", UWB2WayMultiRange::RANGING_STATUS_MESSAGES[raw_result.status], raw_result.status_description);
         }
 #if MEASURE_UWB_RANGING_RATE
         ++range_counter;
@@ -129,23 +151,32 @@
     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);
+        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);
         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(1000);
+#endif
 }
 
 int main()
 {
-    UART_Mbed uart(USBTX, USBRX, 115200);
+    UART_Mbed uart(&pc);
     MAVLinkBridge mb(&uart);
 
-    if (!MAVLINK_COMM)
-    {
-        pc.printf("==== AIT UWB Multi Range ====\r\n");
-    }
+    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)
@@ -159,10 +190,7 @@
     PinName irq_pin = NC;
     InterruptIn irq(irq_pin);
 
-    if (!MAVLINK_COMM)
-    {
-        pc.printf("Performing hardware reset of UWB modules\r\n");
-    }
+    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);
 
@@ -174,42 +202,29 @@
         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());
-        }
+        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)
         {
-            if (!MAVLINK_COMM)
-            {
-                pc.printf("Setting NLOS configuration for Unit %d\r\n", i);
-            }
+            send_status_message(mb, "Setting NLOS configuration for Unit %d", 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);
+    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]);
     }
 
-    if (!MAVLINK_COMM)
-    {
-        pc.printf("Entering main loop\r\n");
-    }
+    send_status_message(mb, "Entering main loop");
 
     while (true)
     {