Problem when connecting more than 4 anchors and 1 tag

[santiagocano298366]

I have a problem when connecting more than 4 anchors, I am using a decawave DW1000 module and I don't know how to modify the code to show me the location with more than 4 anchors, at the moment I want to test it with 6 anchors and a tag.

What parameters should I modify, or what part of the code or library should I change to make it work with 6 anchors?

[jremington]

Unfortunately, the DW1000 library by Thomas Trojer has one or more bugs and does not support more than five devices.

Various people have looked, but no one seems to know what the problem is.

[pizzo00]

The problem is that the tag is telling the anchors when to respond in order to avoid packet collision and this information is stored on the packet. The packet has a maximum size and every anchor information occupies some space.

I have modified the libraries in order to have multiple anchors and multiple tags but I need to adapt it before publishing it to the public because at the moment it has some paricular feature that is specific to my use case

[pizzo00]

@jremington I have re-write and changes the library in various part, if you want we can publish it in this repo under another folder, or we can create a github organization to work on it

[pizzo00]

For example:

• I have added informations about the known anchors on blink so only the unkown will respond
• After a poll an anchor that is not on the poll can respond with a ranging_init on some free time slots (to reduce the wait time until a blink)
• I have increased (by reducing the transmitted data) the maximum number of anchor per tag to 6, the tag can still know more than 6 anchor and query only the 6 with the best signal
• The system is working with multiple tags, the limit is the occupation of the channel so the number of tags supported depends on the update frequency
• Tag would wait for the last poll ack to arrive before sending the range (so if the last anchor is offline you have to wait for it to be remove for inactivity) now it wait for the last one or use a timeout, so the range is always sent

[jremington]

Sounds fantastic! I would be delighted to add another library folder for your version, or once tested, just replace the existing ones. I have to modify the trilateration code to accommodate more anchors as well.

I have no experience with creating a github team but am happy to learn how.

[russelh15]

This sounds promising!

@pizzo00 Do you have the code published anywhere? I help test if needed. I have 6 chips in total, so I can test with 4 anchors and 2 tags, or 5 anchors and 1 tag.

[anthony211212]

Do you encounter occasional random values in the distance measurements when you increase the number of nodes? If not, how did you prevent this?

[jremington]

Yes, occasionally there are errors in the distance measurements. Errors cannot be prevented, only recognized and ignored.

[pizzo00]

Sounds fantastic! I would be delighted to add another library folder for your version, or once tested, just replace the existing ones. I have to modify the trilateration code to accommodate more anchors as well.

I have no experience with creating a github team but am happy to learn how.

Maybe the best solution is to add a folder to this repo, I will prepare a pull request as soon as possible

[pizzo00]

@jremington I have created pull request #23

[hsutzu]

Hi, I am using 4 anchors and 1 tag, and when I use them at the same time, I find out there is a large error, for example, it stayed at 1.5 meters, but suddenly it jumped to 33.7 meters, and then dropped slowly, after that it still may turn into large range number occasionally, has anyone had the same problem and have any solutions on it?

I want to do the 3D localization but with this error, I cannot do the localization, since the original anchor to tag range is wrong, and the filter did not help.

[anthony211212]

First of all, can the antennas clearly see each other? Also, which tag code are you using? Lastly, are the coordinates you've entered for the anchors correct?

[jremington]

I occasionally see large distance errors, but they don't "drop slowly" and can be filtered out using a median filter.

Did you calibrate the antenna delay for each anchor? While doing that, you have a good opportunity to test each anchor+ tag setup and check whether the distance reports are well behaved and stable.

[hsutzu]

First of all, can the antennas clearly see each other? Also, which tag code are you using? Lastly, are the coordinates you've entered for the anchors correct?

yes, I have make sure the antenna able to see each other, the following method of mean error is how I calibrate them, however when I use one anchor to tag the error can be very small and won't have occasionally error, since I saw you mention about this problem, so i think maybe you know the solution of it, I've been bother for this problem for a long time
my tag code is as following:

#include <SPI.h>
#include "DW1000Ranging.h" 
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include "link.h"

// Define the pin connected to the output of the voltage divider
#define VOLTAGE_PIN 36  // GPIO 35 (ADC1_CH7) is commonly used for analog input

