Hello im so sorry i couldn't put my entire code here because of the word limit.. so i have taken out a little of the functions below, but it doesn't really affect the question that i am asking so it should be ok.
Sorry i am a beginner in Arduino programming you see..
#include <Pozyx.h>
#include <Pozyx_definitions.h>
#include <Wire.h>
#define FILTER_TYPE_NONE 0x00 // no filter applied ( default )
#define FILTER_TYPE_FIR 0x01 // a LPF is appliled to filter out HF jitters
#define FILTER_TYPE_MOVINGAVERAGE 0x03 // moving avg is applied to smoothen trajectory
#define FILTER_TYPE_MOVINGMEDIAN 0x04 // moving median is applied filter out outliers
////////////////////////////////////////////////
////////////////// PARAMETERS //////////////////
////////////////////////////////////////////////
uint16_t remote_id = 0x6000; // set this to the ID of the remote device
bool remote = true; // set this to true to use the remote ID
uint32_t last_millis;
boolean use_processing = true; // set this to true to output data for the processing sketch
// the number of anchors
const uint8_t num_anchors = 4; // the number of anchors
uint16_t anchors[num_anchors] = {0x6969, 0x6961, 0x6e6d, 0x6923 }; // the network id of the anchors: change these to the network ids of your anchors.
int32_t anchors_x[num_anchors] = {0, 0 , 1470, 3060 }; // anchor x-coorindates in mm
int32_t anchors_y[num_anchors] = {0, 2350, 0, 1590 }; // anchor y-coordinates in mm
int32_t heights[num_anchors] = {-750, 0 , 0, 1150 }; // anchor z-coordinates in mm
uint8_t algorithm = POZYX_POS_ALG_UWB_ONLY; // positioning algorithm to use. try POZYX_POS_ALG_TRACKING for fast moving objects. POZYX_POS_ALG_UWB_ONLY
uint8_t dimension = POZYX_3D; // positioning dimension
int32_t height = 0; // height of device, required in 2.5D positioning
////////////////////////////////////////////////
void setup(){
Serial.begin(115200);
if(Pozyx.begin(false, MODE_INTERRUPT, POZYX_INT_MASK_IMU) == POZYX_FAILURE){
Serial.println(F("ERROR: Unable to connect to POZYX shield"));
Serial.println(F("Reset required"));
delay(100);
abort();
}
if(!remote){
remote_id = NULL;
last_millis = millis();
delay(10);
}
Serial.println(F("----------POZYX POSITIONING V1.1----------"));
Serial.println(F("NOTES:"));
Serial.println(F("- No parameters required."));
Serial.println();
Serial.println(F("- System will auto start anchor configuration"));
Serial.println();
Serial.println(F("- System will auto start positioning"));
Serial.println(F("----------POZYX POSITIONING V1.1----------"));
Serial.println();
Serial.println(F("Performing manual anchor configuration:"));
// clear all previous devices in the device list
Pozyx.clearDevices(remote_id);
// sets the anchor manually
setAnchorsManual();
// sets the positioning algorithm
Pozyx.setPositionAlgorithm(algorithm, dimension, remote_id);
printCalibrationResult();
delay(2000);
Serial.println(F("Starting positioning: "));
}
void loop(){
coordinates_t position;
int status;
int dt;
uint8_t calibration_status = 0;
sensor_raw_t sensor_raw;
if(remote){
status = Pozyx.getRawSensorData(&sensor_raw, remote_id);
status &= Pozyx.getCalibrationStatus(&calibration_status, remote_id);
status = Pozyx.doRemotePositioning(remote_id, &position, dimension, height, algorithm);
printOrientationAcceleration();
//Pozyx.setPositionFilter(FILTER_TYPE_MOVINGAVERAGE, 10 , remote_id ); // filter_type, filter_strength, remote. Strength 0-15 ( position will be delayed by the number of samples that = to the strength )
Serial.println();
}
if (status == POZYX_SUCCESS){
dt = millis() - last_millis;
last_millis += dt;
// print time difference between last measurement in ms, sensor data, and calibration data
Serial.print(dt, DEC);
Serial.print(",");
printRawSensorData(sensor_raw);
Serial.print(",");
// will be zeros for remote devices as unavailable remotely.
printCalibrationStatus(calibration_status);
Serial.println();
// prints out the result
printCoordinates(position);
Serial.println();
Serial.println();
Serial.println();
Serial.println();
Serial.println();
}
else{
// prints out the error code
printErrorCode("positioning");
}
}
// prints the coordinates for either humans or for processing
void printCoordinates(coordinates_t coor){
uint16_t network_id = remote_id;
if (network_id == NULL){
network_id = 0;
}
if(!use_processing){
Serial.print("POS ID 0x");
Serial.print(network_id, HEX);
Serial.print(", x(mm): ");
Serial.print(coor.x);
Serial.print(", y(mm): ");
Serial.print(coor.y);
Serial.print(", z(mm): ");
Serial.println(coor.z);
//delay(10);
}else{
Serial.print("POS,0x");
Serial.print(network_id,HEX);
Serial.print(",");
Serial.print(coor.x);
Serial.print(",");
Serial.print(coor.y);
Serial.print(",");
Serial.println(coor.z);
//delay(10);
}
}
// error printing function for debugging
void printErrorCode(String operation){
uint8_t error_code;
if (remote_id == NULL){
Pozyx.getErrorCode(&error_code);
Serial.print("ERROR ");
Serial.print(operation);
Serial.print(", local error code: 0x");
Serial.println(error_code, HEX);
return;
}
int status = Pozyx.getErrorCode(&error_code, remote_id);
if(status == POZYX_SUCCESS){
Serial.print("ERROR ");
Serial.print(operation);
Serial.print(" on ID 0x");
Serial.print(remote_id, HEX);
Serial.print(", error code: 0x");
Serial.println(error_code, HEX);
}else{
Pozyx.getErrorCode(&error_code);
Serial.print("ERROR ");
Serial.print(operation);
Serial.print(", couldn't retrieve remote error code, local error: 0x");
Serial.println(error_code, HEX);
}
}
// print out the anchor coordinates (also required for the processing sketch)
void printCalibrationResult(){
uint8_t list_size;
int status;
status = Pozyx.getDeviceListSize(&list_size, remote_id);
Serial.print("list size: ");
Serial.println(status*list_size);
if(list_size == 0){
printErrorCode("configuration");
return;
}
uint16_t device_ids[list_size];
status &= Pozyx.getDeviceIds(device_ids, list_size, remote_id);
Serial.println(F("Calibration result:"));
Serial.print(F("Anchors found: "));
Serial.println(list_size);
coordinates_t anchor_coor;
for(int i = 0; i < list_size; i++)
{
Serial.print("ANCHOR,");
Serial.print("0x");
Serial.print(device_ids[i], HEX);
Serial.print(",");
Pozyx.getDeviceCoordinates(device_ids[i], &anchor_coor, remote_id);
Serial.print(anchor_coor.x);
Serial.print(",");
Serial.print(anchor_coor.y);
Serial.print(",");
Serial.println(anchor_coor.z);
}
}
// function to manually set the anchor coordinates
void setAnchorsManual(){
for(int i = 0; i < num_anchors; i++){
device_coordinates_t anchor;
anchor.network_id = anchors[i];
anchor.flag = 0x1;
anchor.pos.x = anchors_x[i];
anchor.pos.y = anchors_y[i];
anchor.pos.z = heights[i];
Pozyx.addDevice(anchor, remote_id);
}
if (num_anchors > 4){
Pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO, num_anchors, remote_id);
}
}