Tobias Naegeli
Tobi's ubw test branch
Dependencies: mavlink_bridge mbed
Fork of
AIT_UWB_Range
by 
Benjamin Hepp
Diff: main.cpp
- Revision:
- 53:79a72d752ec4
- Parent:
- 52:94688f414dcd
- Child:
- 54:a59803fcce58
--- a/main.cpp Fri Nov 27 16:17:53 2015 +0000
+++ b/main.cpp Mon Jan 04 12:45:37 2016 +0000
@@ -14,145 +14,42 @@
const int SPI_FREQUENCY = 1000000;
const int NUM_OF_ANCHORS = 1;
const int ANCHOR_ADDRESS_OFFSET = 10;
-const bool USE_NLOS_SETTINGS = true;
+const bool USE_NLOS_SETTINGS = false;
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};
+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 = 3;
- const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10, D9, D8};
+const bool MAVLINK_COMM = false;
+const int NUM_OF_DW_UNITS = 2;
+//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D9, D8,D10};
+//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10,D9,D8};
+//const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10,D9,D8,D7};
+const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D9,D10};
+//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) {
+void rangeAndDisplayAll(MM2WayRanging& node, MAVLinkBridge& mb, Timer& timer)
+{
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;
- uint32_t stamp_us = timer.read_us();
- 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[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,
- 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);
+ pc.printf("%d - %d> dist = %.4f\r\n", node.address, remote_address, node.distances[remote_address]);
- /*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("%d - %d> dist = %.4f\r\n", node.address, remote_address, node.distances[remote_address]);
- }
}
}
-
-void calibrationRanging(MM2WayRanging& node, int destination){
- const int numberOfRangings = 100;
- float rangings[numberOfRangings];
- int index = 0;
- float mean = 0;
- float start, stop;
-
- Timer localTimer;
- localTimer.start();
-
- start = localTimer.read();
-
- while (1) {
-
- node.requestRanging(destination);
- if(node.overflow){
- 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) {
- pc.printf("Measurement timed out\r\n");
- wait(0.001);
- continue;
- }
+//continue;
- if (node.distances[destination] < 100) {
- rangings[index] = node.distances[destination];
- //pc.printf("%f\r\n", node.distances[destination]);
- index++;
-
- if (index == numberOfRangings) {
- stop = localTimer.read();
-
- for (int i = 0; i < numberOfRangings - 1; i++)
- rangings[numberOfRangings - 1] += rangings[i];
-
- mean = rangings[numberOfRangings - 1] / numberOfRangings;
- 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;
-
- //wait(2);
-
- start = localTimer.read();
- }
- }
- else
- pc.printf("%f\r\n", node.distances[destination]);
- }
-}
struct __attribute__((packed, aligned(1))) DistancesFrame {
uint8_t source;
@@ -165,7 +62,15 @@
// -----------------------------------------------------------------------------------------------
void altCallbackRX();
-int main() {
+int main()
+{
+ //first set the CS to high
+ DigitalOut test1(D9);
+ DigitalOut test2(D10);
+ test1=1;
+ test2=1;
+
+ wait(0.1);
UART_Mbed uart(USBTX, USBRX, 115200);
MAVLinkBridge mb(&uart);
@@ -185,38 +90,38 @@
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
+ DW1000* dw_array[NUM_OF_DW_UNITS + 0];
+ MM2WayRanging* node_array[NUM_OF_DW_UNITS + 0]; // 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) {
+ for (int i = 0; i < 2; ++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[1] = new DW1000(spi, irq_mp, DW_CS_PINS[1]); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
- // 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
+
+ wait(0.1) ;
+
+ dw.setEUI(0xFAEDCD01FAEDCD01 + i);
+ wait(0.1) ;
+ // 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: %.2fV\r\n", dw.getVoltage());
- }
- }
- for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
- DW1000& dw = *dw_array[i];
+ 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());
+
+
MM2WayRanging& node = *node_array[i];
node.isAnchor = ANCHOR; // declare as anchor or beacon
if (ANCHOR) {
@@ -228,62 +133,34 @@
if (!MAVLINK_COMM)
pc.printf("This node is a Beacon\r\n");
}
+
+ uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + 0;
+ node.requestRanging(remote_address);
+ rangeAndDisplayAll(node, mb, timer);
+ irq_mp.clear();
+ wait(2);
}
- // 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
- //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);
- }
- }
-
+ // wait(1);
+// MM2WayRanging& node = *node_array[1];
+// uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + 0;
+// node.requestRanging(remote_address);
+// rangeAndDisplayAll(node, mb, timer);
if (!MAVLINK_COMM)
pc.printf("Entering main loop\r\n");
- //for (int i = 0; i < 10; ++i) {
- while (true) {
- if (ANCHOR) {
- pc.printf("."); // to see if the core and output is working
- wait_ms(500);
- } else {
- for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
- MM2WayRanging& node = *node_array[j];
- rangeAndDisplayAll(node, mb, timer);
- }
- }
- }
-
- for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
- delete node_array[i];
- delete dw_array[i];
- }
+ //for (int i = 0; i < 2; ++i) {
+//
+// if (ANCHOR) {
+// pc.printf("."); // to see if the core and output is working
+// wait_ms(1);
+// } else {
+// for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
+// MM2WayRanging& node = *node_array[j];
+// rangeAndDisplayAll(node, mb, timer);
+// wait(0.1);
+// }
+// }
+// }
- }
}
-
-
-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++){
- pc.printf("%f, ", distFrame.dist[i]);
- }
- pc.printf("\r\n");
- }
- dw.startRX();
-}
