ils
Fork of GA-Test_copy by
main_minimal.cpp@1:346279def7ac, 2017-12-09 (annotated)
- Committer:
- bberabi
- Date:
- Sat Dec 09 16:02:58 2017 +0000
- Revision:
- 1:346279def7ac
- Parent:
- 0:a3b83d366423
- Child:
- 2:5adf0b785944
errvv
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aungriah | 0:a3b83d366423 | 1 | #include "mbed.h" |
aungriah | 0:a3b83d366423 | 2 | |
aungriah | 0:a3b83d366423 | 3 | |
aungriah | 0:a3b83d366423 | 4 | #include "SMConfig.h" |
aungriah | 0:a3b83d366423 | 5 | #include "DecaWave.h" // DW1000 functions |
aungriah | 0:a3b83d366423 | 6 | //#include "LMMN2WR.h" // 2-Way-Ranging |
aungriah | 0:a3b83d366423 | 7 | #include "PC.h" // Serial Port via USB for debugging with Terminal |
aungriah | 0:a3b83d366423 | 8 | #include "Watchdog.h" // Resets Program if it gets stuck |
aungriah | 0:a3b83d366423 | 9 | #include "nodes.h" |
aungriah | 0:a3b83d366423 | 10 | #include <stddef.h> |
aungriah | 0:a3b83d366423 | 11 | |
aungriah | 0:a3b83d366423 | 12 | #define NELEMS(x) (sizeof(x) / sizeof((x)[0])) |
aungriah | 0:a3b83d366423 | 13 | |
aungriah | 0:a3b83d366423 | 14 | // Function Prototypes |
aungriah | 0:a3b83d366423 | 15 | void dwCallbackTx(const dwt_cb_data_t *rxd); |
aungriah | 0:a3b83d366423 | 16 | void dwCallbackRx(const dwt_cb_data_t *rxd); |
aungriah | 0:a3b83d366423 | 17 | int min (int a, int b); |
aungriah | 0:a3b83d366423 | 18 | void configureDW1000(uint8_t dwMode); |
aungriah | 0:a3b83d366423 | 19 | void rangeAndDisplayOne(uint8_t addr); |
aungriah | 0:a3b83d366423 | 20 | void rangeAndDisplayAll(); |
aungriah | 0:a3b83d366423 | 21 | void executeOrder(char* command); |
aungriah | 0:a3b83d366423 | 22 | |
aungriah | 0:a3b83d366423 | 23 | |
aungriah | 0:a3b83d366423 | 24 | // PA_7 MOSI, PA_6 MISO, PA_5 SCLK, PB_6 CS, PB_9 IRQ |
aungriah | 0:a3b83d366423 | 25 | SPI decaWaveSpi(PA_7, PA_6, PA_5); // Instance of SPI connection to DW1000 |
aungriah | 0:a3b83d366423 | 26 | DigitalOut decaWaveCs(PB_6); |
aungriah | 0:a3b83d366423 | 27 | InterruptIn decaWaveIrq(PB_9); |
aungriah | 0:a3b83d366423 | 28 | DecaWave decaWave = DecaWave(); // Instance of the DW1000 |
aungriah | 0:a3b83d366423 | 29 | |
aungriah | 0:a3b83d366423 | 30 | |
aungriah | 0:a3b83d366423 | 31 | /* BIT and PINS |
aungriah | 0:a3b83d366423 | 32 | BIT 1 = PA_10 |
aungriah | 0:a3b83d366423 | 33 | BIT 2 = PB_3 |
aungriah | 0:a3b83d366423 | 34 | BIT 3 = PB_5 |
aungriah | 0:a3b83d366423 | 35 | BIT 4 = PB_4 |
aungriah | 0:a3b83d366423 | 36 | BIT 5 = PB_10 |
aungriah | 0:a3b83d366423 | 37 | BIT 6 = PA_8 |
aungriah | 0:a3b83d366423 | 38 | BIT 7 = PA_9 |
aungriah | 0:a3b83d366423 | 39 | BIT 8 = PC_7 |
aungriah | 0:a3b83d366423 | 40 | */ |
aungriah | 0:a3b83d366423 | 41 | |
aungriah | 0:a3b83d366423 | 42 | |
aungriah | 0:a3b83d366423 | 43 | BusIn adressInput(PA_9,PA_8,PB_10,PB_4,PB_5,PB_3,PA_10); // first seven 7 bit for ID settings, most left bit = most significant bit |
aungriah | 0:a3b83d366423 | 44 | PC pc(USBTX, USBRX, 921600); // USB UART Terminal |
aungriah | 0:a3b83d366423 | 45 | DigitalIn anchorInput(PC_7); // usage of last bit as deciding bit for: anchor or beacon |
aungriah | 0:a3b83d366423 | 46 | Watchdog wdt = Watchdog(); |
aungriah | 0:a3b83d366423 | 47 | BeaconNode beaconNode(decaWave); |
aungriah | 0:a3b83d366423 | 48 | AnchorNode anchorNode(decaWave); |
aungriah | 0:a3b83d366423 | 49 | BaseStationNode baseStationNode(decaWave); |
aungriah | 0:a3b83d366423 | 50 | Node *node; |
aungriah | 0:a3b83d366423 | 51 | |
aungriah | 0:a3b83d366423 | 52 | |
aungriah | 0:a3b83d366423 | 53 | int main() { |
aungriah | 0:a3b83d366423 | 54 | |
aungriah | 0:a3b83d366423 | 55 | decaWaveSpi.frequency(20000000); // Crank up the SPI frequency from 1Mhz to 3Mhz (maybe more?) |
aungriah | 0:a3b83d366423 | 56 | |
aungriah | 0:a3b83d366423 | 57 | // Check if Reset was from Watchdog or externally |
aungriah | 0:a3b83d366423 | 58 | if(RCC->CSR&0x20000000) |
aungriah | 0:a3b83d366423 | 59 | pc.printf("\r\n\r\n --- !!!!WATCHDOG RESET!!!! --- \r\n\r\n", RCC->CSR); |
aungriah | 0:a3b83d366423 | 60 | else if(RCC->CSR&0x4000000){ |
aungriah | 0:a3b83d366423 | 61 | pc.printf("\r\n\r\n --- External Reset --- \r\n\r\n", RCC->CSR); |
aungriah | 0:a3b83d366423 | 62 | //wdt.kick(); |
aungriah | 0:a3b83d366423 | 63 | } |
aungriah | 0:a3b83d366423 | 64 | __HAL_RCC_CLEAR_RESET_FLAGS(); |
aungriah | 0:a3b83d366423 | 65 | |
aungriah | 0:a3b83d366423 | 66 | // Set all Switches Pull-Down so that it is zero if Switch is not set |
aungriah | 0:a3b83d366423 | 67 | |
aungriah | 0:a3b83d366423 | 68 | adressInput.mode(PullDown); |
aungriah | 0:a3b83d366423 | 69 | anchorInput.mode(PullDown); |
aungriah | 0:a3b83d366423 | 70 | //modeInput.mode(PullDown); |
aungriah | 0:a3b83d366423 | 71 | wait_ms(50); |
aungriah | 0:a3b83d366423 | 72 | |
aungriah | 0:a3b83d366423 | 73 | baseStationNode.setAddress(adressInput & adressInput.mask()); |
aungriah | 0:a3b83d366423 | 74 | anchorNode.setAddress(adressInput & adressInput.mask()); |
aungriah | 0:a3b83d366423 | 75 | beaconNode.setAddress(adressInput & adressInput.mask()); |
aungriah | 0:a3b83d366423 | 76 | |
aungriah | 0:a3b83d366423 | 77 | if((adressInput & adressInput.mask()) == BASE_STATION_ADDR){ |
aungriah | 0:a3b83d366423 | 78 | node = &baseStationNode; |
aungriah | 0:a3b83d366423 | 79 | pc.printf("This node is the Base Station, Adress: %d \r\n \r\n \r\n", node->getAddress()); |
aungriah | 0:a3b83d366423 | 80 | // wdt.kick(2); // Set up WatchDog |
aungriah | 0:a3b83d366423 | 81 | |
aungriah | 0:a3b83d366423 | 82 | } |
aungriah | 0:a3b83d366423 | 83 | else if(anchorInput){ |
aungriah | 0:a3b83d366423 | 84 | node = &anchorNode; |
aungriah | 0:a3b83d366423 | 85 | pc.printf("This node is an Anchor node, Adress: %d \r\n \r\n \r\n", node->getAddress()); |
aungriah | 0:a3b83d366423 | 86 | wdt.kick(2); // Set up WatchDog |
aungriah | 0:a3b83d366423 | 87 | } |
aungriah | 0:a3b83d366423 | 88 | else{ |
aungriah | 0:a3b83d366423 | 89 | node = &beaconNode; |
aungriah | 0:a3b83d366423 | 90 | pc.printf("This node is a Beacon, Adress: %d \r\n \r\n \r\n", node->getAddress()); |
aungriah | 0:a3b83d366423 | 91 | wdt.kick(2); // Set up WatchDog |
aungriah | 0:a3b83d366423 | 92 | } |
aungriah | 0:a3b83d366423 | 93 | |
aungriah | 0:a3b83d366423 | 94 | //pc.printf(" Adress: %d \r\n \r\n \r\n", node->getAddress()); |
aungriah | 0:a3b83d366423 | 95 | |
aungriah | 0:a3b83d366423 | 96 | configureDW1000(7); |
aungriah | 0:a3b83d366423 | 97 | |
aungriah | 0:a3b83d366423 | 98 | // TODO turn on RXMode regularly (every couple seconds) |
aungriah | 0:a3b83d366423 | 99 | |
aungriah | 0:a3b83d366423 | 100 | char command_str[30]; |
aungriah | 0:a3b83d366423 | 101 | |
aungriah | 0:a3b83d366423 | 102 | while(1) { |
aungriah | 0:a3b83d366423 | 103 | |
aungriah | 0:a3b83d366423 | 104 | // Choose between what to execte based on whether this device is a: |
aungriah | 0:a3b83d366423 | 105 | // BEACON |
aungriah | 0:a3b83d366423 | 106 | // BASESTATION |
aungriah | 0:a3b83d366423 | 107 | // ANCHOR |
aungriah | 0:a3b83d366423 | 108 | if (!node->isAnchor() && !node->isBaseStation()){ |
aungriah | 0:a3b83d366423 | 109 | |
aungriah | 0:a3b83d366423 | 110 | // THE BEACON EXECUTES THIS |
aungriah | 0:a3b83d366423 | 111 | if(beaconNode.getRepetitions() > 0){ |
aungriah | 0:a3b83d366423 | 112 | switch(beaconNode.getMode()){ |
aungriah | 0:a3b83d366423 | 113 | case RANGE_ALL: rangeAndDisplayAll(); |
aungriah | 0:a3b83d366423 | 114 | break; |
aungriah | 0:a3b83d366423 | 115 | case RANGE_ONE: rangeAndDisplayOne(beaconNode.getDestination()); |
aungriah | 0:a3b83d366423 | 116 | break; |
aungriah | 0:a3b83d366423 | 117 | default: break; |
aungriah | 0:a3b83d366423 | 118 | } |
aungriah | 0:a3b83d366423 | 119 | beaconNode.decreaseRepetitions(); |
aungriah | 0:a3b83d366423 | 120 | } |
aungriah | 0:a3b83d366423 | 121 | else{ |
aungriah | 0:a3b83d366423 | 122 | beaconNode.clearRec(); |
aungriah | 0:a3b83d366423 | 123 | wait_ms(8); |
aungriah | 0:a3b83d366423 | 124 | } |
aungriah | 0:a3b83d366423 | 125 | wdt.kick(); |
aungriah | 0:a3b83d366423 | 126 | } |
aungriah | 0:a3b83d366423 | 127 | else if (node->isBaseStation()){ |
aungriah | 0:a3b83d366423 | 128 | |
aungriah | 0:a3b83d366423 | 129 | // EXECUTE THIS IF A BASE STATION |
aungriah | 0:a3b83d366423 | 130 | pc.readcommand(executeOrder); |
bberabi | 1:346279def7ac | 131 | wait_ms(80); |
bberabi | 1:346279def7ac | 132 | |
aungriah | 0:a3b83d366423 | 133 | wdt.kick(); |
aungriah | 0:a3b83d366423 | 134 | } |
aungriah | 0:a3b83d366423 | 135 | else { // All Anchor Action is in the Interrupt functions! |
aungriah | 0:a3b83d366423 | 136 | // EXECUTE THIS IF AN ANCHOR |
aungriah | 0:a3b83d366423 | 137 | wait_ms(10); |
aungriah | 0:a3b83d366423 | 138 | wdt.kick(); |
aungriah | 0:a3b83d366423 | 139 | } |
aungriah | 0:a3b83d366423 | 140 | } |
aungriah | 0:a3b83d366423 | 141 | |
aungriah | 0:a3b83d366423 | 142 | |
aungriah | 0:a3b83d366423 | 143 | } |
aungriah | 0:a3b83d366423 | 144 | |
aungriah | 0:a3b83d366423 | 145 | |
aungriah | 0:a3b83d366423 | 146 | void executeOrder(char* command){ |
aungriah | 0:a3b83d366423 | 147 | |
aungriah | 0:a3b83d366423 | 148 | int repetitions = command[3]*100 + command[4]*10 + command[5] - 5328; |
aungriah | 0:a3b83d366423 | 149 | //uint8_t dest1 = command[7] - 48; |
aungriah | 0:a3b83d366423 | 150 | uint8_t dest2 = command[8] - 48; |
aungriah | 0:a3b83d366423 | 151 | uint8_t dest3 = command[9] - 48; |
aungriah | 0:a3b83d366423 | 152 | uint8_t dest1=0; |
aungriah | 0:a3b83d366423 | 153 | |
aungriah | 0:a3b83d366423 | 154 | |
aungriah | 0:a3b83d366423 | 155 | |
aungriah | 0:a3b83d366423 | 156 | |
aungriah | 0:a3b83d366423 | 157 | if(command[7] == 0 && command[8] == 0) |
aungriah | 0:a3b83d366423 | 158 | { |
aungriah | 0:a3b83d366423 | 159 | dest1 = command[9] - 48; |
aungriah | 0:a3b83d366423 | 160 | } |
aungriah | 0:a3b83d366423 | 161 | else if (command[7] == 0 && command[8] != 0) |
aungriah | 0:a3b83d366423 | 162 | { |
aungriah | 0:a3b83d366423 | 163 | dest1 = command[8] * 10 + command[9] - 528; |
aungriah | 0:a3b83d366423 | 164 | } |
aungriah | 0:a3b83d366423 | 165 | else if (command[7] != 0 && command[8] != 0) |
aungriah | 0:a3b83d366423 | 166 | { |
aungriah | 0:a3b83d366423 | 167 | dest1 = command[7] * 100 + command[8] * 10 + command[9] - 5328; |
aungriah | 0:a3b83d366423 | 168 | } |
aungriah | 0:a3b83d366423 | 169 | |
aungriah | 0:a3b83d366423 | 170 | if (strncmp(command, "reset", 5) == 0){ // This command is implemented in order to be able to reset BaseStation from Matlab. |
aungriah | 0:a3b83d366423 | 171 | wdt.kick(2); // Set up WatchDog |
aungriah | 0:a3b83d366423 | 172 | pc.printf("Base Station is RESETTED \r\n\r\n"); |
aungriah | 0:a3b83d366423 | 173 | } |
aungriah | 0:a3b83d366423 | 174 | |
aungriah | 0:a3b83d366423 | 175 | |
aungriah | 0:a3b83d366423 | 176 | else if (strncmp(command, "all", 3) == 0){ |
aungriah | 0:a3b83d366423 | 177 | baseStationNode.sendOrder(0, NOT_USED, RANGE_ALL, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 178 | // pc.printf("Mode: Range all \r\n"); |
aungriah | 0:a3b83d366423 | 179 | } |
aungriah | 0:a3b83d366423 | 180 | else if (strncmp(command, "one", 3) == 0){ |
aungriah | 0:a3b83d366423 | 181 | |
aungriah | 0:a3b83d366423 | 182 | |
aungriah | 0:a3b83d366423 | 183 | |
aungriah | 0:a3b83d366423 | 184 | if(dest1 != 15) |
aungriah | 0:a3b83d366423 | 185 | { |
aungriah | 0:a3b83d366423 | 186 | |
aungriah | 0:a3b83d366423 | 187 | baseStationNode.sendOrder(0, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 188 | } |
aungriah | 0:a3b83d366423 | 189 | else |
aungriah | 0:a3b83d366423 | 190 | { |
aungriah | 0:a3b83d366423 | 191 | baseStationNode.sendOrder(0, 1, RANGE_ONE, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 192 | } |
aungriah | 0:a3b83d366423 | 193 | } |
aungriah | 0:a3b83d366423 | 194 | |
aungriah | 0:a3b83d366423 | 195 | else if (strncmp(command, "bea", 3) == 0){ |
aungriah | 0:a3b83d366423 | 196 | if(dest1 < 15) |
aungriah | 0:a3b83d366423 | 197 | baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 198 | else |
aungriah | 0:a3b83d366423 | 199 | baseStationNode.sendOrder(0, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 200 | |
aungriah | 0:a3b83d366423 | 201 | } |
aungriah | 0:a3b83d366423 | 202 | |
aungriah | 0:a3b83d366423 | 203 | else if (strncmp(command, "anc", 3) == 0){ |
aungriah | 0:a3b83d366423 | 204 | if(dest1 < 15) |
aungriah | 0:a3b83d366423 | 205 | baseStationNode.sendOrder(dest1, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 206 | else |
aungriah | 0:a3b83d366423 | 207 | baseStationNode.sendOrder(0, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 208 | } |
aungriah | 0:a3b83d366423 | 209 | |
aungriah | 0:a3b83d366423 | 210 | else if (strncmp(command, "tri", 3) == 0){ |
aungriah | 0:a3b83d366423 | 211 | // Switch them in correct modes (should already be the case) |
aungriah | 0:a3b83d366423 | 212 | baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 213 | baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 214 | baseStationNode.sendOrder(dest3, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 215 | |
aungriah | 0:a3b83d366423 | 216 | // Ranging from first node |
aungriah | 0:a3b83d366423 | 217 | baseStationNode.sendOrder(dest1, dest2, RANGE_ONE, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 218 | wait_ms(10*repetitions); |
aungriah | 0:a3b83d366423 | 219 | baseStationNode.sendOrder(dest1, dest3, RANGE_ONE, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 220 | wait_ms(10*repetitions); |
aungriah | 0:a3b83d366423 | 221 | |
aungriah | 0:a3b83d366423 | 222 | // Mode Switches |
aungriah | 0:a3b83d366423 | 223 | baseStationNode.sendOrder(dest2, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 224 | baseStationNode.sendOrder(dest1, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 225 | |
aungriah | 0:a3b83d366423 | 226 | // Rangings |
aungriah | 0:a3b83d366423 | 227 | baseStationNode.sendOrder(dest2, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 228 | wait_ms(10*repetitions); |
aungriah | 0:a3b83d366423 | 229 | baseStationNode.sendOrder(dest2, dest3, RANGE_ONE, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 230 | wait_ms(10*repetitions); |
aungriah | 0:a3b83d366423 | 231 | |
aungriah | 0:a3b83d366423 | 232 | // Mode Switches |
aungriah | 0:a3b83d366423 | 233 | baseStationNode.sendOrder(dest3, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 234 | baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 235 | |
aungriah | 0:a3b83d366423 | 236 | // Rangings |
aungriah | 0:a3b83d366423 | 237 | baseStationNode.sendOrder(dest3, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 238 | wait_ms(10*repetitions); |
aungriah | 0:a3b83d366423 | 239 | baseStationNode.sendOrder(dest3, dest2, RANGE_ONE, repetitions, Node::BASE_ORDER); |
aungriah | 0:a3b83d366423 | 240 | wait_ms(10*repetitions); |
aungriah | 0:a3b83d366423 | 241 | |
aungriah | 0:a3b83d366423 | 242 | // Back to original modes |
aungriah | 0:a3b83d366423 | 243 | baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 244 | baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 245 | baseStationNode.sendOrder(dest3, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); |
aungriah | 0:a3b83d366423 | 246 | |
aungriah | 0:a3b83d366423 | 247 | } |
bberabi | 1:346279def7ac | 248 | |
aungriah | 0:a3b83d366423 | 249 | } |
aungriah | 0:a3b83d366423 | 250 | |
aungriah | 0:a3b83d366423 | 251 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
aungriah | 0:a3b83d366423 | 252 | // Called after Frame was received |
aungriah | 0:a3b83d366423 | 253 | void dwCallbackRx(const dwt_cb_data_t *rxd) { |
aungriah | 0:a3b83d366423 | 254 | int64_t rxTimeStamp = 0; |
aungriah | 0:a3b83d366423 | 255 | dwt_readrxdata(node->getRecFrameRef(), min(node->getRecFrameLength(), rxd->datalength), 0); // Read Data Frame from Registers |
aungriah | 0:a3b83d366423 | 256 | |
aungriah | 0:a3b83d366423 | 257 | dwt_readrxtimestamp((uint8_t*) &rxTimeStamp); // Read Timestamp when the frame was received exactly |
aungriah | 0:a3b83d366423 | 258 | rxTimeStamp &= MASK_40BIT; //Mask the 40 Bits of the timestamp |
aungriah | 0:a3b83d366423 | 259 | |
aungriah | 0:a3b83d366423 | 260 | node->callbackRX(rxTimeStamp); // Two Way Ranging Function |
aungriah | 0:a3b83d366423 | 261 | } |
aungriah | 0:a3b83d366423 | 262 | |
aungriah | 0:a3b83d366423 | 263 | |
aungriah | 0:a3b83d366423 | 264 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
aungriah | 0:a3b83d366423 | 265 | // Called after Frame was transmitted |
aungriah | 0:a3b83d366423 | 266 | void dwCallbackTx(const dwt_cb_data_t *txd) { |
aungriah | 0:a3b83d366423 | 267 | int64_t txTimeStamp = 0; |
aungriah | 0:a3b83d366423 | 268 | dwt_readtxtimestamp((uint8_t*) &txTimeStamp); // Read Timestamp when the frame was transmitted exactly |
aungriah | 0:a3b83d366423 | 269 | txTimeStamp &= MASK_40BIT; // Delete the most significant 8 Bits because the timestamp has only 40 Bits |
aungriah | 0:a3b83d366423 | 270 | |
aungriah | 0:a3b83d366423 | 271 | node->callbackTX(txTimeStamp); // Two Way Ranging Function |
aungriah | 0:a3b83d366423 | 272 | } |
aungriah | 0:a3b83d366423 | 273 | |
aungriah | 0:a3b83d366423 | 274 | |
aungriah | 0:a3b83d366423 | 275 | int min (int a, int b){ |
aungriah | 0:a3b83d366423 | 276 | if(a<=b) return a; |
aungriah | 0:a3b83d366423 | 277 | return b; |
aungriah | 0:a3b83d366423 | 278 | } |
aungriah | 0:a3b83d366423 | 279 | |
aungriah | 0:a3b83d366423 | 280 | void configureDW1000(uint8_t dwMode){ |
aungriah | 0:a3b83d366423 | 281 | dwt_config_t dwConfig; |
aungriah | 0:a3b83d366423 | 282 | dwt_txconfig_t dwConfigTx; |
aungriah | 0:a3b83d366423 | 283 | |
aungriah | 0:a3b83d366423 | 284 | SMsetconfig(dwMode, &dwConfig, &dwConfigTx); |
aungriah | 0:a3b83d366423 | 285 | |
aungriah | 0:a3b83d366423 | 286 | decaWave.setup(dwConfig, dwConfigTx, rfDelays[dwConfig.prf - DWT_PRF_16M], dwCallbackTx, dwCallbackRx); |
aungriah | 0:a3b83d366423 | 287 | |
aungriah | 0:a3b83d366423 | 288 | pc.printf("%s\r\n", DW1000_DEVICE_DRIVER_VER_STRING); |
aungriah | 0:a3b83d366423 | 289 | { |
aungriah | 0:a3b83d366423 | 290 | uint16_t tempvbat = dwt_readtempvbat(1); |
aungriah | 0:a3b83d366423 | 291 | if (tempvbat>0) { |
aungriah | 0:a3b83d366423 | 292 | float tempC = 1.13f * (float) (tempvbat >> 8) - 113.0f; |
aungriah | 0:a3b83d366423 | 293 | float vbatV = 0.0057f * (float) (tempvbat & 0xFF) + 2.3f; |
aungriah | 0:a3b83d366423 | 294 | |
aungriah | 0:a3b83d366423 | 295 | pc.printf(" Voltage: %f, Temperature: %f\r\n", vbatV, tempC); |
aungriah | 0:a3b83d366423 | 296 | } else { |
aungriah | 0:a3b83d366423 | 297 | pc.printf("ERROR: Cannot read voltage/temperature\r\n"); |
aungriah | 0:a3b83d366423 | 298 | } |
aungriah | 0:a3b83d366423 | 299 | } |
aungriah | 0:a3b83d366423 | 300 | pc.printf(" Device Lot ID %lu, Part ID %lu\r\n", dwt_getlotid(), dwt_getpartid()); |
aungriah | 0:a3b83d366423 | 301 | |
aungriah | 0:a3b83d366423 | 302 | uint16_t DWID = (dwt_getpartid() & 0xFFFF); |
aungriah | 0:a3b83d366423 | 303 | |
aungriah | 0:a3b83d366423 | 304 | pc.printf( |
aungriah | 0:a3b83d366423 | 305 | " Settings %i: \r\n Channel %u (%1.3f GHz w/ %1.3f GHz BW), Datarate %s, \r\n PRF %s, Preamble Code %u, PreambleLength %u symbols, \r\n PAC Size %u, %s SFD, SDF timeout %ul symbols\r\n", |
aungriah | 0:a3b83d366423 | 306 | dwMode, dwConfig.chan, ChannelFrequency[dwConfig.chan], |
aungriah | 0:a3b83d366423 | 307 | ChannelBandwidth[dwConfig.chan], ChannelBitrate[dwConfig.dataRate], ChannelPRF[dwConfig.prf], |
aungriah | 0:a3b83d366423 | 308 | dwConfig.txCode, ChannelPLEN(dwConfig.txPreambLength), ChannelPAC[dwConfig.rxPAC], |
aungriah | 0:a3b83d366423 | 309 | dwConfig.nsSFD==0?"standard":"non-standard", dwConfig.sfdTO); |
aungriah | 0:a3b83d366423 | 310 | pc.printf(" Power: NORM %2.1f dB, BOOST: 0.125ms %2.1f dB, 0.25ms %2.1f dB, 0.5ms %2.1f dB\r\n", |
aungriah | 0:a3b83d366423 | 311 | SMgain(dwConfigTx.power& 0xFF), SMgain((dwConfigTx.power>>24)& 0xFF), SMgain((dwConfigTx.power>>16)& 0xFF), |
aungriah | 0:a3b83d366423 | 312 | SMgain((dwConfigTx.power>>8)& 0xFF)); |
aungriah | 0:a3b83d366423 | 313 | pc.printf(" Frame length: %d us\r\n", decaWave.computeFrameLength_us()); |
aungriah | 0:a3b83d366423 | 314 | pc.printf(" Antenna Delay set to: %d \r\n", decaWave.getAntennaDelay()); |
aungriah | 0:a3b83d366423 | 315 | |
aungriah | 0:a3b83d366423 | 316 | decaWave.turnonrx(); // start listening |
aungriah | 0:a3b83d366423 | 317 | } |
aungriah | 0:a3b83d366423 | 318 | |
aungriah | 0:a3b83d366423 | 319 | void rangeAndDisplayOne(uint8_t addr){ |
aungriah | 0:a3b83d366423 | 320 | beaconNode.requestRanging(addr); |
bberabi | 1:346279def7ac | 321 | pc.printf("%f(%f) \r\n", beaconNode.getDistance(addr), beaconNode.getSignalStrength(addr)); |
aungriah | 0:a3b83d366423 | 322 | } |
aungriah | 0:a3b83d366423 | 323 | |
aungriah | 0:a3b83d366423 | 324 | |
aungriah | 0:a3b83d366423 | 325 | |
aungriah | 0:a3b83d366423 | 326 | void rangeAndDisplayAll(){ |
aungriah | 0:a3b83d366423 | 327 | beaconNode.requestRangingAll(); |
aungriah | 0:a3b83d366423 | 328 | for(int i = 0; i < ADRESSES_COUNT; i++){ |
aungriah | 0:a3b83d366423 | 329 | /*if(beaconNode.getDistance(i) > -10){ |
aungriah | 0:a3b83d366423 | 330 | pc.printf("#%d %f(%f), ", i, beaconNode.getDistance(i), beaconNode.getSignalStrength(i)); |
aungriah | 0:a3b83d366423 | 331 | }*/ |
aungriah | 0:a3b83d366423 | 332 | } |
aungriah | 0:a3b83d366423 | 333 | // pc.printf("o error3 ? \r\n"); |
aungriah | 0:a3b83d366423 | 334 | } |
aungriah | 0:a3b83d366423 | 335 |