#include <Pozyx.h>
#include <Pozyx_definitions.h>
#include <SoftwareSerial.h>
#include <Wire.h>
////////////////// Pozyx Prams //////////////////////////////
#define CONFIG_TX_GAIN 33.0f
#define NUM_ANCHORS 4
// the network id of the anchors: change these to the network ids of your anchors.
uint16_t anchor_id[4] = { 0x610e, // (0,0)
0x610f, // x-axis
0x6166, // y-axis
0x6167};
// only required for manual anchor calibration.
// Please change this to the coordinates measured for the anchors
int32_t anchors_x[NUM_ANCHORS] = {0, 4700, 0, 4700}; // anchor x-coorindates in mm (horizontal)
int32_t anchors_y[NUM_ANCHORS] = {0, 0, 5150, 5150}; // anchor y-coordinates in mm (vertical)
int32_t heights[NUM_ANCHORS] = {-1400, -1450, -1350, -1550}; // anchor z-coordinates in mm (1.2m above vehicle's starting altitude)
// RX TX serial for flight controller ex) Pixhawk
// https://github.com/PaulStoffregen/AltSoftSerial
SoftwareSerial fcboardSerial(10, 11); // rx, tx
#define MSG_HEADER 0x01
#define MSGID_BEACON_CONFIG 0x02
#define MSGID_BEACON_DIST 0x03
#define MSGID_POSITION 0x04
// structure for messages uploaded to ardupilot
union beacon_config_msg {
struct {
uint8_t beacon_id;
uint8_t beacon_count;
int32_t x;
int32_t y;
int32_t z;
} info;
uint8_t buf[14];
};
union beacon_distance_msg {
struct {
uint8_t beacon_id;
uint32_t distance;
} info;
uint8_t buf[5];
};
union vehicle_position_msg {
struct {
int32_t x;
int32_t y;
int32_t z;
int16_t position_error;
} info;
uint8_t buf[14];
};
////////////////////////////////////////////////
void setup()
{
Serial.begin(115200);
fcboardSerial.begin(115200);
if (Pozyx.begin() == POZYX_FAILURE) {
Serial.println(("ERR: shield"));
delay(100);
abort();
}
Serial.println(("V1.0"));
// clear all previous devices in the device list
Pozyx.clearDevices();
// configure beacons
while (!configure_beacons()) {
delay(1000);
}
int status = Pozyx.doAnchorCalibration(POZYX_2_5D, 10, NUM_ANCHORS, anchor_id, heights);
if (status != POZYX_SUCCESS) {
Serial.println(status);
Serial.println(("ERROR: calibration"));
Serial.println(("Reset required"));
delay(100);
abort();
}
// if the automatic anchor calibration is unsuccessful, try manually setting the anchor coordinates.
// fot this, you must update the arrays anchors_x, anchors_y and heights above
// comment out the doAnchorCalibration block and the if-statement above if you are using manual mode
// SetAnchorsManual();
print_anchor_coordinates();
Serial.println(("Waiting.."));
delay(5000);
Serial.println(("Starting: "));
}
void loop()
{
static uint32_t loop_start = 0;
static uint8_t stage = 0; // 0 = initialisation, 1 = normal flight
static uint16_t beacon_sent_count = 0;
static uint32_t beacon_sent_time = 0;
// initialise start time
if (loop_start == 0) {
loop_start = millis();
}
// advance to normal flight stage after 1min
if (stage == 0) {
uint32_t time_diff = (millis() - loop_start);
if (time_diff > 60000) {
stage = 1;
Serial.println("Stage1");
}
}
// slow down counter
static uint8_t counter = 0;
counter++;
if (counter >= 20) {
counter = 0;
}
// during stage 0 (init) send position and beacon config as quickly as possible
// during stage 1 send about every 2 seconds
if (stage == 0 || counter == 0) {
send_beacon_config();
get_position();
if (beacon_sent_count > 0 && beacon_sent_time != 0) {
uint32_t time_diff = millis() - beacon_sent_time;
float hz = (float)beacon_sent_count / (time_diff / 1000.0f);
Serial.print("Beacon hz:");
Serial.println(hz);
}
beacon_sent_count = 0;
beacon_sent_time = millis();
}
// send beacon distances
get_ranges();
beacon_sent_count++;
}
uint32_t time_start_ms;
void timer_start()
{
time_start_ms = millis();
}
void timer_end()
{
uint32_t time_diff = millis() - time_start_ms;
Serial.print("ms:");
Serial.println(time_diff);
}
void print_comma()
{
Serial.print(",");
}
void print_tab()
{
Serial.print("\t");
}
// set a tag or anchor's gain
// set tag_id to zero to set local device's gain
// returns true on success
bool set_device_gain(uint16_t dev_id, float gain)
{
float tx_power = -1;
// get/set transmit power of tag
bool gain_ok = false;
uint8_t retry = 0;
while (!gain_ok && retry < 5) {
if (Pozyx.getTxPower(&tx_power, dev_id) == POZYX_SUCCESS) {
if (tx_power != gain) {
Pozyx.setTxPower(CONFIG_TX_GAIN, dev_id);
} else {
gain_ok = true;
}
}
retry++;
}
// display final gain
Serial.print("Dev ");
Serial.print(dev_id, HEX);
Serial.print(" gain ");
if (tx_power > 0) {
Serial.print(tx_power);
} else {
Serial.print("unknown");
}
Serial.print(" (retry ");
Serial.print(retry);
Serial.print(")");
Serial.println();
return gain_ok;
}
// performs repeated calls to get reliable distance between devices
bool get_remote_range(uint16_t dev1, uint16_t dev2, int32_t& range)
{
// set distances between tags
uint32_t range_tot = 0;
uint16_t count = 0;
device_range_t dev_range;
for (uint8_t i=0; i <= 10; i++) {
// origin to 1st
if (Pozyx.doRemoteRanging(dev1, dev2, &dev_range) == POZYX_SUCCESS) {
range_tot += dev_range.distance;
count++;
}
if (Pozyx.doRemoteRanging(dev2, dev1, &dev_range) == POZYX_SUCCESS) {
range_tot += dev_range.distance;
count++;
}
}
// success if at least 5 successful ranges were retrieved
if (count > 5) {
range = range_tot / count;
return true;
}
return false;
}
void print_failed_to_range(uint16_t dev1, uint16_t dev2)
{
Serial.print("ranging fail ");
Serial.print(dev1,HEX);
Serial.print(" to ");
Serial.println(dev2,HEX);
}
void set_beacon_position(uint8_t index, int32_t x_mm, int32_t y_mm, int32_t z_mm)
{
anchors_x[index] = x_mm;
anchors_y[index] = y_mm;
heights[index] = z_mm;
}
// configure beacons
bool configure_beacons()
{
bool configured_ok = true;
// get/set transmit power of tag
if (!set_device_gain(0, CONFIG_TX_GAIN)) {
configured_ok = false;
}
// set transmit power of beacons
for (uint8_t i=0; i < NUM_ANCHORS; i++) {
if (!set_device_gain(anchor_id[i], CONFIG_TX_GAIN)) {
configured_ok = false;
}
}
// set distances between tags
int32_t x_range = 0, y_range = 0;
// origin to x-axis (i.e. bottom right)
if (get_remote_range(anchor_id[0], anchor_id[1], x_range)) {
set_beacon_position(1, x_range, 0, heights[1]);
} else {
print_failed_to_range(anchor_id[0], anchor_id[1]);
configured_ok = false;
}
// origin to y-axis (i.e. top left)
if (get_remote_range(anchor_id[0], anchor_id[2], y_range)) {
set_beacon_position(2, 0, y_range, heights[2]);
} else {
print_failed_to_range(anchor_id[0], anchor_id[2]);
configured_ok = false;
}
// top right
if (x_range != 0 && y_range != 0) {
set_beacon_position(3, x_range, y_range, heights[3]);
} else {
Serial.println("beacons too close");
configured_ok = false;
}
if (configured_ok) {
Serial.println("Beacon Configuration complete");
} else {
Serial.println("Beacon Configuration failed!");
}
return configured_ok;
}
// function to manually set the anchor coordinates
void SetAnchorsManual()
{
for (uint8_t i=0; i<NUM_ANCHORS; i++) {
device_coordinates_t anchor;
anchor.network_id = anchor_id[i];
anchor.flag = 0x1;
anchor.pos.x = anchors_x[i];
anchor.pos.y = anchors_y[i];
anchor.pos.z = heights[i];
Pozyx.addDevice(anchor);
}
}
// print coordinates to the serial monitor
void print_coordinates(coordinates_t coor, pos_error_t pos_error)
{
Serial.print("Pos x:");
Serial.print(coor.x);
print_tab();
Serial.print("y:");
Serial.print(coor.y);
print_tab();
Serial.print("z:");
Serial.print(coor.z);
Serial.print(" err x:");
Serial.print(pos_error.x);
Serial.print(" y:");
Serial.print(pos_error.y);
Serial.println();
}
// print out the anchor coordinates
void print_anchor_coordinates()
{
uint8_t list_size;
int status;
status = Pozyx.getDeviceListSize(&list_size);
Serial.print("list: ");
Serial.println(status*list_size);
// print error if no anchors are setup
if (list_size == 0) {
Serial.println("No Anchors");
Serial.println(Pozyx.getSystemError());
return;
}
// retrieve anchor information
uint16_t device_ids[list_size];
status &= Pozyx.getDeviceIds(device_ids,list_size);
Serial.print("Anchors found: ");
Serial.println(list_size);
coordinates_t anchor_coor;
for (int i=0; i<list_size; i++) {
Serial.print("A0x");
Serial.print(device_ids[i], HEX);
print_comma();
status = Pozyx.getDeviceCoordinates(device_ids[i], &anchor_coor);
Serial.print(anchor_coor.x);
print_comma();
Serial.print(anchor_coor.y);
print_comma();
Serial.println(anchor_coor.z);
}
}
// get ranges for each anchor
void get_ranges()
{
// get range for each anchor
device_range_t range;
bool success = false;
for (uint8_t i=0; i<NUM_ANCHORS; i++) {
if (Pozyx.doRanging(anchor_id[i], &range) == POZYX_SUCCESS) {
// send info to ardupilot
send_beacon_distance(i, range.distance);
success = true;
}
}
// display errors
if (!success) {
Serial.println("failed to get any ranges");
}
}
// get position of tag
void get_position()
{
coordinates_t position;
pos_error_t pos_error;
//if (Pozyx.doPositioning(&position, POZYX_2_5D, 0) == POZYX_SUCCESS) {
if (Pozyx.doPositioning(&position, POZYX_3D, 0, 0x00) == POZYX_SUCCESS) {
if (Pozyx.getPositionError(&pos_error) == POZYX_SUCCESS) {
// display position
print_coordinates(position, pos_error);
// send to ardupilot
send_vehicle_position(position, pos_error);
}
} else {
// display errors
Serial.println("failed to calc position");
}
}
// send all beacon config to ardupilot
void send_beacon_config()
{
beacon_config_msg msg;
msg.info.beacon_count = NUM_ANCHORS;
for (uint8_t i=0; i<NUM_ANCHORS; i++) {
msg.info.beacon_id = i;
msg.info.x = anchors_x[i];
msg.info.y = anchors_y[i];
msg.info.z = heights[i];
send_message(MSGID_BEACON_CONFIG, sizeof(msg.buf), msg.buf);
}
Serial.println("Sent anchor info");
}
// send a beacon's distance to ardupilot
void send_beacon_distance(uint8_t beacon_id, uint32_t distance_mm)
{
beacon_distance_msg msg;
msg.info.beacon_id = beacon_id;
msg.info.distance = distance_mm;
send_message(MSGID_BEACON_DIST, sizeof(msg.buf), msg.buf);
}
// send vehicle's position to ardupilot
void send_vehicle_position(coordinates_t& position, pos_error_t& pos_error)
{
vehicle_position_msg msg;
// sanity check position
if (position.x == 0 || position.y == 0) {
return;
}
msg.info.x = position.x;
msg.info.y = position.y;
//msg.info.z = position.z;
msg.info.z = 0;
msg.info.position_error = pos_error.xy;
send_message(MSGID_POSITION, sizeof(msg.buf), msg.buf);
}
void send_message(uint8_t msg_id, uint8_t data_len, uint8_t data_buf[])
{
// sanity check
if (data_len == 0) {
return;
}
// message is buffer length + 1 (for checksum)
uint8_t msg_len = data_len+1;
// calculate checksum and place in last element of array
uint8_t checksum = 0;
checksum ^= msg_id;
checksum ^= msg_len;
for (uint8_t i=0; i<data_len; i++) {
checksum = checksum ^ data_buf[i];
}
// send message
int16_t num_sent = 0;
num_sent += fcboardSerial.write(MSG_HEADER);
num_sent += fcboardSerial.write(msg_id);
num_sent += fcboardSerial.write(msg_len);
num_sent += fcboardSerial.write(data_buf, data_len);
num_sent += fcboardSerial.write(&checksum, 1);
fcboardSerial.flush();
}