#define TAG_ADDR "7D:00:22:EA:82:60:3B:9B" // UWB Tag Address

#define AVERAGE_ERROR 1.0  // Define a default mean error if the anchor is not found

// SPI Pins for UWB
#define SPI_SCK 18
#define SPI_MISO 19
#define SPI_MOSI 23

// UWB Pin Definitions
#define UWB_RST 27  // Reset pin
#define UWB_IRQ 34  // IRQ pin
#define UWB_SS 21   // SPI select pin

// I2C Pins for OLED Display
#define I2C_SDA 4
#define I2C_SCL 5

// WiFi Credentials
const char *ssid = "BBLAB_New";
const char *password = "ntubime405";
const char *host = "192.168.0.137"; // The IP of the Python UDP listener
const int udpPort = 8080;           // The port of the Python UDP listener
unsigned long primaryTimestamp = 0;

WiFiUDP udp;  // UDP instance

// OLED Display Settings
Adafruit_SSD1306 display(128, 64, &Wire, -1);

// Mean Errors for Anchors
struct AnchorInfo {
    String address;
    float meanError;
};

// List of anchors and their mean errors
AnchorInfo anchors[] = {
    {"82175BD5A99AE29C", 0.94}, // Anchor 1
    {"83175BD5A99AE29D", 0.91}, // Anchor 2
    {"85175BD5A99AE29A", 0.97}, // Anchor 3
    {"84175BD5A99AE29B", 1.03}  // Anchor 4
};

// Link structure pointer
struct MyLink *uwb_data;
long runtime = 0;
String all_json = "";
unsigned long lastSendTime = 0;  // Track the last time a message was sent

void setup() {
    Serial.begin(115200);

    // Initialize I2C for OLED Display
    Wire.begin(I2C_SDA, I2C_SCL);
    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("SSD1306 allocation failed"));
        for (;;);
    }
    display.clearDisplay();

    // Show initial logo on OLED
    logoshow();

    // Measure and display the voltage before connecting to Wi-Fi
    measure_and_display_voltage();

    // Initialize SPI for UWB Communication
    SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI);

    // UWB Module Initialization
    DW1000Ranging.initCommunication(UWB_RST, UWB_SS, UWB_IRQ); // Initialize communication with Reset, CS, and IRQ pins
    DW1000Ranging.attachNewRange(newRange);
    DW1000Ranging.attachNewDevice(newDevice);
    DW1000Ranging.attachInactiveDevice(inactiveDevice);
    DW1000Ranging.startAsTag(TAG_ADDR, DW1000.MODE_LONGDATA_RANGE_LOWPOWER);

    // Initialize WiFi connection
    WiFi.mode(WIFI_STA);
    WiFi.setSleep(false);
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("Connected");
    Serial.print("IP Address: ");
    Serial.println(WiFi.localIP());

    // Initialize UWB data link structure
    uwb_data = init_link();

    // Initialize UDP
    udp.begin(udpPort);
}

void loop() {
    
    DW1000Ranging.loop();

    // Update display and send data every second
    if (millis() - runtime >= 1000) {
        display_uwb(uwb_data);
        make_link_json(uwb_data, &all_json);
        send_udp(&all_json);
        runtime += 1000;  // Ensure fixed 1-second intervals
    }
}

// Function to measure the voltage and display it on the OLED
void measure_and_display_voltage() {
    // Read the raw ADC value (0 - 4095)
    int adc_value = analogRead(VOLTAGE_PIN);

    // Convert the ADC value to actual voltage (3.3V reference) for a voltage divider that divides 5V to 2.5V
    float voltage = adc_value * (3.3 / 4095.0) * 2;  // Multiply by 2 to account for the voltage divider

    // Display the voltage on the OLED
    display.setTextSize(2);
    display.setCursor(0, 48); // Position below the other content
    display.clearDisplay();
    display.setTextColor(SSD1306_WHITE);
    display.print("V: ");
    display.print(voltage);
    display.println("V");
    display.display();

    // For debugging purposes
    Serial.print("Measured Voltage: ");
    Serial.print(voltage);
    Serial.println("V");
}

// Function to find the mean error based on the anchor address
float getMeanError(String anchorAddress) {
    for (int i = 0; i < sizeof(anchors)/sizeof(anchors[0]); i++) {
        if (anchors[i].address == anchorAddress) {
            return anchors[i].meanError;
        }
    }
    return AVERAGE_ERROR; // Default mean error if the anchor is not found
}

