Diff: main.cpp

Revision:

52:94688f414dcd

Parent:

51:e9391d04af00

Child:

53:79a72d752ec4

--- a/main.cpp	Thu Nov 26 22:24:17 2015 +0000
+++ b/main.cpp	Fri Nov 27 16:17:53 2015 +0000
@@ -11,9 +11,9 @@
 
 using namespace ait;
 
-//const int ANCHOR_ADDRESS_OFFSET = 100;
 const int SPI_FREQUENCY = 1000000;
-const int ANCHOR_ADDRESS_OFFSET = 1;
+const int NUM_OF_ANCHORS = 1;
+const int ANCHOR_ADDRESS_OFFSET = 10;
 const bool USE_NLOS_SETTINGS = true;
 const PinName DW_RESET_PIN = D15;
 const PinName DW_IRQ_PIN = D14;
@@ -25,53 +25,54 @@
     const int NUM_OF_DW_UNITS = 1;
     const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
 #else
-    const bool MAVLINK_COMM = true;
-//    const bool MAVLINK_COMM = false;
-    const int NUM_OF_DW_UNITS = 2;
-    const PinName DW_CS_PINS[2] = {D10, D9};
+//    const bool MAVLINK_COMM = true;
+    const bool MAVLINK_COMM = false;
+    const int NUM_OF_DW_UNITS = 3;
+    const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10, D9, D8};
 #endif
 
 PC pc(USBTX, USBRX, 115200);           // USB UART Terminal
 
 void rangeAndDisplayAll(MM2WayRanging& node, MAVLinkBridge& mb, Timer& timer) {
-    node.requestRanging(ANCHOR_ADDRESS_OFFSET);                       // Request ranging to all anchors
-    for (int i = 1; i <= 1; i++) {                  // Output Results
+    for (int i = 0; i < NUM_OF_ANCHORS; i++) {
+        uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + i;
+        node.requestRanging(remote_address);
+
         if (MAVLINK_COMM) {
             uint8_t address = node.address;
-            uint8_t remote_address = i;
             uint32_t stamp_us = timer.read_us();
-            float round_trip_time = node.roundtriptimes[i];
-            float range = node.distances[i];
+            float round_trip_time = node.roundtriptimes[remote_address];
+            float range = node.distances[remote_address];
             // Initialize the required buffers
             mavlink_message_t msg;
             // Pack the message
             mavlink_msg_uwb_range_pack(mb.getSysId(), mb.getCompId(), &msg, address, remote_address, stamp_us, round_trip_time, range);
             mb.sendMessage(msg);
 
-            uint16_t std_noise_1 = node.reception_stats[i][0].std_noise;
-            uint16_t std_noise_2 = node.reception_stats[i][1].std_noise;
-            uint16_t std_noise_3 = node.reception_stats[i][2].std_noise;
-            uint16_t preamble_acc_count_1 = node.reception_stats[i][0].preamble_acc_count;
-            uint16_t preamble_acc_count_2 = node.reception_stats[i][1].preamble_acc_count;
-            uint16_t preamble_acc_count_3 = node.reception_stats[i][2].preamble_acc_count;
-            uint16_t first_path_index_1 = node.reception_stats[i][0].first_path_index;
-            uint16_t first_path_index_2 = node.reception_stats[i][1].first_path_index;
-            uint16_t first_path_index_3 = node.reception_stats[i][2].first_path_index;
-            uint16_t first_path_amp_1_1 = node.reception_stats[i][0].first_path_amp_1;
-            uint16_t first_path_amp_1_2 = node.reception_stats[i][1].first_path_amp_1;
-            uint16_t first_path_amp_1_3 = node.reception_stats[i][2].first_path_amp_1;
-            uint16_t first_path_amp_2_1 = node.reception_stats[i][0].first_path_amp_2;
-            uint16_t first_path_amp_2_2 = node.reception_stats[i][1].first_path_amp_2;
-            uint16_t first_path_amp_2_3 = node.reception_stats[i][2].first_path_amp_2;
-            uint16_t first_path_amp_3_1 = node.reception_stats[i][0].first_path_amp_3;
-            uint16_t first_path_amp_3_2 = node.reception_stats[i][1].first_path_amp_3;
-            uint16_t first_path_amp_3_3 = node.reception_stats[i][2].first_path_amp_3;
-            uint16_t channel_impulse_response_power_1 = node.reception_stats[i][0].channel_impulse_response_power;
-            uint16_t channel_impulse_response_power_2 = node.reception_stats[i][1].channel_impulse_response_power;
-            uint16_t channel_impulse_response_power_3 = node.reception_stats[i][2].channel_impulse_response_power;
-            uint8_t prf_1 = node.reception_stats[i][0].prf;
-            uint8_t prf_2 = node.reception_stats[i][1].prf;
-            uint8_t prf_3 = node.reception_stats[i][2].prf;
+            uint16_t std_noise_1 = node.reception_stats[remote_address][0].std_noise;
+            uint16_t std_noise_2 = node.reception_stats[remote_address][1].std_noise;
+            uint16_t std_noise_3 = node.reception_stats[remote_address][2].std_noise;
+            uint16_t preamble_acc_count_1 = node.reception_stats[remote_address][0].preamble_acc_count;
+            uint16_t preamble_acc_count_2 = node.reception_stats[remote_address][1].preamble_acc_count;
+            uint16_t preamble_acc_count_3 = node.reception_stats[remote_address][2].preamble_acc_count;
+            uint16_t first_path_index_1 = node.reception_stats[remote_address][0].first_path_index;
+            uint16_t first_path_index_2 = node.reception_stats[remote_address][1].first_path_index;
+            uint16_t first_path_index_3 = node.reception_stats[remote_address][2].first_path_index;
+            uint16_t first_path_amp_1_1 = node.reception_stats[remote_address][0].first_path_amp_1;
+            uint16_t first_path_amp_1_2 = node.reception_stats[remote_address][1].first_path_amp_1;
+            uint16_t first_path_amp_1_3 = node.reception_stats[remote_address][2].first_path_amp_1;
+            uint16_t first_path_amp_2_1 = node.reception_stats[remote_address][0].first_path_amp_2;
+            uint16_t first_path_amp_2_2 = node.reception_stats[remote_address][1].first_path_amp_2;
+            uint16_t first_path_amp_2_3 = node.reception_stats[remote_address][2].first_path_amp_2;
+            uint16_t first_path_amp_3_1 = node.reception_stats[remote_address][0].first_path_amp_3;
+            uint16_t first_path_amp_3_2 = node.reception_stats[remote_address][1].first_path_amp_3;
+            uint16_t first_path_amp_3_3 = node.reception_stats[remote_address][2].first_path_amp_3;
+            uint16_t channel_impulse_response_power_1 = node.reception_stats[remote_address][0].channel_impulse_response_power;
+            uint16_t channel_impulse_response_power_2 = node.reception_stats[remote_address][1].channel_impulse_response_power;
+            uint16_t channel_impulse_response_power_3 = node.reception_stats[remote_address][2].channel_impulse_response_power;
+            uint8_t prf_1 = node.reception_stats[remote_address][0].prf;
+            uint8_t prf_2 = node.reception_stats[remote_address][1].prf;
+            uint8_t prf_3 = node.reception_stats[remote_address][2].prf;
             // Pack the message
             mavlink_msg_uwb_range_stats_pack(
                 mb.getSysId(), mb.getCompId(), &msg,
@@ -95,9 +96,7 @@
             mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise3", std_noise3);
             mb.sendMessage(msg);*/
         } else {
-            pc.printf("%d> dist = >%f, ", node.address, node.distances[i]);
-            pc.printf("%d> rtt = %f", node.address, node.roundtriptimes[i]);
-            pc.printf("\r\n");
+            pc.printf("%d - %d> dist = %.4f\r\n", node.address, remote_address, node.distances[remote_address]);
         }
     }
 }
@@ -167,9 +166,6 @@
 void altCallbackRX();
 
 int main() {
-    DigitalOut cs(D8);
-    cs = 1;
-
     UART_Mbed uart(USBTX, USBRX, 115200);
     MAVLinkBridge mb(&uart);
 
@@ -201,7 +197,6 @@
     DW1000::hardwareReset(DW_RESET_PIN);
     for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
         dw_array[i] = new DW1000(spi, irq_mp, DW_CS_PINS[i]);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
-        //dw_array[i]->disable_irq();
     }
 
     // Now we can initialize the DW modules
@@ -257,18 +252,16 @@
     if (!MAVLINK_COMM)
         pc.printf("Entering main loop\r\n");
 
+    //for (int i = 0; i < 10; ++i) {
     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);
+            wait_ms(500);
         } else {
-            //for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
             for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
                 MM2WayRanging& node = *node_array[j];
                 rangeAndDisplayAll(node, mb, timer);
             }
-            //calibrationRanging(4);
         }
     }