Benjamin Hepp / Mbed 2 deprecated AIT_UWB_Tracker

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   DW1000 ait_link BufferedSerial mbed

main_multi_range.cpp@6:9055fcc2f0cb, 2016-04-04 (annotated)

Committer:
bhepp
Date:
Mon Apr 04 11:20:13 2016 +0000
Revision:
6:9055fcc2f0cb
Parent:
5:1928eb8c417a
Child:
9:3844cf7e5c00
Added mode for HDLC data transmission instead of Mavlink

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bhepp 1:c070ca30da80 1 #include "settings.h"
bhepp 1:c070ca30da80 2
bhepp 1:c070ca30da80 3 #if not BUILD_SLAVE
bhepp 1:c070ca30da80 4
bhepp 2:5af0f0006f40 5 #include <stdarg.h>
bhepp 1:c070ca30da80 6 #include <mbed.h>
bhepp 1:c070ca30da80 7 #include <DW1000.h>
bhepp 1:c070ca30da80 8 #include <DW1000Utils.h>
bhepp 1:c070ca30da80 9 #include <mavlink_bridge/mavlink_bridge.h>
bhepp 6:9055fcc2f0cb 10 #include "uwb_link_mbed.h"
bhepp 1:c070ca30da80 11
bhepp 2:5af0f0006f40 12 #include "BufferedSerial.h"
bhepp 2:5af0f0006f40 13 #include "UWB2WayMultiRange.h"
bhepp 1:c070ca30da80 14
bhepp 6:9055fcc2f0cb 15 enum CommMode {
bhepp 6:9055fcc2f0cb 16 ASCII,
bhepp 6:9055fcc2f0cb 17 MAVLINK,
bhepp 6:9055fcc2f0cb 18 UWBLINK
bhepp 6:9055fcc2f0cb 19 };
bhepp 6:9055fcc2f0cb 20
bhepp 2:5af0f0006f40 21 using ait::UWB2WayMultiRange;
bhepp 1:c070ca30da80 22
bhepp 1:c070ca30da80 23 //#define MEASURE_UWB_RANGING_RATE 1
bhepp 1:c070ca30da80 24
bhepp 6:9055fcc2f0cb 25 using ait::UWBLink;
bhepp 6:9055fcc2f0cb 26 using ait::UWBLinkMbed;
bhepp 6:9055fcc2f0cb 27 using ait::UWBMessageString;
bhepp 6:9055fcc2f0cb 28 using ait::UWBMessageMultiRange;
bhepp 6:9055fcc2f0cb 29 using ait::UWBMessage;
bhepp 1:c070ca30da80 30 using ait::MAVLinkBridge;
bhepp 1:c070ca30da80 31 using ait::UART_Mbed;
bhepp 1:c070ca30da80 32
bhepp 5:1928eb8c417a 33 // Overall reset rate (if no ranging is performed within the timeout)
bhepp 5:1928eb8c417a 34 const float WATCHDOG_TIMEOUT = 1.0f;
bhepp 5:1928eb8c417a 35
bhepp 5:1928eb8c417a 36 const int SPI_FREQUENCY = 5000000;
bhepp 6:9055fcc2f0cb 37 const float MIN_DW_VOLTAGE = 2.0f;
bhepp 1:c070ca30da80 38
bhepp 2:5af0f0006f40 39 const int TRACKER_ADDRESS = 0;
bhepp 1:c070ca30da80 40 const int NUM_OF_SLAVES = 1;
bhepp 4:1a2c1e5e5516 41 const int SLAVE_ADDRESS_OFFSET = 20;
bhepp 1:c070ca30da80 42
bhepp 1:c070ca30da80 43 const bool USE_NLOS_SETTINGS = true;
bhepp 1:c070ca30da80 44
bhepp 5:1928eb8c417a 45 //const PinName DW_RESET_PIN = D15;
bhepp 5:1928eb8c417a 46 const PinName DW_RESET_PIN = NC;
bhepp 1:c070ca30da80 47 const PinName DW_MOSI_PIN = D11;
bhepp 1:c070ca30da80 48 const PinName DW_MISO_PIN = D12;
bhepp 1:c070ca30da80 49 const PinName DW_SCLK_PIN = D13;
bhepp 1:c070ca30da80 50
bhepp 6:9055fcc2f0cb 51 //const int NUM_OF_DW_UNITS = 4;
bhepp 6:9055fcc2f0cb 52 ////const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7, D8, D9, D10};
bhepp 6:9055fcc2f0cb 53 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D8, D9, D14, D15};
bhepp 5:1928eb8c417a 54 const int NUM_OF_DW_UNITS = 4;
bhepp 5:1928eb8c417a 55 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D7, D8, D9, D10};
bhepp 6:9055fcc2f0cb 56 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D8, D9, D15, D14};
bhepp 6:9055fcc2f0cb 57 const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D8, D9, D7, D10};
bhepp 6:9055fcc2f0cb 58
bhepp 6:9055fcc2f0cb 59 const int MAVLINK_MAX_NUM_OF_DW_UNITS = 4;
bhepp 6:9055fcc2f0cb 60 const int MAX_UWB_LINK_FRAME_LENGTH = 1024;
bhepp 1:c070ca30da80 61 #if _DEBUG
bhepp 6:9055fcc2f0cb 62 // CommMode comm_mode = ASCII;
bhepp 6:9055fcc2f0cb 63 CommMode comm_mode = UWBLINK;
bhepp 6:9055fcc2f0cb 64 // CommMode comm_mode = MAVLINK;
bhepp 1:c070ca30da80 65 #else
bhepp 6:9055fcc2f0cb 66 // CommMode comm_mode = MAVLINK;
bhepp 6:9055fcc2f0cb 67 CommMode comm_mode = UWBLINK;
bhepp 1:c070ca30da80 68 #endif
bhepp 1:c070ca30da80 69
bhepp 1:c070ca30da80 70
bhepp 2:5af0f0006f40 71 BufferedSerial pc(USBTX, USBRX, 115200, 8 * 1024); // USB UART Terminal
bhepp 1:c070ca30da80 72
bhepp 6:9055fcc2f0cb 73 void send_status_message(MAVLinkBridge& mb, UWBLink& ul, char* str, ...)
bhepp 1:c070ca30da80 74 {
bhepp 2:5af0f0006f40 75 va_list args;
bhepp 2:5af0f0006f40 76 va_start(args, str);
bhepp 6:9055fcc2f0cb 77 if (comm_mode == MAVLINK) {
bhepp 2:5af0f0006f40 78 char buffer[MAVLINK_MSG_UWB_STATUS_FIELD_DESCRIPTION_LEN];
bhepp 2:5af0f0006f40 79 int n = vsnprintf(buffer, sizeof(buffer), str, args);
bhepp 2:5af0f0006f40 80 if(n > sizeof(buffer)) {
bhepp 6:9055fcc2f0cb 81 // Dangerous: Could lead to infinite recursion
bhepp 6:9055fcc2f0cb 82 send_status_message(mb, ul, "%s %d buffer to small (buf_size: %d, required: %d)!\r\n", __FILE__, __LINE__, sizeof(buffer), n);
bhepp 2:5af0f0006f40 83 } else {
bhepp 2:5af0f0006f40 84 // Pack and send message
bhepp 2:5af0f0006f40 85 mavlink_message_t msg;
bhepp 6:9055fcc2f0cb 86 mavlink_msg_uwb_status_pack(mb.getSysId(), mb.getCompId(), &msg, 0xFF, buffer);
bhepp 2:5af0f0006f40 87 mb.sendMessage(msg);
bhepp 2:5af0f0006f40 88 }
bhepp 6:9055fcc2f0cb 89 } else if (comm_mode == UWBLINK) {
bhepp 6:9055fcc2f0cb 90 char buffer[MAX_UWB_LINK_FRAME_LENGTH];
bhepp 6:9055fcc2f0cb 91 int n = vsnprintf(buffer, sizeof(buffer), str, args);
bhepp 6:9055fcc2f0cb 92 if(n > sizeof(buffer)) {
bhepp 6:9055fcc2f0cb 93 // Dangerous: Could lead to infinite recursion
bhepp 6:9055fcc2f0cb 94 send_status_message(mb, ul, "%s %d buffer to small (buf_size: %d, required: %d)!\r\n", __FILE__, __LINE__, sizeof(buffer), n);
bhepp 6:9055fcc2f0cb 95 } else {
bhepp 6:9055fcc2f0cb 96 UWBMessageString msg_str(buffer);
bhepp 6:9055fcc2f0cb 97 UWBMessage msg(UWBMessage::UWB_MESSAGE_TYPE_STATUS, &msg_str);
bhepp 6:9055fcc2f0cb 98 if (!ul.sendMessage(msg)) {
bhepp 6:9055fcc2f0cb 99 DEBUG_PRINTF("\r\nSending UWBLink message failed\r\n");
bhepp 6:9055fcc2f0cb 100 }
bhepp 6:9055fcc2f0cb 101 }
bhepp 2:5af0f0006f40 102 } else {
bhepp 2:5af0f0006f40 103 pc.printf(str, args);
bhepp 2:5af0f0006f40 104 pc.printf("\r\n");
bhepp 2:5af0f0006f40 105 }
bhepp 2:5af0f0006f40 106 va_end(args);
bhepp 2:5af0f0006f40 107 }
bhepp 2:5af0f0006f40 108
bhepp 6:9055fcc2f0cb 109 bool measureTimesOfFlight(UWB2WayMultiRange& tracker, MAVLinkBridge& mb, UWBLink& ul, Timer& timer, float ranging_timeout = 0.1f)
bhepp 2:5af0f0006f40 110 {
bhepp 2:5af0f0006f40 111 #if _DEBUG
bhepp 2:5af0f0006f40 112 int time_begin_us = timer.read_us();
bhepp 2:5af0f0006f40 113 #endif
bhepp 2:5af0f0006f40 114
bhepp 1:c070ca30da80 115 #if MEASURE_UWB_RANGING_RATE
bhepp 1:c070ca30da80 116 static int32_t range_counter = 0;
bhepp 1:c070ca30da80 117 static uint32_t last_stamp_us = timer.read_us();
bhepp 1:c070ca30da80 118 #endif
bhepp 1:c070ca30da80 119
bhepp 5:1928eb8c417a 120 bool any_success = false;
bhepp 6:9055fcc2f0cb 121 for (int i = 0; i < NUM_OF_SLAVES; i++) {
bhepp 1:c070ca30da80 122 uint8_t remote_address = SLAVE_ADDRESS_OFFSET + i;
bhepp 2:5af0f0006f40 123 const UWB2WayMultiRange::RawRangingResult& raw_result = tracker.measureTimesOfFlight(remote_address, ranging_timeout);
bhepp 6:9055fcc2f0cb 124 if (raw_result.status == UWB2WayMultiRange::SUCCESS) {
bhepp 5:1928eb8c417a 125 any_success = true;
bhepp 6:9055fcc2f0cb 126 if (comm_mode == MAVLINK) {
bhepp 5:1928eb8c417a 127 uint64_t timestamp_master_request_1[MAVLINK_MAX_NUM_OF_DW_UNITS];
bhepp 5:1928eb8c417a 128 uint64_t timestamp_slave_reply[MAVLINK_MAX_NUM_OF_DW_UNITS];
bhepp 5:1928eb8c417a 129 uint64_t timestamp_master_request_2[MAVLINK_MAX_NUM_OF_DW_UNITS];
bhepp 6:9055fcc2f0cb 130 for (int j = 0; j < tracker.getNumOfModules(); ++j) {
bhepp 1:c070ca30da80 131 timestamp_master_request_1[j] = raw_result.timestamp_master_request_1[j];
bhepp 1:c070ca30da80 132 timestamp_slave_reply[j] = raw_result.timestamp_slave_reply[j];
bhepp 1:c070ca30da80 133 timestamp_master_request_2[j] = raw_result.timestamp_master_request_2[j];
bhepp 1:c070ca30da80 134 }
bhepp 6:9055fcc2f0cb 135 for (int j = tracker.getNumOfModules(); j < MAVLINK_MAX_NUM_OF_DW_UNITS; ++j) {
bhepp 1:c070ca30da80 136 timestamp_master_request_1[j] = 0;
bhepp 1:c070ca30da80 137 timestamp_slave_reply[j] = 0;
bhepp 1:c070ca30da80 138 timestamp_master_request_2[j] = 0;
bhepp 1:c070ca30da80 139 }
bhepp 1:c070ca30da80 140
bhepp 2:5af0f0006f40 141 // Pack and send message
bhepp 1:c070ca30da80 142 mavlink_message_t msg;
bhepp 2:5af0f0006f40 143 mavlink_msg_uwb_2way_multi_range_raw_4_pack(
bhepp 1:c070ca30da80 144 mb.getSysId(), mb.getCompId(), &msg,
bhepp 1:c070ca30da80 145 tracker.getNumOfModules(),
bhepp 1:c070ca30da80 146 tracker.getAddress(),
bhepp 1:c070ca30da80 147 remote_address,
bhepp 1:c070ca30da80 148 raw_result.timestamp_master_request_1_recv,
bhepp 1:c070ca30da80 149 raw_result.timestamp_slave_reply_send,
bhepp 1:c070ca30da80 150 raw_result.timestamp_master_request_2_recv,
bhepp 1:c070ca30da80 151 timestamp_master_request_1,
bhepp 1:c070ca30da80 152 timestamp_slave_reply,
bhepp 1:c070ca30da80 153 timestamp_master_request_2
bhepp 1:c070ca30da80 154 );
bhepp 1:c070ca30da80 155 mb.sendMessage(msg);
bhepp 6:9055fcc2f0cb 156 } else if (comm_mode == UWBLINK) {
bhepp 6:9055fcc2f0cb 157 UWBMessageMultiRange msg_multi_range(
bhepp 6:9055fcc2f0cb 158 tracker.getAddress(),
bhepp 6:9055fcc2f0cb 159 remote_address
bhepp 6:9055fcc2f0cb 160 );
bhepp 6:9055fcc2f0cb 161 for (int j = 0; j < tracker.getNumOfModules(); ++j) {
bhepp 6:9055fcc2f0cb 162 msg_multi_range.addModuleMeasurement(raw_result.timestamp_master_request_1[j], raw_result.timestamp_slave_reply[j], raw_result.timestamp_master_request_2[j]);
bhepp 6:9055fcc2f0cb 163 }
bhepp 6:9055fcc2f0cb 164 msg_multi_range.setSlaveMeasurement(raw_result.timestamp_master_request_1_recv, raw_result.timestamp_slave_reply_send, raw_result.timestamp_master_request_2_recv);
bhepp 6:9055fcc2f0cb 165 UWBMessage msg(UWBMessage::UWB_MESSAGE_TYPE_MULTI_RANGE, &msg_multi_range);
bhepp 6:9055fcc2f0cb 166 if (!ul.sendMessage(msg)) {
bhepp 6:9055fcc2f0cb 167 ERROR_PRINTF("\r\nSending UWBLink message failed\r\n");
bhepp 6:9055fcc2f0cb 168 }
bhepp 6:9055fcc2f0cb 169 } else {
bhepp 6:9055fcc2f0cb 170 for (int j = 0; j < tracker.getNumOfModules(); ++j) {
bhepp 1:c070ca30da80 171 int64_t timediff_slave = raw_result.timestamp_master_request_1_recv + raw_result.timestamp_master_request_2_recv - 2 * raw_result.timestamp_slave_reply_send;
bhepp 1:c070ca30da80 172 // Calculation of the summand on the sending node/beacon
bhepp 1:c070ca30da80 173 int64_t timediff_master = 2 * raw_result.timestamp_slave_reply[j] - raw_result.timestamp_master_request_1[j] - raw_result.timestamp_master_request_2[j];
bhepp 1:c070ca30da80 174 // Calculation of the resulting sum of all four ToFs.
bhepp 1:c070ca30da80 175 int64_t timediff = timediff_master + timediff_slave;
bhepp 1:c070ca30da80 176 float tof = tracker.convertDWTimeunitsToMicroseconds(timediff) / 4.0f;
bhepp 1:c070ca30da80 177 float range = tracker.convertTimeOfFlightToDistance(tof);
bhepp 1:c070ca30da80 178
bhepp 6:9055fcc2f0cb 179 send_status_message(mb, ul, "%d.%d - %d> range = %.2f, tof = %.2e", tracker.getAddress(), j, remote_address, range, tof);
bhepp 1:c070ca30da80 180 }
bhepp 1:c070ca30da80 181 }
bhepp 6:9055fcc2f0cb 182 } else {
bhepp 6:9055fcc2f0cb 183 send_status_message(mb, ul, "Ranging failed: %s - %s", UWB2WayMultiRange::RANGING_STATUS_MESSAGES[raw_result.status], raw_result.status_description);
bhepp 1:c070ca30da80 184 }
bhepp 1:c070ca30da80 185 #if MEASURE_UWB_RANGING_RATE
bhepp 1:c070ca30da80 186 ++range_counter;
bhepp 1:c070ca30da80 187 #endif
bhepp 1:c070ca30da80 188 }
bhepp 1:c070ca30da80 189
bhepp 1:c070ca30da80 190 #if MEASURE_UWB_RANGING_RATE
bhepp 1:c070ca30da80 191 uint32_t now_stamp_us = timer.read_us();
bhepp 1:c070ca30da80 192 uint32_t dt_us = now_stamp_us - last_stamp_us;
bhepp 1:c070ca30da80 193 if (dt_us > 2 * 1000 * 1000)
bhepp 1:c070ca30da80 194 {
bhepp 1:c070ca30da80 195 float rate = 1000 * 1000 * range_counter / ((float)dt_us);
bhepp 6:9055fcc2f0cb 196 send_status_message(mb, ul, "Rate = %f.2Hz", rate);
bhepp 6:9055fcc2f0cb 197 send_status_message(mb, ul, "range_counter = %d, stamp_us = %u, last_stamp_us = %u", range_counter, now_stamp_us, last_stamp_us);
bhepp 1:c070ca30da80 198 range_counter = 0;
bhepp 1:c070ca30da80 199 last_stamp_us = now_stamp_us;
bhepp 1:c070ca30da80 200 }
bhepp 1:c070ca30da80 201 #endif
bhepp 2:5af0f0006f40 202
bhepp 2:5af0f0006f40 203 #if _DEBUG
bhepp 2:5af0f0006f40 204 int time_end_us = timer.read_us();
bhepp 2:5af0f0006f40 205 int time_elapsed_us = time_end_us - time_begin_us;
bhepp 2:5af0f0006f40 206 int time_elapsed_ms = time_elapsed_us / 1000;
bhepp 2:5af0f0006f40 207 DEBUG_PRINTF_VA("Time elapsed for ranging and output: %d ms (%d microseconds)\r\n", time_elapsed_ms, time_elapsed_us);
bhepp 2:5af0f0006f40 208 DEBUG_PRINTF("\r\n\r\n");
bhepp 2:5af0f0006f40 209 #endif
bhepp 2:5af0f0006f40 210
bhepp 2:5af0f0006f40 211 #if _DEBUG
bhepp 6:9055fcc2f0cb 212 ////wait_ms(500);
bhepp 2:5af0f0006f40 213 #endif
bhepp 5:1928eb8c417a 214
bhepp 5:1928eb8c417a 215 return any_success;
bhepp 1:c070ca30da80 216 }
bhepp 1:c070ca30da80 217
bhepp 1:c070ca30da80 218 int main()
bhepp 1:c070ca30da80 219 {
bhepp 5:1928eb8c417a 220 while (true) {
bhepp 1:c070ca30da80 221
bhepp 5:1928eb8c417a 222 const PinName CS_PINS[5] = {D8, D9, D10, D14, D15};
bhepp 5:1928eb8c417a 223 for (int i = 0; i < sizeof(CS_PINS) / sizeof(PinName); ++i) {
bhepp 5:1928eb8c417a 224 DigitalOut out(CS_PINS[i]);
bhepp 5:1928eb8c417a 225 out = 1;
bhepp 5:1928eb8c417a 226 }
bhepp 5:1928eb8c417a 227
bhepp 5:1928eb8c417a 228 UART_Mbed uart(&pc);
bhepp 5:1928eb8c417a 229 MAVLinkBridge mb(&uart);
bhepp 6:9055fcc2f0cb 230 UWBLinkMbed ul(&pc, MAX_UWB_LINK_FRAME_LENGTH);
bhepp 6:9055fcc2f0cb 231
bhepp 6:9055fcc2f0cb 232 // while (true) {
bhepp 6:9055fcc2f0cb 233 // UWBMessage msg(UWBMessage::UWB_MESSAGE_TYPE_NOP);
bhepp 6:9055fcc2f0cb 234 // if (!ul.sendMessage(msg)) {
bhepp 6:9055fcc2f0cb 235 // ERROR_PRINTF("\r\nSending UWBLink message failed\r\n");
bhepp 6:9055fcc2f0cb 236 // }
bhepp 6:9055fcc2f0cb 237 // }
bhepp 6:9055fcc2f0cb 238
bhepp 5:1928eb8c417a 239 SPI spi(DW_MOSI_PIN, DW_MISO_PIN, DW_SCLK_PIN);
bhepp 5:1928eb8c417a 240 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 5:1928eb8c417a 241 // NOTE: Minimum Frequency 1MHz. Below it is now working. Could be something with the activation and deactivation of interrupts.
bhepp 5:1928eb8c417a 242 spi.frequency(SPI_FREQUENCY); // with a 1MHz clock rate (worked up to 49MHz in our Test)
bhepp 5:1928eb8c417a 243
bhepp 5:1928eb8c417a 244 Timer timer;
bhepp 5:1928eb8c417a 245 timer.start();
bhepp 5:1928eb8c417a 246
bhepp 5:1928eb8c417a 247 DW1000* dw_array[NUM_OF_DW_UNITS];
bhepp 6:9055fcc2f0cb 248 bool dw_init_success[NUM_OF_DW_UNITS];
bhepp 5:1928eb8c417a 249 InterruptIn* irq = NULL;
bhepp 5:1928eb8c417a 250
bhepp 6:9055fcc2f0cb 251 send_status_message(mb, ul, "Performing hardware reset of UWB modules\r\n");
bhepp 5:1928eb8c417a 252 // == IMPORTANT == Create all DW objects first (this will cause a reset of the DW module)
bhepp 5:1928eb8c417a 253 DW1000::hardwareReset(DW_RESET_PIN);
bhepp 6:9055fcc2f0cb 254
bhepp 5:1928eb8c417a 255 // Now we can initialize the DW modules
bhepp 6:9055fcc2f0cb 256 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 6:9055fcc2f0cb 257 dw_init_success[i] = true;
bhepp 5:1928eb8c417a 258 dw_array[i] = new DW1000(spi, irq, DW_CS_PINS[i]); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
bhepp 5:1928eb8c417a 259
bhepp 5:1928eb8c417a 260 DW1000& dw = *dw_array[i];
bhepp 6:9055fcc2f0cb 261 uint64_t eui = 0xFAEDCD01FAEDCD01 + i;
bhepp 6:9055fcc2f0cb 262 dw.setEUI(eui); // basic methods called to check if we have a working SPI connection
bhepp 6:9055fcc2f0cb 263 uint64_t query_eui = dw.getEUI();
bhepp 6:9055fcc2f0cb 264 float query_voltage = dw.getVoltage();
bhepp 6:9055fcc2f0cb 265 uint32_t query_device_id = dw.getDeviceID();
bhepp 5:1928eb8c417a 266
bhepp 6:9055fcc2f0cb 267 send_status_message(mb, ul, "\r\nUnit %d", i);
bhepp 6:9055fcc2f0cb 268 send_status_message(mb, ul, "\r\nDecaWave 1.0 up and running!"); // Splashscreen
bhepp 6:9055fcc2f0cb 269 send_status_message(mb, ul, "DEVICE_ID register: 0x%X", query_device_id);
bhepp 6:9055fcc2f0cb 270 send_status_message(mb, ul, "EUI register: %016llX", query_eui);
bhepp 6:9055fcc2f0cb 271 send_status_message(mb, ul, "Voltage: %.2fV", query_voltage);
bhepp 6:9055fcc2f0cb 272
bhepp 6:9055fcc2f0cb 273 if (query_eui != eui) {
bhepp 6:9055fcc2f0cb 274 send_status_message(mb, ul, "ERROR: Queried EUI is not equal to the configured EUI");
bhepp 6:9055fcc2f0cb 275 dw_init_success[i] = false;
bhepp 6:9055fcc2f0cb 276 continue;
bhepp 6:9055fcc2f0cb 277 }
bhepp 6:9055fcc2f0cb 278
bhepp 6:9055fcc2f0cb 279 if (query_voltage < MIN_DW_VOLTAGE) {
bhepp 6:9055fcc2f0cb 280 send_status_message(mb, ul, "ERROR: Queried voltage is below minimum value");
bhepp 6:9055fcc2f0cb 281 dw_init_success[i] = false;
bhepp 6:9055fcc2f0cb 282 continue;
bhepp 6:9055fcc2f0cb 283 }
bhepp 5:1928eb8c417a 284
bhepp 5:1928eb8c417a 285 // Set NLOS settings (According to DecaWave Application Note APS006)
bhepp 6:9055fcc2f0cb 286 if (USE_NLOS_SETTINGS) {
bhepp 6:9055fcc2f0cb 287 send_status_message(mb, ul, "Setting NLOS configuration for Unit %d", i);
bhepp 5:1928eb8c417a 288 DW1000Utils::setNLOSSettings(&dw, DATA_RATE_SETTING, PRF_SETTING, PREAMBLE_SETTING);
bhepp 5:1928eb8c417a 289 }
bhepp 5:1928eb8c417a 290 }
bhepp 6:9055fcc2f0cb 291
bhepp 6:9055fcc2f0cb 292 bool all_dw_initialized = true;
bhepp 6:9055fcc2f0cb 293 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 6:9055fcc2f0cb 294 if (!dw_init_success[i]) {
bhepp 6:9055fcc2f0cb 295 all_dw_initialized = false;
bhepp 6:9055fcc2f0cb 296 break;
bhepp 6:9055fcc2f0cb 297 }
bhepp 5:1928eb8c417a 298 }
bhepp 6:9055fcc2f0cb 299
bhepp 6:9055fcc2f0cb 300 if (all_dw_initialized) {
bhepp 6:9055fcc2f0cb 301 send_status_message(mb, ul, "Initializing tracker with address %d", TRACKER_ADDRESS);
bhepp 6:9055fcc2f0cb 302 UWB2WayMultiRange tracker(TRACKER_ADDRESS);
bhepp 6:9055fcc2f0cb 303
bhepp 6:9055fcc2f0cb 304 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 6:9055fcc2f0cb 305 tracker.addModule(dw_array[i]);
bhepp 6:9055fcc2f0cb 306 }
bhepp 6:9055fcc2f0cb 307
bhepp 6:9055fcc2f0cb 308 send_status_message(mb, ul, "Resetting all modules and waiting 100ms");
bhepp 6:9055fcc2f0cb 309 for (int i = 0; i < tracker.getNumOfModules(); ++i) {
bhepp 6:9055fcc2f0cb 310 tracker.getModule(i)->softwareReset();
bhepp 6:9055fcc2f0cb 311 }
bhepp 6:9055fcc2f0cb 312 wait_us(100); // This helps sometimes
bhepp 6:9055fcc2f0cb 313
bhepp 6:9055fcc2f0cb 314 Timer watchdog_timer;
bhepp 6:9055fcc2f0cb 315 watchdog_timer.start();
bhepp 6:9055fcc2f0cb 316 float watchdog_last_time = watchdog_timer.read();
bhepp 6:9055fcc2f0cb 317 bool watchdog_timeout_flag = false;
bhepp 5:1928eb8c417a 318
bhepp 6:9055fcc2f0cb 319 send_status_message(mb, ul, "Entering main loop");
bhepp 6:9055fcc2f0cb 320 while (!watchdog_timeout_flag) {
bhepp 6:9055fcc2f0cb 321 bool success = measureTimesOfFlight(tracker, mb, ul, timer);
bhepp 6:9055fcc2f0cb 322 float current_time = watchdog_timer.read();
bhepp 6:9055fcc2f0cb 323 if (success) {
bhepp 6:9055fcc2f0cb 324 watchdog_last_time = current_time;
bhepp 6:9055fcc2f0cb 325 } else {
bhepp 6:9055fcc2f0cb 326 float watchdog_time = current_time - watchdog_last_time;
bhepp 6:9055fcc2f0cb 327 if (watchdog_time > WATCHDOG_TIMEOUT) {
bhepp 6:9055fcc2f0cb 328 watchdog_timeout_flag = true;
bhepp 6:9055fcc2f0cb 329 }
bhepp 5:1928eb8c417a 330 }
bhepp 5:1928eb8c417a 331 }
bhepp 6:9055fcc2f0cb 332 send_status_message(mb, ul, "");
bhepp 6:9055fcc2f0cb 333 send_status_message(mb, ul, "Watchdog timed out ...");
bhepp 6:9055fcc2f0cb 334 send_status_message(mb, ul, "Restarting ...");
bhepp 6:9055fcc2f0cb 335 send_status_message(mb, ul, "");
bhepp 6:9055fcc2f0cb 336 } else {
bhepp 6:9055fcc2f0cb 337 send_status_message(mb, ul, "\r\nNot all DW units could be initialized.");
bhepp 6:9055fcc2f0cb 338 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 6:9055fcc2f0cb 339 if (!dw_init_success[i]) {
bhepp 6:9055fcc2f0cb 340 send_status_message(mb, ul, "Failed to initialize DW unit %d", i);
bhepp 6:9055fcc2f0cb 341 }
bhepp 6:9055fcc2f0cb 342 }
bhepp 6:9055fcc2f0cb 343 send_status_message(mb, ul, "Trying again ...");
bhepp 6:9055fcc2f0cb 344 wait_ms(1000);
bhepp 5:1928eb8c417a 345 }
bhepp 1:c070ca30da80 346
bhepp 6:9055fcc2f0cb 347 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 5:1928eb8c417a 348 delete dw_array[i];
bhepp 1:c070ca30da80 349 }
bhepp 1:c070ca30da80 350 }
bhepp 1:c070ca30da80 351 }
bhepp 1:c070ca30da80 352 #endif