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@9:3844cf7e5c00, 2016-04-05 (annotated)

Committer:
bhepp
Date:
Tue Apr 05 12:20:29 2016 +0000
Revision:
9:3844cf7e5c00
Parent:
6:9055fcc2f0cb
Child:
10:6e95128c2efa
Added comment for CS pins

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