Skip to content
Snippets Groups Projects
Commit 27ae5624 authored by Paul-Winpenny's avatar Paul-Winpenny
Browse files

Testing how the two anchor scripts range each other.

parent 04ec9daa
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Wireless_Communication/UWB/Anchor/anchors/Anchor1/Anchor1.ino
+ 111
− 78
View file @ 27ae5624
/* /*
* ---------------------------------------------------------------------------- ----------------------------------------------------------------------------
* _ _ _ _ _ _
* (_) | | | (_) | | |
* _| | __ _| |__ ___ _| | __ _| |__ ___
* | | | / _` | '_ \/ __| | | | / _` | '_ \/ __|
* | | |___| (_| | |_) \__ \ | | |___| (_| | |_) \__ \
* |_|______\__,_|_.__/|___/ |_|______\__,_|_.__/|___/
*
* ---------------------------------------------------------------------------- ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42): "THE BEER-WARE LICENSE" (Revision 42):
* <pontus@ilabs.se> wrote this file. As long as you retain this notice you <pontus@ilabs.se> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return - Pontus Oldberg this stuff is worth it, you can buy me a beer in return - Pontus Oldberg
* ---------------------------------------------------------------------------- ----------------------------------------------------------------------------
*/ */
#include "dw3000.h" #include "dw3000.h"
#include "app.h" #include "app.h"
#define APP_NAME "ANCHOR" #define APP_NAME "ANCHOR 1"
#define TX_ANT_DLY 16385 #define TX_ANT_DLY 16385
#define RX_ANT_DLY 16385 #define RX_ANT_DLY 16385
...@@ -34,19 +34,19 @@ ...@@ -34,19 +34,19 @@
/* Default communication configuration. We use default non-STS DW mode. */ /* Default communication configuration. We use default non-STS DW mode. */
static dwt_config_t config = { static dwt_config_t config = {
5, /* Channel number. */ 5, /* Channel number. */
DWT_PLEN_128, /* Preamble length. Used in TX only. */ DWT_PLEN_128, /* Preamble length. Used in TX only. */
DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */ DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
9, /* TX preamble code. Used in TX only. */ 9, /* TX preamble code. Used in TX only. */
9, /* RX preamble code. Used in RX only. */ 9, /* RX preamble code. Used in RX only. */
1, /* 0 to use standard 8 symbol SFD, 1 to use non-standard 8 symbol, 2 for non-standard 16 symbol SFD and 3 for 4z 8 symbol SDF type */ 1, /* 0 to use standard 8 symbol SFD, 1 to use non-standard 8 symbol, 2 for non-standard 16 symbol SFD and 3 for 4z 8 symbol SDF type */
DWT_BR_6M8, /* Data rate. */ DWT_BR_6M8, /* Data rate. */
DWT_PHRMODE_STD, /* PHY header mode. */ DWT_PHRMODE_STD, /* PHY header mode. */
DWT_PHRRATE_STD, /* PHY header rate. */ DWT_PHRRATE_STD, /* PHY header rate. */
(129 + 8 - 8), /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */ (129 + 8 - 8), /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
DWT_STS_MODE_OFF, /* STS disabled */ DWT_STS_MODE_OFF, /* STS disabled */
DWT_STS_LEN_64, /* STS length see allowed values in Enum dwt_sts_lengths_e */ DWT_STS_LEN_64, /* STS length see allowed values in Enum dwt_sts_lengths_e */
DWT_PDOA_M0 /* PDOA mode off */ DWT_PDOA_M0 /* PDOA mode off */
}; };
size_t rxLen; size_t rxLen;
...@@ -65,8 +65,8 @@ void setup() ...@@ -65,8 +65,8 @@ void setup()
{ {
pinMode(LED_BUILTIN, OUTPUT); pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, LOW); digitalWrite(LED_BUILTIN, LOW);
// while (!Serial) while (!Serial)
delay(100); delay(100);
Serial.begin(115200); Serial.begin(115200);
...@@ -88,7 +88,7 @@ void setup() ...@@ -88,7 +88,7 @@ void setup()
dwt_softreset(); dwt_softreset();
delay(200); delay(200);
while (!dwt_checkidlerc()) // Need to make sure DW IC is in IDLE_RC before proceeding while (!dwt_checkidlerc()) // Need to make sure DW IC is in IDLE_RC before proceeding
{ {
Serial.print("IDLE FAILED\r\n"); Serial.print("IDLE FAILED\r\n");
while (1); while (1);
...@@ -111,11 +111,11 @@ void setup() ...@@ -111,11 +111,11 @@ void setup()
dwt_setrxantennadelay(RX_ANT_DLY); dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY); dwt_settxantennadelay(TX_ANT_DLY);
dwt_setpanid(PAN_ID); dwt_setpanid(PAN_ID);
dwt_setaddress16(MY_ADDR); dwt_setaddress16(MY_ADDR);
dwt_configureframefilter(DWT_FF_ENABLE_802_15_4, DWT_FF_DATA_EN | DWT_FF_ACK_EN | DWT_FF_IMPBRCAST_EN | DWT_FF_MAC_EN); dwt_configureframefilter(DWT_FF_ENABLE_802_15_4, DWT_FF_DATA_EN | DWT_FF_ACK_EN | DWT_FF_IMPBRCAST_EN | DWT_FF_MAC_EN);
dwt_setrxtimeout(RX_TIMEOUT); dwt_setrxtimeout(RX_TIMEOUT);
} }
...@@ -132,53 +132,86 @@ void loop() ...@@ -132,53 +132,86 @@ void loop()
// Beacons will only ever be in BEACON_POSITIONING_MODE mode during beacon positioning // Beacons will only ever be in BEACON_POSITIONING_MODE mode during beacon positioning
// hence this portion of code only does ranging with other anchors // hence this portion of code only does ranging with other anchors
case BEACON_POSITIONING_MODE: case BEACON_POSITIONING_MODE:
double dist; dwt_setrxtimeout(RX_TIMEOUT);
if (NUMBER_OF_BEACONS_TO_RANGE > 0) { double dist;
uint16_t addresses[NUMBER_OF_BEACONS_TO_RANGE] = {}; if (NUMBER_OF_BEACONS_TO_RANGE > 0) {
double distances[NUMBER_OF_BEACONS_TO_RANGE] = {}; uint16_t addresses[NUMBER_OF_BEACONS_TO_RANGE] = {};
double distances[NUMBER_OF_BEACONS_TO_RANGE] = {};
// range the remaining beacons
for (uint8_t i = 0; i < NUMBER_OF_BEACONS_TO_RANGE; i++) { // anchor shouldn't range itself // range the remaining beacons
addresses[i] = i + MY_ADDR + 1; // store destination address for (uint8_t i = 0; i < NUMBER_OF_BEACONS_TO_RANGE; i++) { // anchor shouldn't range itself
distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR, 3); // range beacon at addresses[i] addresses[i] = i + 1; // store destination address
double totalDistance = 0.0; // Variable to accumulate distances for averaging
int validMeasurements = 0; // Counter for valid measurements
double measuredDistance = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR, 1); // single range
if (measuredDistance != 0) { // If a valid distance is returned
totalDistance += measuredDistance; // Add the valid distance to the total
validMeasurements++; // Increment valid measurement count
Serial.print("Distance: ");
Serial.print(measuredDistance);
Serial.println("m");
delay(INTER_RANGING_DELAY); // Delay between range requests
}
// // If at least one valid measurement was made, calculate average distance
// if (validMeasurements > 0) {
// distances[i] = totalDistance / validMeasurements; // Calculate average distance
// Serial.print("Average Distance: ");
// Serial.print(distances[i]);
// Serial.println("m");
// } else {
// distances[i] = 0; // No valid measurements, set distance to 0
// Serial.println("No valid measurements for this beacon.");
// }
// }
}
Serial.println("Sending distances to tag");
ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR);
delay(INTER_RANGING_DELAY); delay(INTER_RANGING_DELAY);
}
Serial.println("Sending distances to tag"); if (MY_ADDR < ANCHOR3) {
ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR); Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1);
delay(INTER_RANGING_DELAY); ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
delay(INTER_RANGING_DELAY);
}
}
state = RESPONDER;
break;
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);
}
}
state = RESPONDER;
break;
case RESPONDER: case RESPONDER:
frameReceived = receiveData(&rxMHR, &rxLen, rxBuffer);
if (!frameReceived) {return;} // jump to next loop iteration if frame timed out dwt_setrxtimeout(0);
frameReceived = receiveData(&rxMHR, &rxLen, rxBuffer);
// Beacon should never receive SEND_DISTANCES and POLL_RESPONSE commands if (!frameReceived) {
switch (rxBuffer[0]) { // command is the first byte return; // jump to next loop iteration if frame timed out
case LIVE_CHECK: }
sendAcknowledgement(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR));
Serial.printf("Live check from %d\n", getMacSrcAddr(&rxMHR)); // Beacon should never receive SEND_DISTANCES and POLL_RESPONSE commands
break; switch (rxBuffer[0]) { // command is the first byte
case LIVE_CHECK:
case BPM_COMMAND: //range other beacons sendAcknowledgement(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR));
sendAcknowledgement(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR)); Serial.printf("Live check from %d\n", getMacSrcAddr(&rxMHR));
Serial.println("bpm"); break;
state = BEACON_POSITIONING_MODE;
break; case BPM_COMMAND: //range other beacons
sendAcknowledgement(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR));
case POLL_REQUEST: //TOF Serial.println("bpm");
uint64_t poll_rx_ts = get_rx_timestamp_u64(); state = BEACON_POSITIONING_MODE;
handlePollRequest(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR), poll_rx_ts); break;
break;
} case POLL_REQUEST: //TOF
break; Serial.println("poll request");
uint64_t poll_rx_ts = get_rx_timestamp_u64();
handlePollRequest(&txMHR, PAN_ID, MY_ADDR, getMacSrcAddr(&rxMHR), poll_rx_ts);
break;
}
break;
} }
} }
Wireless_Communication/UWB/Anchor/anchors/Anchor2/Anchor2.ino
+ 18
− 7
View file @ 27ae5624
...@@ -18,7 +18,7 @@ ...@@ -18,7 +18,7 @@
#include "dw3000.h" #include "dw3000.h"
#include "app.h" #include "app.h"
#define APP_NAME "ANCHOR" #define APP_NAME "ANCHOR 2"
#define TX_ANT_DLY 16385 #define TX_ANT_DLY 16385
#define RX_ANT_DLY 16385 #define RX_ANT_DLY 16385
...@@ -59,14 +59,14 @@ extern dwt_txconfig_t txconfig_options; ...@@ -59,14 +59,14 @@ extern dwt_txconfig_t txconfig_options;
macHeader txMHR, rxMHR; macHeader txMHR, rxMHR;
bool frameReceived = 0; bool frameReceived = 0;
bool ack; bool ack;
deviceState state = RESPONDER; deviceState state = BEACON_POSITIONING_MODE;
void setup() void setup()
{ {
pinMode(LED_BUILTIN, OUTPUT); pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, LOW); digitalWrite(LED_BUILTIN, LOW);
// while (!Serial) while (!Serial)
delay(100); delay(100);
Serial.begin(115200); Serial.begin(115200);
...@@ -138,9 +138,20 @@ void loop() ...@@ -138,9 +138,20 @@ void loop()
double distances[NUMBER_OF_BEACONS_TO_RANGE] = {}; double distances[NUMBER_OF_BEACONS_TO_RANGE] = {};
// range the remaining beacons // range the remaining beacons
for (uint8_t i = 0; i < NUMBER_OF_BEACONS_TO_RANGE; i++) { // anchor shouldn't range itself for (uint8_t i = 0; i < NUMBER_OF_BEACONS_TO_RANGE; i++) { // anchor shouldn't range itself
addresses[i] = i + MY_ADDR + 1; // store destination address addresses[i] = i + 1; // store destination address
distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR, 3); // range beacon at addresses[i] Serial.print("Sending to anchor");
Serial.println(addresses[i]);
distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR,31 ); // range beacon at addresses[i]
if (distances[i] != 0) { // If a valid distance is returned
Serial.print("Distance: ");
Serial.print(distances[i]);
Serial.println("m");
}
delay(INTER_RANGING_DELAY); delay(INTER_RANGING_DELAY);
} }
...@@ -150,7 +161,7 @@ void loop() ...@@ -150,7 +161,7 @@ void loop()
if (MY_ADDR < ANCHOR3) { if (MY_ADDR < ANCHOR3) {
Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1); 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); ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR - 1, BPM_COMMAND, &rxMHR, 3);
delay(INTER_RANGING_DELAY); delay(INTER_RANGING_DELAY);
} }
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment