Tobias Naegeli
Tobi's ubw test branch
Dependencies: mavlink_bridge mbed
Fork of
AIT_UWB_Range
by 
Benjamin Hepp
Diff: main.cpp
- Revision:
- 48:5999e510f154
- Parent:
- 47:b6120c152ad1
- Child:
- 50:50b8aea54a51
diff -r b6120c152ad1 -r 5999e510f154 main.cpp
--- a/main.cpp Thu Mar 19 12:54:28 2015 +0000
+++ b/main.cpp Tue Nov 24 16:41:23 2015 +0000
@@ -3,25 +3,108 @@
#include "PC.h" // Serial Port via USB for debugging with Terminal
#include "DW1000.h" // our DW1000 device driver
#include "MM2WayRanging.h" // our self developed ranging application
+#include "InterruptMultiplexer.h"
-#define myprintf pc.printf // to make the code adaptable to other outputs that support printf
+#include "mavlink_bridge/mavlink_bridge.h"
+
+#define ANCHOR false
+
+using namespace ait;
-PC pc(USBTX, USBRX, 921600); // USB UART Terminal
-DW1000 dw(PA_7, PA_6, PA_5, PB_6, PB_9); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
-MM2WayRanging node(dw); // Instance of the two way ranging algorithm
+//const int ANCHOR_ADDRESS_OFFSET = 100;
+const int ANCHOR_ADDRESS_OFFSET = 1;
+const bool USE_NLOS_SETTINGS = true;
+const PinName DW_RESET_PIN = D15;
+const PinName DW_IRQ_PIN = D14;
+const PinName DW_MOSI_PIN = D11;
+const PinName DW_MISO_PIN = D12;
+const PinName DW_SCLK_PIN = D13;
+#if ANCHOR
+ const bool MAVLINK_COMM = false;
+ 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[NUM_OF_DW_UNITS] = {D10, D9};
+ const PinName DW_IRQ_PINS[NUM_OF_DW_UNITS] = {D14, D6};
+ //const int NUM_OF_DW_UNITS = 1;
+ //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
+#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
+ 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];
+ // 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);
-void rangeAndDisplayAll(){
- node.requestRangingAll(); // Request ranging to all anchors
- for (int i = 1; i <= 5; i++) { // Output Results
- myprintf("%f, ", node.distances[i]);
- //myprintf("T:%f", node.roundtriptimes[i]);
- //myprintf("\r\n");
+ 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;
+ // Pack the message
+ mavlink_msg_uwb_range_stats_pack(
+ mb.getSysId(), mb.getCompId(), &msg,
+ address, remote_address, stamp_us,
+ round_trip_time, range,
+ std_noise_1, std_noise_2, std_noise_2,
+ preamble_acc_count_1, preamble_acc_count_2, preamble_acc_count_3,
+ first_path_index_1, first_path_index_2, first_path_index_3,
+ first_path_amp_1_1, first_path_amp_1_2, first_path_amp_1_3,
+ first_path_amp_2_1, first_path_amp_2_2, first_path_amp_2_3,
+ first_path_amp_3_1, first_path_amp_3_2, first_path_amp_3_3,
+ channel_impulse_response_power_1, channel_impulse_response_power_2, channel_impulse_response_power_3,
+ prf_1, prf_2, prf_3
+ );
+ mb.sendMessage(msg);
+
+ /*mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise1", std_noise1);
+ mb.sendMessage(msg);
+ mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise2", std_noise2);
+ mb.sendMessage(msg);
+ 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("\r\n");
+ }
}
- myprintf("\r\n");
}
-void calibrationRanging(int destination){
-
+void calibrationRanging(MM2WayRanging& node, int destination){
const int numberOfRangings = 100;
float rangings[numberOfRangings];
int index = 0;
@@ -37,29 +120,29 @@
node.requestRanging(destination);
if(node.overflow){
- myprintf("Overflow! Measured: %f\r\n", node.distances[destination]); // This is the output to see if a timer overflow was corrected by the two way ranging class
+ pc.printf("Overflow! Measured: %f\r\n", node.distances[destination]); // This is the output to see if a timer overflow was corrected by the two way ranging class
}
if (node.distances[destination] == -1) {
- myprintf("Measurement timed out\r\n");
+ pc.printf("Measurement timed out\r\n");
wait(0.001);
continue;
}
if (node.distances[destination] < 100) {
rangings[index] = node.distances[destination];
- //myprintf("%f\r\n", node.distances[destination]);
+ //pc.printf("%f\r\n", node.distances[destination]);
index++;
if (index == numberOfRangings) {
- stop = localTimer.read();
+ stop = localTimer.read();
for (int i = 0; i < numberOfRangings - 1; i++)
rangings[numberOfRangings - 1] += rangings[i];
mean = rangings[numberOfRangings - 1] / numberOfRangings;
- myprintf("\r\n\r\nMean %i: %f\r\n", destination, mean);
- myprintf("Elapsed Time for %i: %f\r\n", numberOfRangings, stop - start);
+ pc.printf("\r\n\r\nMean %i: %f\r\n", destination, mean);
+ pc.printf("Elapsed Time for %i: %f\r\n", numberOfRangings, stop - start);
mean = 0;
index = 0;
@@ -69,71 +152,123 @@
start = localTimer.read();
}
}
-
- else myprintf("%f\r\n", node.distances[destination]);
-
- }
-
-}
+ else
+ pc.printf("%f\r\n", node.distances[destination]);
+ }
+}
struct __attribute__((packed, aligned(1))) DistancesFrame {
- uint8_t source;
- uint8_t destination;
- uint8_t type;
- float dist[4];
- };
-
+ uint8_t source;
+ uint8_t destination;
+ uint8_t type;
+ float dist[4];
+};
-
+
+// -----------------------------------------------------------------------------------------------
void altCallbackRX();
-// -----------------------------------------------------------------------------------------------
int main() {
- pc.printf("\r\nDecaWave 1.0 up and running!\r\n"); // Splashscreen
- dw.setEUI(0xFAEDCD01FAEDCD01); // basic methods called to check if we have a working SPI connection
- 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());
-
- node.isAnchor = true; // declare as anchor or beacon
-
- if (node.isAnchor) {
- node.address = 1;
- myprintf("This node is Anchor node %d \r\n", node.address);
- } else {
- node.address = 0;
- myprintf("This node is a Beacon. ");
+ // 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
+
+ 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.
+ spi.frequency(1000000); // with a 1MHz clock rate (worked up to 49MHz in our Test)
+
+ DW1000* dw_array[NUM_OF_DW_UNITS + 2];
+ MM2WayRanging* node_array[NUM_OF_DW_UNITS + 2]; // Instance of the two way ranging algorithm
+
+ // == 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) {
+ 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
+ for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
+ DW1000& dw = *dw_array[i];
+ dw.setEUI(0xFAEDCD01FAEDCD01 + i); // basic methods called to check if we have a working SPI connection
+ node_array[i] = new MM2WayRanging(*dw_array[i]);
+ 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: %fV\r\n", dw.getVoltage());
+ }
}
-
- if (node.address == 5){ // the node with address 5 was used as an observer node putting out the results in our test case
- dw.setCallbacks(&altCallbackRX, NULL);
+
+ 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];
+ node.isAnchor = ANCHOR; // declare as anchor or beacon
+ if (ANCHOR) {
+ node.address = ANCHOR_ADDRESS_OFFSET + i;
+ if (!MAVLINK_COMM)
+ pc.printf("This node is Anchor node %d\r\n", node.address);
+ } else {
+ node.address = i;
+ if (!MAVLINK_COMM)
+ pc.printf("This node is a Beacon.\r\n ");
+ }
}
-
- while(1) {
- if (!node.isAnchor){
- rangeAndDisplayAll();
+
+ // Set NLOS settings
+ if (USE_NLOS_SETTINGS) {
+ for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
+ DW1000& dw = *dw_array[i];
+ dw.writeRegister8(DW1000_LDE_CTRL, 0x0806, 0x07); //LDE_CFG1
+ dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x0003); //LDE_CFG2
+ //dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1603); //LDE_CFG2
+ // Channel control
+ uint16_t prf_mask = (0x1 << 19) | (0x1 << 18);
+ uint16_t prf = 0x01 << 18; // 16 MHz
+ uint16_t v = dw.readRegister16(DW1000_CHAN_CTRL, 0x00);
+ v &= ~prf_mask;
+ v |= prf;
+ dw.writeRegister16(DW1000_CHAN_CTRL, 0x00, v);
+ }
+ }
+
+ while (true) {
+ if (ANCHOR) {
+ pc.printf("."); // to see if the core and output is working
+ wait(0.5);
+ } 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);
-
- } else {
- //myprintf("."); // to see if the core and output is working
- wait(0.5);
}
}
}
-void altCallbackRX() { // this callback was used in our verification test case for the observer node which only receives and outputs the resulting data
-
+void altCallbackRX(DW1000& dw) { // this callback was used in our verification test case for the observer node which only receives and outputs the resulting data
DistancesFrame distFrame;
float distances[4];
dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&distFrame, dw.getFramelength());
-
+
if (distFrame.destination == 5) {
for (int i = 0; i<4; i++){
- myprintf("%f, ", distFrame.dist[i]);
+ pc.printf("%f, ", distFrame.dist[i]);
}
-
- myprintf("\r\n");
+ pc.printf("\r\n");
}
dw.startRX();
}
