Diff: main.cpp

Revision:

50:50b8aea54a51

Parent:

48:5999e510f154

Child:

51:e9391d04af00

--- a/main.cpp	Tue Nov 24 16:43:13 2015 +0000
+++ b/main.cpp	Thu Nov 26 21:42:51 2015 +0000
@@ -12,6 +12,7 @@
 using namespace ait;
 
 //const int ANCHOR_ADDRESS_OFFSET = 100;
+const int SPI_FREQUENCY = 1000000;
 const int ANCHOR_ADDRESS_OFFSET = 1;
 const bool USE_NLOS_SETTINGS = true;
 const PinName DW_RESET_PIN = D15;
@@ -27,8 +28,7 @@
 //    const bool MAVLINK_COMM = true;
     const bool MAVLINK_COMM = false;
     const int NUM_OF_DW_UNITS = 2;
-    const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10, D9};
-    const PinName DW_IRQ_PINS[NUM_OF_DW_UNITS] = {D14, D6};
+    const PinName DW_CS_PINS[2] = {D10, D9};
     //const int NUM_OF_DW_UNITS = 1;
     //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
 #endif
@@ -97,8 +97,8 @@
             mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise3", std_noise3);
             mb.sendMessage(msg);*/
         } else {
-            pc.printf("^R>%f, ", node.distances[i]);
-            pc.printf("T:%f", node.roundtriptimes[i]);
+            pc.printf("%d> dist = >%f, ", node.address, node.distances[i]);
+            pc.printf("%d> rtt = %f", node.address, node.roundtriptimes[i]);
             pc.printf("\r\n");
         }
     }
@@ -169,19 +169,36 @@
 void altCallbackRX();
 
 int main() {
-    // Setup interrupt pin
-    InterruptMultiplexer irq_mp;
-    InterruptIn irq(DW_IRQ_PIN);
-    irq.rise(&irq_mp, &InterruptMultiplexer::trigger);  // attach interrupt handler to rising edge of interrupt pin from DW1000
+    DigitalOut cs(D8);
+    cs = 1;
+
+    UART_Mbed uart(USBTX, USBRX, 115200);
+    MAVLinkBridge mb(&uart);
+
+    if (!MAVLINK_COMM) {
+        pc.printf("==== AIT UWB 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)
+    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(1000000);             // with a 1MHz clock rate (worked up to 49MHz in our Test)
+    spi.frequency(SPI_FREQUENCY);             // with a 1MHz clock rate (worked up to 49MHz in our Test)
+
+    // Setup interrupt pin
+    InterruptMultiplexer irq_mp(DW_IRQ_PIN);
+    irq_mp.getIRQ().rise(&irq_mp, &InterruptMultiplexer::trigger);  // attach interrupt handler to rising edge of interrupt pin from DW1000
+
+    Timer timer;
+    timer.start();
+
+    while (true) {
 
     DW1000* dw_array[NUM_OF_DW_UNITS + 2];
     MM2WayRanging* node_array[NUM_OF_DW_UNITS + 2];                           // Instance of the two way ranging algorithm
 
+    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);
     for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
@@ -201,16 +218,10 @@
             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: %fV\r\n", dw.getVoltage());
+            pc.printf("Voltage: %.2fV\r\n", dw.getVoltage());
         }
     }
 
-    Timer timer;
-    timer.start();
-
-    UART_Mbed uart(USBTX, USBRX, 115200);
-    MAVLinkBridge mb(&uart);
-
     for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
         DW1000& dw = *dw_array[i];
         MM2WayRanging& node = *node_array[i];
@@ -222,13 +233,15 @@
         } else {
             node.address = i;
             if (!MAVLINK_COMM)
-                pc.printf("This node is a Beacon.\r\n ");
+                pc.printf("This node is a Beacon\r\n");
         }
     }
 
     // Set NLOS settings
     if (USE_NLOS_SETTINGS) {
         for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
+            if (!MAVLINK_COMM)
+                pc.printf("Setting NLOS configuration for Unit %d\r\n", i);
             DW1000& dw = *dw_array[i];
             dw.writeRegister8(DW1000_LDE_CTRL, 0x0806, 0x07);              //LDE_CFG1
             dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x0003);           //LDE_CFG2
@@ -243,7 +256,10 @@
         }
     }
 
+    if (!MAVLINK_COMM)
+        pc.printf("Entering main loop\r\n");
     while (true) {
+    //for (int i = 0; i < 10; ++i) {
         if (ANCHOR) {
             pc.printf("."); // to see if the core and output is working
             wait(0.5);
@@ -256,6 +272,13 @@
             //calibrationRanging(4);
         }
     }
+
+    for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
+        delete node_array[i];
+        delete dw_array[i];
+    }
+
+    }
 }