Tobias Naegeli / Mbed 2 deprecated AIT_UWB_Range

Tobi's ubw test branch

Dependencies:   mavlink_bridge mbed

Fork of AIT_UWB_Range by Benjamin Hepp

main.cpp@48:5999e510f154, 2015-11-24 (annotated)

Committer:
bhepp
Date:
Tue Nov 24 16:41:23 2015 +0000
Revision:
48:5999e510f154
Parent:
47:b6120c152ad1
Child:
50:50b8aea54a51
Multiple Receivers implemented

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manumaet 24:6f25ba679490 1 // by Matthias Grob & Manuel Stalder - ETH Zürich - 2015
manumaet 0:f50e671ffff7 2 #include "mbed.h"
manumaet 26:a65c6f26c458 3 #include "PC.h" // Serial Port via USB for debugging with Terminal
manumaet 27:71178fdb78e1 4 #include "DW1000.h" // our DW1000 device driver
manumaet 44:2e0045042a59 5 #include "MM2WayRanging.h" // our self developed ranging application
bhepp 48:5999e510f154 6 #include "InterruptMultiplexer.h"
manumaet 44:2e0045042a59 7
bhepp 48:5999e510f154 8 #include "mavlink_bridge/mavlink_bridge.h"
bhepp 48:5999e510f154 9
bhepp 48:5999e510f154 10 #define ANCHOR false
bhepp 48:5999e510f154 11
bhepp 48:5999e510f154 12 using namespace ait;
manumaet 44:2e0045042a59 13
bhepp 48:5999e510f154 14 //const int ANCHOR_ADDRESS_OFFSET = 100;
bhepp 48:5999e510f154 15 const int ANCHOR_ADDRESS_OFFSET = 1;
bhepp 48:5999e510f154 16 const bool USE_NLOS_SETTINGS = true;
bhepp 48:5999e510f154 17 const PinName DW_RESET_PIN = D15;
bhepp 48:5999e510f154 18 const PinName DW_IRQ_PIN = D14;
bhepp 48:5999e510f154 19 const PinName DW_MOSI_PIN = D11;
bhepp 48:5999e510f154 20 const PinName DW_MISO_PIN = D12;
bhepp 48:5999e510f154 21 const PinName DW_SCLK_PIN = D13;
bhepp 48:5999e510f154 22 #if ANCHOR
bhepp 48:5999e510f154 23 const bool MAVLINK_COMM = false;
bhepp 48:5999e510f154 24 const int NUM_OF_DW_UNITS = 1;
bhepp 48:5999e510f154 25 const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
bhepp 48:5999e510f154 26 #else
bhepp 48:5999e510f154 27 // const bool MAVLINK_COMM = true;
bhepp 48:5999e510f154 28 const bool MAVLINK_COMM = false;
bhepp 48:5999e510f154 29 const int NUM_OF_DW_UNITS = 2;
bhepp 48:5999e510f154 30 const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10, D9};
bhepp 48:5999e510f154 31 const PinName DW_IRQ_PINS[NUM_OF_DW_UNITS] = {D14, D6};
bhepp 48:5999e510f154 32 //const int NUM_OF_DW_UNITS = 1;
bhepp 48:5999e510f154 33 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
bhepp 48:5999e510f154 34 #endif
bhepp 48:5999e510f154 35
bhepp 48:5999e510f154 36 PC pc(USBTX, USBRX, 115200); // USB UART Terminal
bhepp 48:5999e510f154 37
bhepp 48:5999e510f154 38 void rangeAndDisplayAll(MM2WayRanging& node, MAVLinkBridge& mb, Timer& timer) {
bhepp 48:5999e510f154 39 node.requestRanging(ANCHOR_ADDRESS_OFFSET); // Request ranging to all anchors
bhepp 48:5999e510f154 40 for (int i = 1; i <= 1; i++) { // Output Results
bhepp 48:5999e510f154 41 if (MAVLINK_COMM) {
bhepp 48:5999e510f154 42 uint8_t address = node.address;
bhepp 48:5999e510f154 43 uint8_t remote_address = i;
bhepp 48:5999e510f154 44 uint32_t stamp_us = timer.read_us();
bhepp 48:5999e510f154 45 float round_trip_time = node.roundtriptimes[i];
bhepp 48:5999e510f154 46 float range = node.distances[i];
bhepp 48:5999e510f154 47 // Initialize the required buffers
bhepp 48:5999e510f154 48 mavlink_message_t msg;
bhepp 48:5999e510f154 49 // Pack the message
bhepp 48:5999e510f154 50 mavlink_msg_uwb_range_pack(mb.getSysId(), mb.getCompId(), &msg, address, remote_address, stamp_us, round_trip_time, range);
bhepp 48:5999e510f154 51 mb.sendMessage(msg);
manumaet 0:f50e671ffff7 52
bhepp 48:5999e510f154 53 uint16_t std_noise_1 = node.reception_stats[i][0].std_noise;
bhepp 48:5999e510f154 54 uint16_t std_noise_2 = node.reception_stats[i][1].std_noise;
bhepp 48:5999e510f154 55 uint16_t std_noise_3 = node.reception_stats[i][2].std_noise;
bhepp 48:5999e510f154 56 uint16_t preamble_acc_count_1 = node.reception_stats[i][0].preamble_acc_count;
bhepp 48:5999e510f154 57 uint16_t preamble_acc_count_2 = node.reception_stats[i][1].preamble_acc_count;
bhepp 48:5999e510f154 58 uint16_t preamble_acc_count_3 = node.reception_stats[i][2].preamble_acc_count;
bhepp 48:5999e510f154 59 uint16_t first_path_index_1 = node.reception_stats[i][0].first_path_index;
bhepp 48:5999e510f154 60 uint16_t first_path_index_2 = node.reception_stats[i][1].first_path_index;
bhepp 48:5999e510f154 61 uint16_t first_path_index_3 = node.reception_stats[i][2].first_path_index;
bhepp 48:5999e510f154 62 uint16_t first_path_amp_1_1 = node.reception_stats[i][0].first_path_amp_1;
bhepp 48:5999e510f154 63 uint16_t first_path_amp_1_2 = node.reception_stats[i][1].first_path_amp_1;
bhepp 48:5999e510f154 64 uint16_t first_path_amp_1_3 = node.reception_stats[i][2].first_path_amp_1;
bhepp 48:5999e510f154 65 uint16_t first_path_amp_2_1 = node.reception_stats[i][0].first_path_amp_2;
bhepp 48:5999e510f154 66 uint16_t first_path_amp_2_2 = node.reception_stats[i][1].first_path_amp_2;
bhepp 48:5999e510f154 67 uint16_t first_path_amp_2_3 = node.reception_stats[i][2].first_path_amp_2;
bhepp 48:5999e510f154 68 uint16_t first_path_amp_3_1 = node.reception_stats[i][0].first_path_amp_3;
bhepp 48:5999e510f154 69 uint16_t first_path_amp_3_2 = node.reception_stats[i][1].first_path_amp_3;
bhepp 48:5999e510f154 70 uint16_t first_path_amp_3_3 = node.reception_stats[i][2].first_path_amp_3;
bhepp 48:5999e510f154 71 uint16_t channel_impulse_response_power_1 = node.reception_stats[i][0].channel_impulse_response_power;
bhepp 48:5999e510f154 72 uint16_t channel_impulse_response_power_2 = node.reception_stats[i][1].channel_impulse_response_power;
bhepp 48:5999e510f154 73 uint16_t channel_impulse_response_power_3 = node.reception_stats[i][2].channel_impulse_response_power;
bhepp 48:5999e510f154 74 uint8_t prf_1 = node.reception_stats[i][0].prf;
bhepp 48:5999e510f154 75 uint8_t prf_2 = node.reception_stats[i][1].prf;
bhepp 48:5999e510f154 76 uint8_t prf_3 = node.reception_stats[i][2].prf;
bhepp 48:5999e510f154 77 // Pack the message
bhepp 48:5999e510f154 78 mavlink_msg_uwb_range_stats_pack(
bhepp 48:5999e510f154 79 mb.getSysId(), mb.getCompId(), &msg,
bhepp 48:5999e510f154 80 address, remote_address, stamp_us,
bhepp 48:5999e510f154 81 round_trip_time, range,
bhepp 48:5999e510f154 82 std_noise_1, std_noise_2, std_noise_2,
bhepp 48:5999e510f154 83 preamble_acc_count_1, preamble_acc_count_2, preamble_acc_count_3,
bhepp 48:5999e510f154 84 first_path_index_1, first_path_index_2, first_path_index_3,
bhepp 48:5999e510f154 85 first_path_amp_1_1, first_path_amp_1_2, first_path_amp_1_3,
bhepp 48:5999e510f154 86 first_path_amp_2_1, first_path_amp_2_2, first_path_amp_2_3,
bhepp 48:5999e510f154 87 first_path_amp_3_1, first_path_amp_3_2, first_path_amp_3_3,
bhepp 48:5999e510f154 88 channel_impulse_response_power_1, channel_impulse_response_power_2, channel_impulse_response_power_3,
bhepp 48:5999e510f154 89 prf_1, prf_2, prf_3
bhepp 48:5999e510f154 90 );
bhepp 48:5999e510f154 91 mb.sendMessage(msg);
bhepp 48:5999e510f154 92
bhepp 48:5999e510f154 93 /*mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise1", std_noise1);
bhepp 48:5999e510f154 94 mb.sendMessage(msg);
bhepp 48:5999e510f154 95 mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise2", std_noise2);
bhepp 48:5999e510f154 96 mb.sendMessage(msg);
bhepp 48:5999e510f154 97 mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise3", std_noise3);
bhepp 48:5999e510f154 98 mb.sendMessage(msg);*/
bhepp 48:5999e510f154 99 } else {
bhepp 48:5999e510f154 100 pc.printf("^R>%f, ", node.distances[i]);
bhepp 48:5999e510f154 101 pc.printf("T:%f", node.roundtriptimes[i]);
bhepp 48:5999e510f154 102 pc.printf("\r\n");
bhepp 48:5999e510f154 103 }
manumaet 44:2e0045042a59 104 }
manumaet 44:2e0045042a59 105 }
manumaet 6:d5864a1b9e17 106
bhepp 48:5999e510f154 107 void calibrationRanging(MM2WayRanging& node, int destination){
manumaet 45:01a33363bc21 108 const int numberOfRangings = 100;
manumaet 45:01a33363bc21 109 float rangings[numberOfRangings];
manumaet 45:01a33363bc21 110 int index = 0;
manumaet 45:01a33363bc21 111 float mean = 0;
manumaet 45:01a33363bc21 112 float start, stop;
manumaet 45:01a33363bc21 113
manumaet 45:01a33363bc21 114 Timer localTimer;
manumaet 45:01a33363bc21 115 localTimer.start();
manumaet 45:01a33363bc21 116
manumaet 45:01a33363bc21 117 start = localTimer.read();
manumaet 45:01a33363bc21 118
manumaet 45:01a33363bc21 119 while (1) {
manumaet 45:01a33363bc21 120
manumaet 45:01a33363bc21 121 node.requestRanging(destination);
manumaet 45:01a33363bc21 122 if(node.overflow){
bhepp 48:5999e510f154 123 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
manumaet 45:01a33363bc21 124 }
manumaet 45:01a33363bc21 125
manumaet 45:01a33363bc21 126 if (node.distances[destination] == -1) {
bhepp 48:5999e510f154 127 pc.printf("Measurement timed out\r\n");
manumaet 45:01a33363bc21 128 wait(0.001);
manumaet 45:01a33363bc21 129 continue;
manumaet 45:01a33363bc21 130 }
manumaet 45:01a33363bc21 131
manumaet 45:01a33363bc21 132 if (node.distances[destination] < 100) {
manumaet 45:01a33363bc21 133 rangings[index] = node.distances[destination];
bhepp 48:5999e510f154 134 //pc.printf("%f\r\n", node.distances[destination]);
manumaet 45:01a33363bc21 135 index++;
manumaet 45:01a33363bc21 136
manumaet 45:01a33363bc21 137 if (index == numberOfRangings) {
bhepp 48:5999e510f154 138 stop = localTimer.read();
manumaet 45:01a33363bc21 139
manumaet 45:01a33363bc21 140 for (int i = 0; i < numberOfRangings - 1; i++)
manumaet 45:01a33363bc21 141 rangings[numberOfRangings - 1] += rangings[i];
manumaet 45:01a33363bc21 142
manumaet 45:01a33363bc21 143 mean = rangings[numberOfRangings - 1] / numberOfRangings;
bhepp 48:5999e510f154 144 pc.printf("\r\n\r\nMean %i: %f\r\n", destination, mean);
bhepp 48:5999e510f154 145 pc.printf("Elapsed Time for %i: %f\r\n", numberOfRangings, stop - start);
manumaet 45:01a33363bc21 146
manumaet 45:01a33363bc21 147 mean = 0;
manumaet 45:01a33363bc21 148 index = 0;
manumaet 45:01a33363bc21 149
manumaet 45:01a33363bc21 150 //wait(2);
manumaet 45:01a33363bc21 151
manumaet 45:01a33363bc21 152 start = localTimer.read();
manumaet 45:01a33363bc21 153 }
manumaet 45:01a33363bc21 154 }
bhepp 48:5999e510f154 155 else
bhepp 48:5999e510f154 156 pc.printf("%f\r\n", node.distances[destination]);
bhepp 48:5999e510f154 157 }
bhepp 48:5999e510f154 158 }
manumaet 45:01a33363bc21 159
manumaet 46:6398237672a0 160 struct __attribute__((packed, aligned(1))) DistancesFrame {
bhepp 48:5999e510f154 161 uint8_t source;
bhepp 48:5999e510f154 162 uint8_t destination;
bhepp 48:5999e510f154 163 uint8_t type;
bhepp 48:5999e510f154 164 float dist[4];
bhepp 48:5999e510f154 165 };
manumaet 45:01a33363bc21 166
bhepp 48:5999e510f154 167
bhepp 48:5999e510f154 168 // -----------------------------------------------------------------------------------------------
manumaet 46:6398237672a0 169 void altCallbackRX();
manumaet 45:01a33363bc21 170
manumaet 0:f50e671ffff7 171 int main() {
bhepp 48:5999e510f154 172 // Setup interrupt pin
bhepp 48:5999e510f154 173 InterruptMultiplexer irq_mp;
bhepp 48:5999e510f154 174 InterruptIn irq(DW_IRQ_PIN);
bhepp 48:5999e510f154 175 irq.rise(&irq_mp, &InterruptMultiplexer::trigger); // attach interrupt handler to rising edge of interrupt pin from DW1000
bhepp 48:5999e510f154 176
bhepp 48:5999e510f154 177 SPI spi(DW_MOSI_PIN, DW_MISO_PIN, DW_SCLK_PIN);
bhepp 48:5999e510f154 178 spi.format(8,0); // Setup the spi for standard 8 bit data and SPI-Mode 0 (GPIO5, GPIO6 open circuit or ground on DW1000)
bhepp 48:5999e510f154 179 // NOTE: Minimum Frequency 1MHz. Below it is now working. Could be something with the activation and deactivation of interrupts.
bhepp 48:5999e510f154 180 spi.frequency(1000000); // with a 1MHz clock rate (worked up to 49MHz in our Test)
bhepp 48:5999e510f154 181
bhepp 48:5999e510f154 182 DW1000* dw_array[NUM_OF_DW_UNITS + 2];
bhepp 48:5999e510f154 183 MM2WayRanging* node_array[NUM_OF_DW_UNITS + 2]; // Instance of the two way ranging algorithm
bhepp 48:5999e510f154 184
bhepp 48:5999e510f154 185 // == IMPORTANT == Create all DW objects first (this will cause a reset of the DW module)
bhepp 48:5999e510f154 186 DW1000::hardwareReset(DW_RESET_PIN);
bhepp 48:5999e510f154 187 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 48:5999e510f154 188 dw_array[i] = new DW1000(spi, irq_mp, DW_CS_PINS[i]); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
bhepp 48:5999e510f154 189 //dw_array[i]->disable_irq();
bhepp 48:5999e510f154 190 }
bhepp 48:5999e510f154 191
bhepp 48:5999e510f154 192 // Now we can initialize the DW modules
bhepp 48:5999e510f154 193 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 48:5999e510f154 194 DW1000& dw = *dw_array[i];
bhepp 48:5999e510f154 195 dw.setEUI(0xFAEDCD01FAEDCD01 + i); // basic methods called to check if we have a working SPI connection
bhepp 48:5999e510f154 196 node_array[i] = new MM2WayRanging(*dw_array[i]);
bhepp 48:5999e510f154 197 if (MAVLINK_COMM) {
bhepp 48:5999e510f154 198 // TODO
bhepp 48:5999e510f154 199 } else {
bhepp 48:5999e510f154 200 pc.printf("\r\nUnit %d\r\n", i);
bhepp 48:5999e510f154 201 pc.printf("\r\nDecaWave 1.0 up and running!\r\n"); // Splashscreen
bhepp 48:5999e510f154 202 pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
bhepp 48:5999e510f154 203 pc.printf("EUI register: %016llX\r\n", dw.getEUI());
bhepp 48:5999e510f154 204 pc.printf("Voltage: %fV\r\n", dw.getVoltage());
bhepp 48:5999e510f154 205 }
manumaet 44:2e0045042a59 206 }
bhepp 48:5999e510f154 207
bhepp 48:5999e510f154 208 Timer timer;
bhepp 48:5999e510f154 209 timer.start();
bhepp 48:5999e510f154 210
bhepp 48:5999e510f154 211 UART_Mbed uart(USBTX, USBRX, 115200);
bhepp 48:5999e510f154 212 MAVLinkBridge mb(&uart);
bhepp 48:5999e510f154 213
bhepp 48:5999e510f154 214 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 48:5999e510f154 215 DW1000& dw = *dw_array[i];
bhepp 48:5999e510f154 216 MM2WayRanging& node = *node_array[i];
bhepp 48:5999e510f154 217 node.isAnchor = ANCHOR; // declare as anchor or beacon
bhepp 48:5999e510f154 218 if (ANCHOR) {
bhepp 48:5999e510f154 219 node.address = ANCHOR_ADDRESS_OFFSET + i;
bhepp 48:5999e510f154 220 if (!MAVLINK_COMM)
bhepp 48:5999e510f154 221 pc.printf("This node is Anchor node %d\r\n", node.address);
bhepp 48:5999e510f154 222 } else {
bhepp 48:5999e510f154 223 node.address = i;
bhepp 48:5999e510f154 224 if (!MAVLINK_COMM)
bhepp 48:5999e510f154 225 pc.printf("This node is a Beacon.\r\n ");
bhepp 48:5999e510f154 226 }
manumaet 47:b6120c152ad1 227 }
bhepp 48:5999e510f154 228
bhepp 48:5999e510f154 229 // Set NLOS settings
bhepp 48:5999e510f154 230 if (USE_NLOS_SETTINGS) {
bhepp 48:5999e510f154 231 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 48:5999e510f154 232 DW1000& dw = *dw_array[i];
bhepp 48:5999e510f154 233 dw.writeRegister8(DW1000_LDE_CTRL, 0x0806, 0x07); //LDE_CFG1
bhepp 48:5999e510f154 234 dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x0003); //LDE_CFG2
bhepp 48:5999e510f154 235 //dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1603); //LDE_CFG2
bhepp 48:5999e510f154 236 // Channel control
bhepp 48:5999e510f154 237 uint16_t prf_mask = (0x1 << 19) | (0x1 << 18);
bhepp 48:5999e510f154 238 uint16_t prf = 0x01 << 18; // 16 MHz
bhepp 48:5999e510f154 239 uint16_t v = dw.readRegister16(DW1000_CHAN_CTRL, 0x00);
bhepp 48:5999e510f154 240 v &= ~prf_mask;
bhepp 48:5999e510f154 241 v |= prf;
bhepp 48:5999e510f154 242 dw.writeRegister16(DW1000_CHAN_CTRL, 0x00, v);
bhepp 48:5999e510f154 243 }
bhepp 48:5999e510f154 244 }
bhepp 48:5999e510f154 245
bhepp 48:5999e510f154 246 while (true) {
bhepp 48:5999e510f154 247 if (ANCHOR) {
bhepp 48:5999e510f154 248 pc.printf("."); // to see if the core and output is working
bhepp 48:5999e510f154 249 wait(0.5);
bhepp 48:5999e510f154 250 } else {
bhepp 48:5999e510f154 251 //for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
bhepp 48:5999e510f154 252 for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
bhepp 48:5999e510f154 253 MM2WayRanging& node = *node_array[j];
bhepp 48:5999e510f154 254 rangeAndDisplayAll(node, mb, timer);
bhepp 48:5999e510f154 255 }
manumaet 45:01a33363bc21 256 //calibrationRanging(4);
manumaet 47:b6120c152ad1 257 }
manumaet 0:f50e671ffff7 258 }
manumaet 46:6398237672a0 259 }
manumaet 46:6398237672a0 260
manumaet 46:6398237672a0 261
bhepp 48:5999e510f154 262 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
manumaet 46:6398237672a0 263 DistancesFrame distFrame;
manumaet 46:6398237672a0 264 float distances[4];
manumaet 46:6398237672a0 265 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&distFrame, dw.getFramelength());
bhepp 48:5999e510f154 266
manumaet 47:b6120c152ad1 267 if (distFrame.destination == 5) {
manumaet 46:6398237672a0 268 for (int i = 0; i<4; i++){
bhepp 48:5999e510f154 269 pc.printf("%f, ", distFrame.dist[i]);
manumaet 46:6398237672a0 270 }
bhepp 48:5999e510f154 271 pc.printf("\r\n");
manumaet 46:6398237672a0 272 }
manumaet 46:6398237672a0 273 dw.startRX();
manumaet 46:6398237672a0 274 }