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@52:94688f414dcd, 2015-11-27 (annotated)

Committer:
bhepp
Date:
Fri Nov 27 16:17:53 2015 +0000
Revision:
52:94688f414dcd
Parent:
51:e9391d04af00
Child:
53:79a72d752ec4
Fixed bug in loop of anchor nodes and addressing of anchor nodes

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