diff --git a/Wireless_Communication/UWB/Anchor/Anchor1/Anchor1.ino b/Wireless_Communication/UWB/Anchor/Anchor1/Anchor1.ino index 176cde444841157ba458457efb0697c6bd85406d..5407c0b0f5603215e626db01205c115e22f3ccdb 100644 --- a/Wireless_Communication/UWB/Anchor/Anchor1/Anchor1.ino +++ b/Wireless_Communication/UWB/Anchor/Anchor1/Anchor1.ino @@ -28,7 +28,7 @@ #define MY_ADDR ANCHOR1 #define RX_TIMEOUT 5000 -#define INTER_RANGING_DELAY 5 +#define INTER_RANGING_DELAY 0 #define NUMBER_OF_BEACONS_TO_RANGE 4 - MY_ADDR @@ -49,16 +49,12 @@ static dwt_config_t config = { DWT_PDOA_M0 /* PDOA mode off */ }; -size_t rxLen; static uint8_t rxBuffer[FRAME_LEN_MAX - MHR_LENGTH] = {}; /* Hold copy of status register state here for reference so that it can be examined at a debug breakpoint. */ uint32_t status_reg; extern dwt_txconfig_t txconfig_options; -macHeader txMHR, rxMHR; -bool frameReceived = 0; -bool ack; deviceState state = RESPONDER; void setup() @@ -107,6 +103,8 @@ void setup() while (1) ; } + + txconfig_options.power = 0xffffffff; dwt_configuretxrf(&txconfig_options); dwt_setrxantennadelay(RX_ANT_DLY); @@ -121,16 +119,13 @@ void setup() void loop() { - // Reset global parameters - clearMacHeader(&txMHR); - clearMacHeader(&rxMHR); - rxLen = 0; + macHeader txMHR, rxMHR; + bool frameReceived = 0; + bool ack; + size_t rxLen = 0; memset(rxBuffer, 0, sizeof(rxBuffer)); - frameReceived = 0; switch (state) { - // Beacons will only ever be in BEACON_POSITIONING_MODE mode during beacon positioning - // hence this portion of code only does ranging with other anchors case BEACON_POSITIONING_MODE: double dist; if (NUMBER_OF_BEACONS_TO_RANGE > 0) { @@ -142,6 +137,7 @@ void loop() addresses[i] = i + MY_ADDR + 1; // store destination addresss for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("Ranging %d retry %d\n", addresses[i], retries); distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i] // distances[i] = getRangingDistanceDelayedTX(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i] if (distances[i] != 0.0) {break;} @@ -153,18 +149,20 @@ void loop() // ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR, 5); // delay(INTER_RANGING_DELAY); for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("send distance retry %d\n", retries); sendDistances(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, true); ack = receiveAcknowledgement(&rxMHR); if (ack) {break;} delay(INTER_RANGING_DELAY); } - if (MY_ADDR < ANCHOR3) { // Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1); // ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3); // delay(INTER_RANGING_DELAY); + for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("Send bpm retry %d\n", retries); sendCommand(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, true); ack = receiveAcknowledgement(&rxMHR); if (ack) {break;} @@ -192,7 +190,7 @@ void loop() state = BEACON_POSITIONING_MODE; break; - case POLL_REQUEST: //TOF + case POLL_REQUEST: // TOF handlePollRequest(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR)); // handlePollRequestDelayedTX(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR), TX_ANT_DLY); break; diff --git a/Wireless_Communication/UWB/Anchor/Anchor2/Anchor2.ino b/Wireless_Communication/UWB/Anchor/Anchor2/Anchor2.ino index 4a60cd2ea7b78e841797fd692ae6a36dde1fdd30..ef0cd1017d82aa9a7bbdb22d508c1057755e6727 100644 --- a/Wireless_Communication/UWB/Anchor/Anchor2/Anchor2.ino +++ b/Wireless_Communication/UWB/Anchor/Anchor2/Anchor2.ino @@ -28,7 +28,7 @@ #define MY_ADDR ANCHOR2 #define RX_TIMEOUT 5000 -#define INTER_RANGING_DELAY 5 +#define INTER_RANGING_DELAY 0 #define NUMBER_OF_BEACONS_TO_RANGE 4 - MY_ADDR @@ -49,16 +49,12 @@ static dwt_config_t config = { DWT_PDOA_M0 /* PDOA mode off */ }; -size_t rxLen; static uint8_t rxBuffer[FRAME_LEN_MAX - MHR_LENGTH] = {}; /* Hold copy of status register state here for reference so that it can be examined at a debug breakpoint. */ uint32_t status_reg; extern dwt_txconfig_t txconfig_options; -macHeader txMHR, rxMHR; -bool frameReceived = 0; -bool ack; deviceState state = RESPONDER; void setup() @@ -107,6 +103,8 @@ void setup() while (1) ; } + + txconfig_options.power = 0xffffffff; dwt_configuretxrf(&txconfig_options); dwt_setrxantennadelay(RX_ANT_DLY); @@ -121,16 +119,13 @@ void setup() void loop() { - // Reset global parameters - clearMacHeader(&txMHR); - clearMacHeader(&rxMHR); - rxLen = 0; + macHeader txMHR, rxMHR; + bool frameReceived = 0; + bool ack; + size_t rxLen = 0; memset(rxBuffer, 0, sizeof(rxBuffer)); - frameReceived = 0; switch (state) { - // Beacons will only ever be in BEACON_POSITIONING_MODE mode during beacon positioning - // hence this portion of code only does ranging with other anchors case BEACON_POSITIONING_MODE: double dist; if (NUMBER_OF_BEACONS_TO_RANGE > 0) { @@ -142,6 +137,7 @@ void loop() addresses[i] = i + MY_ADDR + 1; // store destination addresss for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("Ranging %d retry %d\n", addresses[i], retries); distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i] // distances[i] = getRangingDistanceDelayedTX(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i] if (distances[i] != 0.0) {break;} @@ -153,18 +149,20 @@ void loop() // ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR, 5); // delay(INTER_RANGING_DELAY); for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("send distance retry %d\n", retries); sendDistances(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, true); ack = receiveAcknowledgement(&rxMHR); if (ack) {break;} delay(INTER_RANGING_DELAY); } - if (MY_ADDR < ANCHOR3) { // Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1); // ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3); // delay(INTER_RANGING_DELAY); + for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("Send bpm retry %d\n", retries); sendCommand(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, true); ack = receiveAcknowledgement(&rxMHR); if (ack) {break;} @@ -192,7 +190,7 @@ void loop() state = BEACON_POSITIONING_MODE; break; - case POLL_REQUEST: //TOF + case POLL_REQUEST: // TOF handlePollRequest(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR)); // handlePollRequestDelayedTX(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR), TX_ANT_DLY); break; diff --git a/Wireless_Communication/UWB/Anchor/Anchor3/Anchor3.ino b/Wireless_Communication/UWB/Anchor/Anchor3/Anchor3.ino index 30586f17a18845dce234c28fa19060af672ae76e..9985d98243034a05085746b7cd87fe13f74ec8a6 100644 --- a/Wireless_Communication/UWB/Anchor/Anchor3/Anchor3.ino +++ b/Wireless_Communication/UWB/Anchor/Anchor3/Anchor3.ino @@ -28,7 +28,7 @@ #define MY_ADDR ANCHOR3 #define RX_TIMEOUT 5000 -#define INTER_RANGING_DELAY 5 +#define INTER_RANGING_DELAY 0 #define NUMBER_OF_BEACONS_TO_RANGE 4 - MY_ADDR @@ -49,16 +49,12 @@ static dwt_config_t config = { DWT_PDOA_M0 /* PDOA mode off */ }; -size_t rxLen; static uint8_t rxBuffer[FRAME_LEN_MAX - MHR_LENGTH] = {}; /* Hold copy of status register state here for reference so that it can be examined at a debug breakpoint. */ uint32_t status_reg; extern dwt_txconfig_t txconfig_options; -macHeader txMHR, rxMHR; -bool frameReceived = 0; -bool ack; deviceState state = RESPONDER; void setup() @@ -107,6 +103,8 @@ void setup() while (1) ; } + + txconfig_options.power = 0xffffffff; dwt_configuretxrf(&txconfig_options); dwt_setrxantennadelay(RX_ANT_DLY); @@ -121,16 +119,13 @@ void setup() void loop() { - // Reset global parameters - clearMacHeader(&txMHR); - clearMacHeader(&rxMHR); - rxLen = 0; + macHeader txMHR, rxMHR; + bool frameReceived = 0; + bool ack; + size_t rxLen = 0; memset(rxBuffer, 0, sizeof(rxBuffer)); - frameReceived = 0; switch (state) { - // Beacons will only ever be in BEACON_POSITIONING_MODE mode during beacon positioning - // hence this portion of code only does ranging with other anchors case BEACON_POSITIONING_MODE: double dist; if (NUMBER_OF_BEACONS_TO_RANGE > 0) { @@ -142,6 +137,7 @@ void loop() addresses[i] = i + MY_ADDR + 1; // store destination addresss for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("Ranging %d retry %d\n", addresses[i], retries); distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i] // distances[i] = getRangingDistanceDelayedTX(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i] if (distances[i] != 0.0) {break;} @@ -153,18 +149,20 @@ void loop() // ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR, 5); // delay(INTER_RANGING_DELAY); for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("send distance retry %d\n", retries); sendDistances(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, true); ack = receiveAcknowledgement(&rxMHR); if (ack) {break;} delay(INTER_RANGING_DELAY); } - if (MY_ADDR < ANCHOR3) { // Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1); // ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3); // delay(INTER_RANGING_DELAY); + for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("Send bpm retry %d\n", retries); sendCommand(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, true); ack = receiveAcknowledgement(&rxMHR); if (ack) {break;} @@ -192,7 +190,7 @@ void loop() state = BEACON_POSITIONING_MODE; break; - case POLL_REQUEST: //TOF + case POLL_REQUEST: // TOF handlePollRequest(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR)); // handlePollRequestDelayedTX(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR), TX_ANT_DLY); break; diff --git a/Wireless_Communication/UWB/Anchor/Anchor4/Anchor4.ino b/Wireless_Communication/UWB/Anchor/Anchor4/Anchor4.ino index 50692f38b41d1e16322dcacbb42c55146f781aec..f4bb968e4c3f42372722d6f55a194718738361f6 100644 --- a/Wireless_Communication/UWB/Anchor/Anchor4/Anchor4.ino +++ b/Wireless_Communication/UWB/Anchor/Anchor4/Anchor4.ino @@ -28,7 +28,7 @@ #define MY_ADDR ANCHOR4 #define RX_TIMEOUT 5000 -#define INTER_RANGING_DELAY 5 +#define INTER_RANGING_DELAY 0 #define NUMBER_OF_BEACONS_TO_RANGE 4 - MY_ADDR @@ -49,16 +49,12 @@ static dwt_config_t config = { DWT_PDOA_M0 /* PDOA mode off */ }; -size_t rxLen; static uint8_t rxBuffer[FRAME_LEN_MAX - MHR_LENGTH] = {}; /* Hold copy of status register state here for reference so that it can be examined at a debug breakpoint. */ uint32_t status_reg; extern dwt_txconfig_t txconfig_options; -macHeader txMHR, rxMHR; -bool frameReceived = 0; -bool ack; deviceState state = RESPONDER; void setup() @@ -107,6 +103,8 @@ void setup() while (1) ; } + + txconfig_options.power = 0xffffffff; dwt_configuretxrf(&txconfig_options); dwt_setrxantennadelay(RX_ANT_DLY); @@ -121,16 +119,13 @@ void setup() void loop() { - // Reset global parameters - clearMacHeader(&txMHR); - clearMacHeader(&rxMHR); - rxLen = 0; + macHeader txMHR, rxMHR; + bool frameReceived = 0; + bool ack; + size_t rxLen = 0; memset(rxBuffer, 0, sizeof(rxBuffer)); - frameReceived = 0; switch (state) { - // Beacons will only ever be in BEACON_POSITIONING_MODE mode during beacon positioning - // hence this portion of code only does ranging with other anchors case BEACON_POSITIONING_MODE: double dist; if (NUMBER_OF_BEACONS_TO_RANGE > 0) { @@ -142,6 +137,7 @@ void loop() addresses[i] = i + MY_ADDR + 1; // store destination addresss for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("Ranging %d retry %d\n", addresses[i], retries); distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i] // distances[i] = getRangingDistanceDelayedTX(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i] if (distances[i] != 0.0) {break;} @@ -153,18 +149,20 @@ void loop() // ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR, 5); // delay(INTER_RANGING_DELAY); for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("send distance retry %d\n", retries); sendDistances(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, true); ack = receiveAcknowledgement(&rxMHR); if (ack) {break;} delay(INTER_RANGING_DELAY); } - if (MY_ADDR < ANCHOR3) { // Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1); // ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3); // delay(INTER_RANGING_DELAY); + for (uint8_t retries = 0; retries < 10; retries++) { + // Serial.printf("Send bpm retry %d\n", retries); sendCommand(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, true); ack = receiveAcknowledgement(&rxMHR); if (ack) {break;} @@ -192,7 +190,7 @@ void loop() state = BEACON_POSITIONING_MODE; break; - case POLL_REQUEST: //TOF + case POLL_REQUEST: // TOF handlePollRequest(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR)); // handlePollRequestDelayedTX(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR), TX_ANT_DLY); break; diff --git a/Wireless_Communication/UWB/Beacons_tag_position/realtime_location_with_serial_gui.py b/Wireless_Communication/UWB/Beacons_tag_position/realtime_location_with_serial_gui.py index c45a4ff9d546486e72ab115ead51d19d19e97165..e20c797be6e205ef8f17751bf047c34dfde4cf3b 100644 --- a/Wireless_Communication/UWB/Beacons_tag_position/realtime_location_with_serial_gui.py +++ b/Wireless_Communication/UWB/Beacons_tag_position/realtime_location_with_serial_gui.py @@ -196,7 +196,7 @@ class AnchorTagGUI: # Guess for B based on distance A-B and B-D x_B = measured_distances[0] / 2 - y_B = measured_distances[4] / 2 + 100 # Start y_B above y_C + y_B = measured_distances[4] / 2 + 200 # Start y_B above y_C # Guess for C based on distance C-D and A-C x_C = measured_distances[5] / 2 @@ -211,17 +211,17 @@ class AnchorTagGUI: # Define lower and upper bounds based on measured distances lower_bound = [ - -max_dist / 2, # x_B lower bound - min_dist / 2, # y_B lower bound (above y_C) - -max_dist / 2, # x_C lower bound - min_dist / 4, # y_C lower bound + -max_dist, # x_B lower bound + -max_dist, # y_B lower bound (above y_C) + -max_dist, # x_C lower bound + -max_dist, # y_C lower bound min_dist / 2 # y_A lower bound ] upper_bound = [ max_dist * 1.5, # x_B upper bound max_dist * 1.5, # y_B upper bound max_dist * 1.5, # x_C upper bound - max_dist * 1.25, # y_C upper bound + max_dist * 1.5, # y_C upper bound max_dist * 1.5 # y_A upper bound ] return lower_bound, upper_bound @@ -274,14 +274,14 @@ class AnchorTagGUI: ) optimized_coords_noisy = result_noisy.x h=250 - print(f"Optimized coordinates: {optimized_coords_noisy}") + # print(f"Optimized coordinates: {optimized_coords_noisy}") self.anchors = { "A1": (0, optimized_coords_noisy[4], h), "A2": (optimized_coords_noisy[0], optimized_coords_noisy[1], h), "A3": (optimized_coords_noisy[2], optimized_coords_noisy[3], h), "A4": (0, 0, h), } - print(self.anchors) + # print(self.anchors) self.draw_canvas() print(f"Optimized anchor coordinates: {self.anchors}") return {k: (round(v[0], 2), round(v[1], 2)) for k, v in self.anchors.items()} @@ -416,7 +416,7 @@ class AnchorTagGUI: canvas_width, canvas_height = 600, 600 center_x, center_y = canvas_width // 2, canvas_height // 2 - scale_factor = 0.1 + scale_factor = 0.2 # Determine avg anchor position for centering if hasattr(self, 'anchors') and self.anchors: @@ -429,8 +429,8 @@ class AnchorTagGUI: y_offset = center_y - avg_y def transform_coordinates(x, y): - x_scaled = int((x + x_offset) * scale_factor) +100 - y_scaled = int(canvas_height - (y + y_offset) * scale_factor) - 200 + x_scaled = int((x + x_offset) * scale_factor) + 200 + y_scaled = int(canvas_height - (y + y_offset) * scale_factor) - 250 return x_scaled, y_scaled # Draw anchorss diff --git a/Wireless_Communication/UWB/MAC_Responder/MAC_Responder.ino b/Wireless_Communication/UWB/MAC_Responder/MAC_Responder.ino index 08b97f184510fe2b2b2f71782ce2e294cd09ecbd..5af768afa1dfaa6c888477cb81dc285f83bbda52 100644 --- a/Wireless_Communication/UWB/MAC_Responder/MAC_Responder.ino +++ b/Wireless_Communication/UWB/MAC_Responder/MAC_Responder.ino @@ -162,8 +162,8 @@ void loop() // Send acknowledgement if AR bit is set if (checkAckRequest(&rxMHR)) { Serial.println("Sending acknowledgement..."); - initializeEnhAckMHR(&txMHR, PAN_ID, MY_ADDR, 0x1234); - // initializeEnhAckMHR(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR)); + // initializeEnhAckMHR(&txMHR, PAN_ID, MY_ADDR, 0x1234); + initializeEnhAckMHR(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR)); Serial.printf("ACK MHR: "); for (int i = 0; i < MHR_LENGTH; i++) { diff --git a/Wireless_Communication/UWB/Tag/Tag1/Tag1.ino b/Wireless_Communication/UWB/Tag/Tag1/Tag1.ino index f80226d8ff78584d08c5c88d9246542eacbd0c59..52c8e037eb49efa85112ad9be7951267fc55d24e 100644 --- a/Wireless_Communication/UWB/Tag/Tag1/Tag1.ino +++ b/Wireless_Communication/UWB/Tag/Tag1/Tag1.ino @@ -29,7 +29,7 @@ #define RX_TIMEOUT 5000 #define INTER_RANGING_DELAY 5 -#define TIMEOUT_MILLIS 300 +#define TIMEOUT_MILLIS 2000 unsigned long startTime; @@ -124,6 +124,7 @@ void setup() } // Serial.println("CONFIG OK"); /* Configure the TX spectrum parameters (power PG delay and PG Count) */ + // txconfig_options.power = 0xffffffff; dwt_configuretxrf(&txconfig_options); dwt_setrxantennadelay(RX_ANT_DLY); @@ -172,15 +173,15 @@ void loop() if (strcmp(serialBuffer, "bpm") == 0) { //Perform live checks on all 4 anchors - for (uint8_t beacon = ANCHOR1; beacon <= ANCHOR4; beacon++) { - ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, beacon, LIVE_CHECK, &rxMHR, 3); - // ack = 1; // DEBUGGING - anchorAcks[beacon - 1] = ack; - if (!ack) {allTrue = false;} - delay(INTER_RANGING_DELAY); - } - - if (allTrue) { // do BPM if all anchors are alive + // for (uint8_t beacon = ANCHOR1; beacon <= ANCHOR4; beacon++) { + // ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, beacon, LIVE_CHECK, &rxMHR, 3); + // // ack = 1; // DEBUGGING + // anchorAcks[beacon - 1] = ack; + // if (!ack) {allTrue = false;} + // delay(INTER_RANGING_DELAY); + // } + + // if (allTrue) { // do BPM if all anchors are alive BPMResponseCount = 0; memset(BPMDistances, 0, sizeof(BPMDistances)); ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, ANCHOR1, BPM_COMMAND, &rxMHR); // initiate BPM chain reaction starting with ANCHOR1 @@ -189,15 +190,24 @@ void loop() memset(serialBuffer, 0, sizeof(serialBuffer)); //clear the serial buffer once done serialDataReady = false; //reset serial data ready flag return; - } - else { // tell the pi which anchors are offline - Serial.printf("%d %d %d %d", anchorAcks[0], anchorAcks[1], anchorAcks[2], anchorAcks[3]); - Serial.println(); - } + // } + // else { // tell the pi which anchors are offline + // Serial.printf("%d %d %d %d", anchorAcks[0], anchorAcks[1], anchorAcks[2], anchorAcks[3]); + // Serial.println(); + // } } else if (strcmp(serialBuffer, "rng") == 0) { state = RANGING_INITIATOR; } + // else if (strcmp(serialBuffer, "lc") == 0) { + // Serial.print("Live check: "); + // for (uint8_t beacon = ANCHOR1; beacon <= ANCHOR4; beacon++) { + // ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, beacon, LIVE_CHECK, &rxMHR, 3); + // printf("%d: %d ", beacon, ack); + // // delay(INTER_RANGING_DELAY); + // } + // Serial.println(); + // } // else { // Serial.println("Serial command not recognized. "); // } @@ -287,11 +297,11 @@ void loop() readDistancesBPM(senderAnchor, rxBuffer, BPMDistances); startTime = millis(); //reset start time - // Serial.printf("Received distances from %d\n", senderAnchor); - // for (int i = 0; i < 6; i++) { - // Serial.printf("%lf ", BPMDistances[i] * 100.0); - // } - // Serial.println(); + // Serial.printf("Received distances from %d\n", senderAnchor); + // for (int i = 0; i < 6; i++) { + // Serial.printf("%lf ", BPMDistances[i] * 100.0); + // } + // Serial.println(); } else { Serial.println(3); // did not receive a send distances frame