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@1:c070ca30da80, 2016-02-11 (annotated)

Committer:
bhepp
Date:
Thu Feb 11 10:49:49 2016 +0000
Revision:
1:c070ca30da80
Child:
2:5af0f0006f40
Multi Ranging working

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