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@57:b4e7324b0faf, 2016-01-04 (annotated)

Committer:
naegelit
Date:
Mon Jan 04 20:06:38 2016 +0000
Revision:
57:b4e7324b0faf
Parent:
56:2eba104e663f
Child:
58:b824bdac7cd8
wip

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 54:a59803fcce58 6 // HACK
bhepp 54:a59803fcce58 7 //#include "InterruptMultiplexer.h"
bhepp 54:a59803fcce58 8 #include "InterruptHandler.h"
manumaet 44:2e0045042a59 9
bhepp 48:5999e510f154 10 #include "mavlink_bridge/mavlink_bridge.h"
bhepp 48:5999e510f154 11
bhepp 48:5999e510f154 12 #define ANCHOR false
bhepp 48:5999e510f154 13
bhepp 48:5999e510f154 14 using namespace ait;
manumaet 44:2e0045042a59 15
bhepp 50:50b8aea54a51 16 const int SPI_FREQUENCY = 1000000;
bhepp 52:94688f414dcd 17 const int NUM_OF_ANCHORS = 1;
bhepp 52:94688f414dcd 18 const int ANCHOR_ADDRESS_OFFSET = 10;
naegelit 53:79a72d752ec4 19 const bool USE_NLOS_SETTINGS = false;
bhepp 48:5999e510f154 20 const PinName DW_RESET_PIN = D15;
bhepp 48:5999e510f154 21 const PinName DW_IRQ_PIN = D14;
bhepp 48:5999e510f154 22 const PinName DW_MOSI_PIN = D11;
bhepp 48:5999e510f154 23 const PinName DW_MISO_PIN = D12;
bhepp 48:5999e510f154 24 const PinName DW_SCLK_PIN = D13;
bhepp 48:5999e510f154 25 #if ANCHOR
naegelit 53:79a72d752ec4 26 const bool MAVLINK_COMM = false;
naegelit 53:79a72d752ec4 27 const int NUM_OF_DW_UNITS = 1;
naegelit 53:79a72d752ec4 28 const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
bhepp 48:5999e510f154 29 #else
bhepp 52:94688f414dcd 30 // const bool MAVLINK_COMM = true;
naegelit 53:79a72d752ec4 31 const bool MAVLINK_COMM = false;
naegelit 55:0587f7ad4bdd 32 const int NUM_OF_DW_UNITS = 4;
naegelit 53:79a72d752ec4 33 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D9, D8,D10};
naegelit 53:79a72d752ec4 34 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10,D9,D8};
naegelit 55:0587f7ad4bdd 35 const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10,D9,D8,D7};
naegelit 55:0587f7ad4bdd 36 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10,D9};
naegelit 53:79a72d752ec4 37 //const PinName DW_CS_PINS[NUM_OF_DW_UNITS] = {D10};
bhepp 48:5999e510f154 38 #endif
bhepp 48:5999e510f154 39
bhepp 48:5999e510f154 40 PC pc(USBTX, USBRX, 115200); // USB UART Terminal
bhepp 48:5999e510f154 41
naegelit 53:79a72d752ec4 42 void rangeAndDisplayAll(MM2WayRanging& node, MAVLinkBridge& mb, Timer& timer)
naegelit 53:79a72d752ec4 43 {
bhepp 52:94688f414dcd 44 for (int i = 0; i < NUM_OF_ANCHORS; i++) {
bhepp 52:94688f414dcd 45 uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + i;
bhepp 52:94688f414dcd 46 node.requestRanging(remote_address);
bhepp 52:94688f414dcd 47
manumaet 0:f50e671ffff7 48
naegelit 53:79a72d752ec4 49 pc.printf("%d - %d> dist = %.4f\r\n", node.address, remote_address, node.distances[remote_address]);
bhepp 48:5999e510f154 50
manumaet 44:2e0045042a59 51 }
manumaet 44:2e0045042a59 52 }
naegelit 53:79a72d752ec4 53 //continue;
manumaet 45:01a33363bc21 54
manumaet 45:01a33363bc21 55
manumaet 46:6398237672a0 56 struct __attribute__((packed, aligned(1))) DistancesFrame {
bhepp 48:5999e510f154 57 uint8_t source;
bhepp 48:5999e510f154 58 uint8_t destination;
bhepp 48:5999e510f154 59 uint8_t type;
bhepp 48:5999e510f154 60 float dist[4];
bhepp 48:5999e510f154 61 };
manumaet 45:01a33363bc21 62
bhepp 48:5999e510f154 63
bhepp 48:5999e510f154 64 // -----------------------------------------------------------------------------------------------
manumaet 46:6398237672a0 65 void altCallbackRX();
manumaet 45:01a33363bc21 66
naegelit 53:79a72d752ec4 67 int main()
naegelit 53:79a72d752ec4 68 {
naegelit 53:79a72d752ec4 69 //first set the CS to high
naegelit 55:0587f7ad4bdd 70 DigitalOut test1(D7);
naegelit 55:0587f7ad4bdd 71 DigitalOut test2(D8);
naegelit 55:0587f7ad4bdd 72 DigitalOut test3(D9);
naegelit 55:0587f7ad4bdd 73 DigitalOut test4(D10);
naegelit 53:79a72d752ec4 74 test1=1;
naegelit 53:79a72d752ec4 75 test2=1;
naegelit 55:0587f7ad4bdd 76 test3=1;
naegelit 55:0587f7ad4bdd 77 test4=1;
naegelit 53:79a72d752ec4 78
naegelit 53:79a72d752ec4 79 wait(0.1);
bhepp 50:50b8aea54a51 80 UART_Mbed uart(USBTX, USBRX, 115200);
bhepp 50:50b8aea54a51 81 MAVLinkBridge mb(&uart);
bhepp 50:50b8aea54a51 82
bhepp 50:50b8aea54a51 83 if (!MAVLINK_COMM) {
bhepp 50:50b8aea54a51 84 pc.printf("==== AIT UWB Range ====\r\n");
bhepp 50:50b8aea54a51 85 }
bhepp 48:5999e510f154 86
bhepp 48:5999e510f154 87 SPI spi(DW_MOSI_PIN, DW_MISO_PIN, DW_SCLK_PIN);
bhepp 50:50b8aea54a51 88 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 89 // NOTE: Minimum Frequency 1MHz. Below it is now working. Could be something with the activation and deactivation of interrupts.
bhepp 50:50b8aea54a51 90 spi.frequency(SPI_FREQUENCY); // with a 1MHz clock rate (worked up to 49MHz in our Test)
bhepp 50:50b8aea54a51 91
bhepp 50:50b8aea54a51 92 // Setup interrupt pin
bhepp 54:a59803fcce58 93 // HACK
bhepp 54:a59803fcce58 94 //InterruptMultiplexer irq_mp(DW_IRQ_PIN);
bhepp 54:a59803fcce58 95 //irq_mp.getIRQ().rise(&irq_mp, &InterruptMultiplexer::trigger); // attach interrupt handler to rising edge of interrupt pin from DW1000
bhepp 50:50b8aea54a51 96
bhepp 50:50b8aea54a51 97 Timer timer;
bhepp 50:50b8aea54a51 98 timer.start();
bhepp 50:50b8aea54a51 99
naegelit 53:79a72d752ec4 100
bhepp 48:5999e510f154 101
naegelit 53:79a72d752ec4 102 DW1000* dw_array[NUM_OF_DW_UNITS + 0];
naegelit 53:79a72d752ec4 103 MM2WayRanging* node_array[NUM_OF_DW_UNITS + 0]; // Instance of the two way ranging algorithm
bhepp 48:5999e510f154 104
bhepp 50:50b8aea54a51 105 if (!MAVLINK_COMM) {
bhepp 50:50b8aea54a51 106 pc.printf("Performing hardware reset of UWB modules\r\n");
bhepp 50:50b8aea54a51 107 }
bhepp 48:5999e510f154 108 // == IMPORTANT == Create all DW objects first (this will cause a reset of the DW module)
bhepp 48:5999e510f154 109 DW1000::hardwareReset(DW_RESET_PIN);
naegelit 55:0587f7ad4bdd 110 for (int i = 0; i < 4; ++i) {
naegelit 53:79a72d752ec4 111
bhepp 54:a59803fcce58 112 // HACK
bhepp 54:a59803fcce58 113 InterruptHandler irq_mp(DW_IRQ_PIN);
bhepp 56:2eba104e663f 114 if (i == 0)
bhepp 56:2eba104e663f 115 {
bhepp 56:2eba104e663f 116 irq_mp.getIRQ().rise(&irq_mp, &InterruptHandler::trigger); // attach interrupt handler to rising edge of interrupt pin from DW1000
bhepp 56:2eba104e663f 117 }
bhepp 54:a59803fcce58 118
bhepp 48:5999e510f154 119 dw_array[i] = new DW1000(spi, irq_mp, DW_CS_PINS[i]); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
naegelit 53:79a72d752ec4 120
naegelit 53:79a72d752ec4 121 // dw_array[1] = new DW1000(spi, irq_mp, DW_CS_PINS[1]); // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ, RESET)
bhepp 48:5999e510f154 122
bhepp 48:5999e510f154 123 DW1000& dw = *dw_array[i];
naegelit 53:79a72d752ec4 124
naegelit 53:79a72d752ec4 125 wait(0.1) ;
naegelit 53:79a72d752ec4 126
naegelit 53:79a72d752ec4 127 dw.setEUI(0xFAEDCD01FAEDCD01 + i);
naegelit 53:79a72d752ec4 128 wait(0.1) ;
naegelit 53:79a72d752ec4 129 // basic methods called to check if we have a working SPI connection
bhepp 48:5999e510f154 130 node_array[i] = new MM2WayRanging(*dw_array[i]);
bhepp 48:5999e510f154 131
naegelit 53:79a72d752ec4 132 pc.printf("\r\nUnit %d\r\n", i);
naegelit 53:79a72d752ec4 133 pc.printf("\r\nDecaWave 1.0 up and running!\r\n"); // Splashscreen
naegelit 53:79a72d752ec4 134 pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
naegelit 53:79a72d752ec4 135 pc.printf("EUI register: %016llX\r\n", dw.getEUI());
naegelit 53:79a72d752ec4 136 pc.printf("Voltage: %.2fV\r\n", dw.getVoltage());
naegelit 53:79a72d752ec4 137
naegelit 53:79a72d752ec4 138
bhepp 48:5999e510f154 139 MM2WayRanging& node = *node_array[i];
bhepp 48:5999e510f154 140 node.isAnchor = ANCHOR; // declare as anchor or beacon
bhepp 48:5999e510f154 141 if (ANCHOR) {
bhepp 48:5999e510f154 142 node.address = ANCHOR_ADDRESS_OFFSET + i;
bhepp 48:5999e510f154 143 if (!MAVLINK_COMM)
bhepp 48:5999e510f154 144 pc.printf("This node is Anchor node %d\r\n", node.address);
bhepp 48:5999e510f154 145 } else {
bhepp 48:5999e510f154 146 node.address = i;
bhepp 48:5999e510f154 147 if (!MAVLINK_COMM)
bhepp 50:50b8aea54a51 148 pc.printf("This node is a Beacon\r\n");
bhepp 48:5999e510f154 149 }
naegelit 53:79a72d752ec4 150
bhepp 56:2eba104e663f 151 /*uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + 0;
naegelit 53:79a72d752ec4 152 node.requestRanging(remote_address);
naegelit 53:79a72d752ec4 153 rangeAndDisplayAll(node, mb, timer);
naegelit 53:79a72d752ec4 154 irq_mp.clear();
bhepp 56:2eba104e663f 155 wait(0.5);*/
manumaet 47:b6120c152ad1 156 }
bhepp 48:5999e510f154 157
naegelit 55:0587f7ad4bdd 158 // wait(1);
naegelit 57:b4e7324b0faf 159
bhepp 56:2eba104e663f 160 uint8_t remote_address = ANCHOR_ADDRESS_OFFSET + 0;
naegelit 57:b4e7324b0faf 161
naegelit 57:b4e7324b0faf 162 //for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
naegelit 57:b4e7324b0faf 163 MM2WayRanging& node = *node_array[0];
naegelit 57:b4e7324b0faf 164 rangeAndDisplayAll(node, mb, timer);
naegelit 57:b4e7324b0faf 165 wait(0.1);
naegelit 57:b4e7324b0faf 166 // }
naegelit 57:b4e7324b0faf 167
naegelit 57:b4e7324b0faf 168 //
naegelit 57:b4e7324b0faf 169 // MM2WayRanging& node = *node_array[0];
naegelit 57:b4e7324b0faf 170 // node.requestRanging(remote_address);
naegelit 57:b4e7324b0faf 171 // pc.printf("%d - %d> dist = %.4f\r\n", node.address, remote_address, node.distances[remote_address]);
bhepp 56:2eba104e663f 172 //rangeAndDisplayAll(node, mb, timer);
bhepp 56:2eba104e663f 173
bhepp 50:50b8aea54a51 174 if (!MAVLINK_COMM)
bhepp 50:50b8aea54a51 175 pc.printf("Entering main loop\r\n");
bhepp 51:e9391d04af00 176
naegelit 53:79a72d752ec4 177 //for (int i = 0; i < 2; ++i) {
naegelit 53:79a72d752ec4 178 //
naegelit 53:79a72d752ec4 179 // if (ANCHOR) {
naegelit 53:79a72d752ec4 180 // pc.printf("."); // to see if the core and output is working
naegelit 53:79a72d752ec4 181 // wait_ms(1);
naegelit 53:79a72d752ec4 182 // } else {
naegelit 53:79a72d752ec4 183 // for (int j = 0; j < NUM_OF_DW_UNITS; ++j) {
naegelit 53:79a72d752ec4 184 // MM2WayRanging& node = *node_array[j];
naegelit 53:79a72d752ec4 185 // rangeAndDisplayAll(node, mb, timer);
naegelit 53:79a72d752ec4 186 // wait(0.1);
naegelit 53:79a72d752ec4 187 // }
naegelit 53:79a72d752ec4 188 // }
naegelit 57:b4e7324b0faf 189 // }
bhepp 50:50b8aea54a51 190
naegelit 55:0587f7ad4bdd 191 while (true) {
bhepp 54:a59803fcce58 192 }
manumaet 46:6398237672a0 193 }