Tobias Naegeli / Mbed 2 deprecated AIT_UWB_Range

Tobi's ubw test branch

Dependencies:   mavlink_bridge mbed

Fork of AIT_UWB_Range by Benjamin Hepp

main.cpp@50:50b8aea54a51, 2015-11-26 (annotated)

Committer:
bhepp
Date:
Thu Nov 26 21:42:51 2015 +0000
Revision:
50:50b8aea54a51
Parent:
48:5999e510f154
Child:
51:e9391d04af00
Multiple receiver basically working.; ; * Receiving from two DWM1000 modules working.; * Still issues with interrupts. Cleanup necessary.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manumaet 24:6f25ba679490 1 // by Matthias Grob & Manuel Stalder - ETH Zürich - 2015
manumaet 0:f50e671ffff7 2 #include "mbed.h"
manumaet 26:a65c6f26c458 3 #include "PC.h" // Serial Port via USB for debugging with Terminal
manumaet 27:71178fdb78e1 4 #include "DW1000.h" // our DW1000 device driver
manumaet 44:2e0045042a59 5 #include "MM2WayRanging.h" // our self developed ranging application
bhepp 48:5999e510f154 6 #include "InterruptMultiplexer.h"
manumaet 44:2e0045042a59 7
bhepp 48:5999e510f154 8 #include "mavlink_bridge/mavlink_bridge.h"
bhepp 48:5999e510f154 9
bhepp 48:5999e510f154 10 #define ANCHOR false
bhepp 48:5999e510f154 11
bhepp 48:5999e510f154 12 using namespace ait;
manumaet 44:2e0045042a59 13
bhepp 48:5999e510f154 14 //const int ANCHOR_ADDRESS_OFFSET = 100;
bhepp 50:50b8aea54a51 15 const int SPI_FREQUENCY = 1000000;
bhepp 48:5999e510f154 16 const int ANCHOR_ADDRESS_OFFSET = 1;
bhepp 48:5999e510f154 17 const bool USE_NLOS_SETTINGS = true;
bhepp 48:5999e510f154 18 const PinName DW_RESET_PIN = D15;
bhepp 48:5999e510f154 19 const PinName DW_IRQ_PIN = D14;
bhepp 48:5999e510f154 20 const PinName DW_MOSI_PIN = D11;
bhepp 48:5999e510f154 21 const PinName DW_MISO_PIN = D12;
bhepp 48:5999e510f154 22 const PinName DW_SCLK_PIN = D13;
bhepp 48:5999e510f154 23 #if ANCHOR
bhepp 48:5999e510f154 24 const bool MAVLINK_COMM = false;
bhepp 48:5999e510f154 25 const int NUM_OF_DW_UNITS = 1;
bhepp 48:5999e510f154 26 const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
bhepp 48:5999e510f154 27 #else
bhepp 48:5999e510f154 28 // const bool MAVLINK_COMM = true;
bhepp 48:5999e510f154 29 const bool MAVLINK_COMM = false;
bhepp 48:5999e510f154 30 const int NUM_OF_DW_UNITS = 2;
bhepp 50:50b8aea54a51 31 const PinName DW_CS_PINS[2] = {D10, D9};
bhepp 48:5999e510f154 32 //const int NUM_OF_DW_UNITS = 1;
bhepp 48:5999e510f154 33 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
bhepp 48:5999e510f154 34 #endif
bhepp 48:5999e510f154 35
bhepp 48:5999e510f154 36 PC pc(USBTX, USBRX, 115200); // USB UART Terminal
bhepp 48:5999e510f154 37
bhepp 48:5999e510f154 38 void rangeAndDisplayAll(MM2WayRanging& node, MAVLinkBridge& mb, Timer& timer) {
bhepp 48:5999e510f154 39 node.requestRanging(ANCHOR_ADDRESS_OFFSET); // Request ranging to all anchors
bhepp 48:5999e510f154 40 for (int i = 1; i <= 1; i++) { // Output Results
bhepp 48:5999e510f154 41 if (MAVLINK_COMM) {
bhepp 48:5999e510f154 42 uint8_t address = node.address;
bhepp 48:5999e510f154 43 uint8_t remote_address = i;
bhepp 48:5999e510f154 44 uint32_t stamp_us = timer.read_us();
bhepp 48:5999e510f154 45 float round_trip_time = node.roundtriptimes[i];
bhepp 48:5999e510f154 46 float range = node.distances[i];
bhepp 48:5999e510f154 47 // Initialize the required buffers
bhepp 48:5999e510f154 48 mavlink_message_t msg;
bhepp 48:5999e510f154 49 // Pack the message
bhepp 48:5999e510f154 50 mavlink_msg_uwb_range_pack(mb.getSysId(), mb.getCompId(), &msg, address, remote_address, stamp_us, round_trip_time, range);
bhepp 48:5999e510f154 51 mb.sendMessage(msg);
manumaet 0:f50e671ffff7 52
bhepp 48:5999e510f154 53 uint16_t std_noise_1 = node.reception_stats[i][0].std_noise;
bhepp 48:5999e510f154 54 uint16_t std_noise_2 = node.reception_stats[i][1].std_noise;
bhepp 48:5999e510f154 55 uint16_t std_noise_3 = node.reception_stats[i][2].std_noise;
bhepp 48:5999e510f154 56 uint16_t preamble_acc_count_1 = node.reception_stats[i][0].preamble_acc_count;
bhepp 48:5999e510f154 57 uint16_t preamble_acc_count_2 = node.reception_stats[i][1].preamble_acc_count;
bhepp 48:5999e510f154 58 uint16_t preamble_acc_count_3 = node.reception_stats[i][2].preamble_acc_count;
bhepp 48:5999e510f154 59 uint16_t first_path_index_1 = node.reception_stats[i][0].first_path_index;
bhepp 48:5999e510f154 60 uint16_t first_path_index_2 = node.reception_stats[i][1].first_path_index;
bhepp 48:5999e510f154 61 uint16_t first_path_index_3 = node.reception_stats[i][2].first_path_index;
bhepp 48:5999e510f154 62 uint16_t first_path_amp_1_1 = node.reception_stats[i][0].first_path_amp_1;
bhepp 48:5999e510f154 63 uint16_t first_path_amp_1_2 = node.reception_stats[i][1].first_path_amp_1;
bhepp 48:5999e510f154 64 uint16_t first_path_amp_1_3 = node.reception_stats[i][2].first_path_amp_1;
bhepp 48:5999e510f154 65 uint16_t first_path_amp_2_1 = node.reception_stats[i][0].first_path_amp_2;
bhepp 48:5999e510f154 66 uint16_t first_path_amp_2_2 = node.reception_stats[i][1].first_path_amp_2;
bhepp 48:5999e510f154 67 uint16_t first_path_amp_2_3 = node.reception_stats[i][2].first_path_amp_2;
bhepp 48:5999e510f154 68 uint16_t first_path_amp_3_1 = node.reception_stats[i][0].first_path_amp_3;
bhepp 48:5999e510f154 69 uint16_t first_path_amp_3_2 = node.reception_stats[i][1].first_path_amp_3;
bhepp 48:5999e510f154 70 uint16_t first_path_amp_3_3 = node.reception_stats[i][2].first_path_amp_3;
bhepp 48:5999e510f154 71 uint16_t channel_impulse_response_power_1 = node.reception_stats[i][0].channel_impulse_response_power;
bhepp 48:5999e510f154 72 uint16_t channel_impulse_response_power_2 = node.reception_stats[i][1].channel_impulse_response_power;
bhepp 48:5999e510f154 73 uint16_t channel_impulse_response_power_3 = node.reception_stats[i][2].channel_impulse_response_power;
bhepp 48:5999e510f154 74 uint8_t prf_1 = node.reception_stats[i][0].prf;
bhepp 48:5999e510f154 75 uint8_t prf_2 = node.reception_stats[i][1].prf;
bhepp 48:5999e510f154 76 uint8_t prf_3 = node.reception_stats[i][2].prf;
bhepp 48:5999e510f154 77 // Pack the message
bhepp 48:5999e510f154 78 mavlink_msg_uwb_range_stats_pack(
bhepp 48:5999e510f154 79 mb.getSysId(), mb.getCompId(), &msg,
bhepp 48:5999e510f154 80 address, remote_address, stamp_us,
bhepp 48:5999e510f154 81 round_trip_time, range,
bhepp 48:5999e510f154 82 std_noise_1, std_noise_2, std_noise_2,
bhepp 48:5999e510f154 83 preamble_acc_count_1, preamble_acc_count_2, preamble_acc_count_3,
bhepp 48:5999e510f154 84 first_path_index_1, first_path_index_2, first_path_index_3,
bhepp 48:5999e510f154 85 first_path_amp_1_1, first_path_amp_1_2, first_path_amp_1_3,
bhepp 48:5999e510f154 86 first_path_amp_2_1, first_path_amp_2_2, first_path_amp_2_3,
bhepp 48:5999e510f154 87 first_path_amp_3_1, first_path_amp_3_2, first_path_amp_3_3,
bhepp 48:5999e510f154 88 channel_impulse_response_power_1, channel_impulse_response_power_2, channel_impulse_response_power_3,
bhepp 48:5999e510f154 89 prf_1, prf_2, prf_3
bhepp 48:5999e510f154 90 );
bhepp 48:5999e510f154 91 mb.sendMessage(msg);
bhepp 48:5999e510f154 92
bhepp 48:5999e510f154 93 /*mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise1", std_noise1);
bhepp 48:5999e510f154 94 mb.sendMessage(msg);
bhepp 48:5999e510f154 95 mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise2", std_noise2);
bhepp 48:5999e510f154 96 mb.sendMessage(msg);
bhepp 48:5999e510f154 97 mavlink_msg_named_value_int_pack(mb.getSysId(), mb.getCompId(), &msg, stamp_us, "noise3", std_noise3);
bhepp 48:5999e510f154 98 mb.sendMessage(msg);*/
bhepp 48:5999e510f154 99 } else {
bhepp 50:50b8aea54a51 100 pc.printf("%d> dist = >%f, ", node.address, node.distances[i]);
bhepp 50:50b8aea54a51 101 pc.printf("%d> rtt = %f", node.address, node.roundtriptimes[i]);
bhepp 48:5999e510f154 102 pc.printf("\r\n");
bhepp 48:5999e510f154 103 }
manumaet 44:2e0045042a59 104 }
manumaet 44:2e0045042a59 105 }
manumaet 6:d5864a1b9e17 106
bhepp 48:5999e510f154 107 void calibrationRanging(MM2WayRanging& node, int destination){
manumaet 45:01a33363bc21 108 const int numberOfRangings = 100;
manumaet 45:01a33363bc21 109 float rangings[numberOfRangings];
manumaet 45:01a33363bc21 110 int index = 0;
manumaet 45:01a33363bc21 111 float mean = 0;
manumaet 45:01a33363bc21 112 float start, stop;
manumaet 45:01a33363bc21 113
manumaet 45:01a33363bc21 114 Timer localTimer;
manumaet 45:01a33363bc21 115 localTimer.start();
manumaet 45:01a33363bc21 116
manumaet 45:01a33363bc21 117 start = localTimer.read();
manumaet 45:01a33363bc21 118
manumaet 45:01a33363bc21 119 while (1) {
manumaet 45:01a33363bc21 120
manumaet 45:01a33363bc21 121 node.requestRanging(destination);
manumaet 45:01a33363bc21 122 if(node.overflow){
bhepp 48:5999e510f154 123 pc.printf("Overflow! Measured: %f\r\n", node.distances[destination]); // This is the output to see if a timer overflow was corrected by the two way ranging class
manumaet 45:01a33363bc21 124 }
manumaet 45:01a33363bc21 125
manumaet 45:01a33363bc21 126 if (node.distances[destination] == -1) {
bhepp 48:5999e510f154 127 pc.printf("Measurement timed out\r\n");
manumaet 45:01a33363bc21 128 wait(0.001);
manumaet 45:01a33363bc21 129 continue;
manumaet 45:01a33363bc21 130 }
manumaet 45:01a33363bc21 131
manumaet 45:01a33363bc21 132 if (node.distances[destination] < 100) {
manumaet 45:01a33363bc21 133 rangings[index] = node.distances[destination];
bhepp 48:5999e510f154 134 //pc.printf("%f\r\n", node.distances[destination]);
manumaet 45:01a33363bc21 135 index++;
manumaet 45:01a33363bc21 136
manumaet 45:01a33363bc21 137 if (index == numberOfRangings) {
bhepp 48:5999e510f154 138 stop = localTimer.read();
manumaet 45:01a33363bc21 139
manumaet 45:01a33363bc21 140 for (int i = 0; i < numberOfRangings - 1; i++)
manumaet 45:01a33363bc21 141 rangings[numberOfRangings - 1] += rangings[i];
manumaet 45:01a33363bc21 142
manumaet 45:01a33363bc21 143 mean = rangings[numberOfRangings - 1] / numberOfRangings;
bhepp 48:5999e510f154 144 pc.printf("\r\n\r\nMean %i: %f\r\n", destination, mean);
bhepp 48:5999e510f154 145 pc.printf("Elapsed Time for %i: %f\r\n", numberOfRangings, stop - start);
manumaet 45:01a33363bc21 146
manumaet 45:01a33363bc21 147 mean = 0;
manumaet 45:01a33363bc21 148 index = 0;
manumaet 45:01a33363bc21 149
manumaet 45:01a33363bc21 150 //wait(2);
manumaet 45:01a33363bc21 151
manumaet 45:01a33363bc21 152 start = localTimer.read();
manumaet 45:01a33363bc21 153 }
manumaet 45:01a33363bc21 154 }
bhepp 48:5999e510f154 155 else
bhepp 48:5999e510f154 156 pc.printf("%f\r\n", node.distances[destination]);
bhepp 48:5999e510f154 157 }
bhepp 48:5999e510f154 158 }
manumaet 45:01a33363bc21 159
manumaet 46:6398237672a0 160 struct __attribute__((packed, aligned(1))) DistancesFrame {
bhepp 48:5999e510f154 161 uint8_t source;
bhepp 48:5999e510f154 162 uint8_t destination;
bhepp 48:5999e510f154 163 uint8_t type;
bhepp 48:5999e510f154 164 float dist[4];
bhepp 48:5999e510f154 165 };
manumaet 45:01a33363bc21 166
bhepp 48:5999e510f154 167
bhepp 48:5999e510f154 168 // -----------------------------------------------------------------------------------------------
manumaet 46:6398237672a0 169 void altCallbackRX();
manumaet 45:01a33363bc21 170
manumaet 0:f50e671ffff7 171 int main() {
bhepp 50:50b8aea54a51 172 DigitalOut cs(D8);
bhepp 50:50b8aea54a51 173 cs = 1;
bhepp 50:50b8aea54a51 174
bhepp 50:50b8aea54a51 175 UART_Mbed uart(USBTX, USBRX, 115200);
bhepp 50:50b8aea54a51 176 MAVLinkBridge mb(&uart);
bhepp 50:50b8aea54a51 177
bhepp 50:50b8aea54a51 178 if (!MAVLINK_COMM) {
bhepp 50:50b8aea54a51 179 pc.printf("==== AIT UWB Range ====\r\n");
bhepp 50:50b8aea54a51 180 }
bhepp 48:5999e510f154 181
bhepp 48:5999e510f154 182 SPI spi(DW_MOSI_PIN, DW_MISO_PIN, DW_SCLK_PIN);
bhepp 50:50b8aea54a51 183 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 48:5999e510f154 184 // NOTE: Minimum Frequency 1MHz. Below it is now working. Could be something with the activation and deactivation of interrupts.
bhepp 50:50b8aea54a51 185 spi.frequency(SPI_FREQUENCY); // with a 1MHz clock rate (worked up to 49MHz in our Test)
bhepp 50:50b8aea54a51 186
bhepp 50:50b8aea54a51 187 // Setup interrupt pin
bhepp 50:50b8aea54a51 188 InterruptMultiplexer irq_mp(DW_IRQ_PIN);
bhepp 50:50b8aea54a51 189 irq_mp.getIRQ().rise(&irq_mp, &InterruptMultiplexer::trigger); // attach interrupt handler to rising edge of interrupt pin from DW1000
bhepp 50:50b8aea54a51 190
bhepp 50:50b8aea54a51 191 Timer timer;
bhepp 50:50b8aea54a51 192 timer.start();
bhepp 50:50b8aea54a51 193
bhepp 50:50b8aea54a51 194 while (true) {
bhepp 48:5999e510f154 195
bhepp 48:5999e510f154 196 DW1000* dw_array[NUM_OF_DW_UNITS + 2];
bhepp 48:5999e510f154 197 MM2WayRanging* node_array[NUM_OF_DW_UNITS + 2]; // Instance of the two way ranging algorithm
bhepp 48:5999e510f154 198
bhepp 50:50b8aea54a51 199 if (!MAVLINK_COMM) {
bhepp 50:50b8aea54a51 200 pc.printf("Performing hardware reset of UWB modules\r\n");
bhepp 50:50b8aea54a51 201 }
bhepp 48:5999e510f154 202 // == IMPORTANT == Create all DW objects first (this will cause a reset of the DW module)
bhepp 48:5999e510f154 203 DW1000::hardwareReset(DW_RESET_PIN);
bhepp 48:5999e510f154 204 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 48:5999e510f154 205 dw_array[i] = new DW1000(spi, irq_mp, DW_CS_PINS[i]); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
bhepp 48:5999e510f154 206 //dw_array[i]->disable_irq();
bhepp 48:5999e510f154 207 }
bhepp 48:5999e510f154 208
bhepp 48:5999e510f154 209 // Now we can initialize the DW modules
bhepp 48:5999e510f154 210 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 48:5999e510f154 211 DW1000& dw = *dw_array[i];
bhepp 48:5999e510f154 212 dw.setEUI(0xFAEDCD01FAEDCD01 + i); // basic methods called to check if we have a working SPI connection
bhepp 48:5999e510f154 213 node_array[i] = new MM2WayRanging(*dw_array[i]);
bhepp 48:5999e510f154 214 if (MAVLINK_COMM) {
bhepp 48:5999e510f154 215 // TODO
bhepp 48:5999e510f154 216 } else {
bhepp 48:5999e510f154 217 pc.printf("\r\nUnit %d\r\n", i);
bhepp 48:5999e510f154 218 pc.printf("\r\nDecaWave 1.0 up and running!\r\n"); // Splashscreen
bhepp 48:5999e510f154 219 pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
bhepp 48:5999e510f154 220 pc.printf("EUI register: %016llX\r\n", dw.getEUI());
bhepp 50:50b8aea54a51 221 pc.printf("Voltage: %.2fV\r\n", dw.getVoltage());
bhepp 48:5999e510f154 222 }
manumaet 44:2e0045042a59 223 }
bhepp 48:5999e510f154 224
bhepp 48:5999e510f154 225 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 48:5999e510f154 226 DW1000& dw = *dw_array[i];
bhepp 48:5999e510f154 227 MM2WayRanging& node = *node_array[i];
bhepp 48:5999e510f154 228 node.isAnchor = ANCHOR; // declare as anchor or beacon
bhepp 48:5999e510f154 229 if (ANCHOR) {
bhepp 48:5999e510f154 230 node.address = ANCHOR_ADDRESS_OFFSET + i;
bhepp 48:5999e510f154 231 if (!MAVLINK_COMM)
bhepp 48:5999e510f154 232 pc.printf("This node is Anchor node %d\r\n", node.address);
bhepp 48:5999e510f154 233 } else {
bhepp 48:5999e510f154 234 node.address = i;
bhepp 48:5999e510f154 235 if (!MAVLINK_COMM)
bhepp 50:50b8aea54a51 236 pc.printf("This node is a Beacon\r\n");
bhepp 48:5999e510f154 237 }
manumaet 47:b6120c152ad1 238 }
bhepp 48:5999e510f154 239
bhepp 48:5999e510f154 240 // Set NLOS settings
bhepp 48:5999e510f154 241 if (USE_NLOS_SETTINGS) {
bhepp 48:5999e510f154 242 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 50:50b8aea54a51 243 if (!MAVLINK_COMM)
bhepp 50:50b8aea54a51 244 pc.printf("Setting NLOS configuration for Unit %d\r\n", i);
bhepp 48:5999e510f154 245 DW1000& dw = *dw_array[i];
bhepp 48:5999e510f154 246 dw.writeRegister8(DW1000_LDE_CTRL, 0x0806, 0x07); //LDE_CFG1
bhepp 48:5999e510f154 247 dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x0003); //LDE_CFG2
bhepp 48:5999e510f154 248 //dw.writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1603); //LDE_CFG2
bhepp 48:5999e510f154 249 // Channel control
bhepp 48:5999e510f154 250 uint16_t prf_mask = (0x1 << 19) | (0x1 << 18);
bhepp 48:5999e510f154 251 uint16_t prf = 0x01 << 18; // 16 MHz
bhepp 48:5999e510f154 252 uint16_t v = dw.readRegister16(DW1000_CHAN_CTRL, 0x00);
bhepp 48:5999e510f154 253 v &= ~prf_mask;
bhepp 48:5999e510f154 254 v |= prf;
bhepp 48:5999e510f154 255 dw.writeRegister16(DW1000_CHAN_CTRL, 0x00, v);
bhepp 48:5999e510f154 256 }
bhepp 48:5999e510f154 257 }
bhepp 48:5999e510f154 258
bhepp 50:50b8aea54a51 259 if (!MAVLINK_COMM)
bhepp 50:50b8aea54a51 260 pc.printf("Entering main loop\r\n");
bhepp 48:5999e510f154 261 while (true) {
bhepp 50:50b8aea54a51 262 //for (int i = 0; i < 10; ++i) {
bhepp 48:5999e510f154 263 if (ANCHOR) {
bhepp 48:5999e510f154 264 pc.printf("."); // to see if the core and output is working
bhepp 48:5999e510f154 265 wait(0.5);
bhepp 48:5999e510f154 266 } else {
bhepp 48:5999e510f154 267 //for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
bhepp 48:5999e510f154 268 for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
bhepp 48:5999e510f154 269 MM2WayRanging& node = *node_array[j];
bhepp 48:5999e510f154 270 rangeAndDisplayAll(node, mb, timer);
bhepp 48:5999e510f154 271 }
manumaet 45:01a33363bc21 272 //calibrationRanging(4);
manumaet 47:b6120c152ad1 273 }
manumaet 0:f50e671ffff7 274 }
bhepp 50:50b8aea54a51 275
bhepp 50:50b8aea54a51 276 for (int i = 0; i < NUM_OF_DW_UNITS; ++i) {
bhepp 50:50b8aea54a51 277 delete node_array[i];
bhepp 50:50b8aea54a51 278 delete dw_array[i];
bhepp 50:50b8aea54a51 279 }
bhepp 50:50b8aea54a51 280
bhepp 50:50b8aea54a51 281 }
manumaet 46:6398237672a0 282 }
manumaet 46:6398237672a0 283
manumaet 46:6398237672a0 284
bhepp 48:5999e510f154 285 void altCallbackRX(DW1000& dw) { // this callback was used in our verification test case for the observer node which only receives and outputs the resulting data
manumaet 46:6398237672a0 286 DistancesFrame distFrame;
manumaet 46:6398237672a0 287 float distances[4];
manumaet 46:6398237672a0 288 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&distFrame, dw.getFramelength());
bhepp 48:5999e510f154 289
manumaet 47:b6120c152ad1 290 if (distFrame.destination == 5) {
manumaet 46:6398237672a0 291 for (int i = 0; i<4; i++){
bhepp 48:5999e510f154 292 pc.printf("%f, ", distFrame.dist[i]);
manumaet 46:6398237672a0 293 }
bhepp 48:5999e510f154 294 pc.printf("\r\n");
manumaet 46:6398237672a0 295 }
manumaet 46:6398237672a0 296 dw.startRX();
manumaet 46:6398237672a0 297 }