Tobi's ubw test branch

Dependencies:   mavlink_bridge mbed

Fork of AIT_UWB_Range by Benjamin Hepp

Committer:
bhepp
Date:
Thu Nov 26 22:24:17 2015 +0000
Revision:
51:e9391d04af00
Parent:
50:50b8aea54a51
Child:
52:94688f414dcd
Fixed problems with receiver start and stop.

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