// UWB Range Callback
void newRange() {
    String anchorAddress = String(DW1000Ranging.getDistantDevice()->getShortAddress(), HEX);
    anchorAddress.toUpperCase(); // Convert to uppercase for consistency
    float meanError = getMeanError(anchorAddress);

    float measuredRange = DW1000Ranging.getDistantDevice()->getRange();
    float correctedRange = measuredRange - meanError;  // Apply the specific mean error
    unsigned long timestamp = micros();  // Use micros() for precise timing
    // Check if this is the primary anchor
    if (anchorAddress == "82175BD5A99AE29C") {  // Example: First anchor as primary
        primaryTimestamp = currentTimestamp;
    }


    Serial.print("From: ");
    Serial.print(anchorAddress);
    Serial.print("\t Corrected Range: ");
    Serial.print(correctedRange, 2);  // Print with two decimal places
    Serial.print(" m");
    Serial.print("\t RX power: ");
    Serial.print(DW1000Ranging.getDistantDevice()->getRXPower());
    Serial.print(" dBm");
    Serial.print("\t Timestamp: ");
    Serial.println(primaryTimestamp);  // Print the timestamp

    // Update the link data structure with the new corrected range and timestamp
    fresh_link(uwb_data, DW1000Ranging.getDistantDevice()->getShortAddress(), correctedRange, DW1000Ranging.getDistantDevice()->getRXPower(), primaryTimestamp);
        
    // Send UDP packet immediately after new measurement
    make_link_json(uwb_data, &all_json);
    send_udp(&all_json);
}

// UWB New Device Callback
void newDevice(DW1000Device *device) {
    Serial.print("Ranging init; 1 device added! -> Short address: ");
    Serial.println(device->getShortAddress(), HEX);

    // Add the new device to the link structure
    add_link(uwb_data, device->getShortAddress());
}

// UWB Inactive Device Callback
void inactiveDevice(DW1000Device *device) {
    Serial.print("Removing inactive device: ");
    Serial.println(device->getShortAddress(), HEX);

    // Remove the inactive device from the link structure
    delete_link(uwb_data, device->getShortAddress());
}

void send_udp(String *msg_json) {
    udp.beginPacket(host, udpPort); // Begin UDP packet

    *msg_json += "\"Timestamp\":" + String(primaryTimestamp) + ",";
    // Convert the string to a byte array and send it
    //udp.write((const uint8_t *)msg_json->c_str(), msg_json->length());

    udp.endPacket(); // End and send the packet
    Serial.println("UDP data sent");
}


// OLED Logo Display
void logoshow(void) {
    display.clearDisplay();
    display.setTextSize(2);              // Normal 1:1 pixel scale
    display.setTextColor(SSD1306_WHITE); // Draw white text
    display.setCursor(0, 0);             // Start at top-left corner
    display.println(F("Makerfabs"));
    display.setTextSize(1);
    display.setCursor(0, 20);            // Start at top-left corner
    display.println(F("DW1000 DEMO"));
    display.display();
    delay(2000);
}

// Function to Display UWB Data on OLED
void display_uwb(struct MyLink *p) {
    struct MyLink *temp = p;
    int row = 0;

    display.clearDisplay();
    display.setTextColor(SSD1306_WHITE);
    display.setTextSize(1); // Smaller text size to fit more data

    if (temp->next == NULL) {
        display.setTextSize(2);
        display.setCursor(0, 0);
        display.println("No Anchor");
        display.display();
        return;
    }

    while (temp->next != NULL) {
        temp = temp->next;
        char c[40];

        sprintf(c, "Addr: %X\nRange: %.2fm", temp->anchor_addr, temp->range[0]);
        display.setCursor(0, row * 16); // Adjust row spacing for multiple anchors
        display.println(c);

        if (++row >= 4) { // Limit to 4 rows to fit the screen
            break;
        }
    }
    display.display();
}

Is the coordinate that you mention the placement of the anchors' coordinates? If so, I had make sure the coordinate is right!

[anthony211212]

First, please try the standard codes for trilateration in a 2D scenario with 4 anchors and 1 tag. You likely won't encounter a significant error.