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@2:5af0f0006f40, 2016-02-13 (annotated)

Committer:
bhepp
Date:
Sat Feb 13 17:11:04 2016 +0000
Revision:
2:5af0f0006f40
Parent:
1:c070ca30da80
Child:
4:1a2c1e5e5516
Added mavlink status messages and fixed use of buffered serial port

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