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