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@5:1928eb8c417a, 2016-03-31 (annotated)

Committer:
bhepp
Date:
Thu Mar 31 15:39:52 2016 +0000
Revision:
5:1928eb8c417a
Parent:
4:1a2c1e5e5516
Child:
6:9055fcc2f0cb
A lot of cleanup

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