zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Pozyx: update uno sketch to work with AP_Beacon

master
Randy Mackay 8 years ago
parent
b2a1e0cdce
commit
cff13e4672
1 changed files with 381 additions and 243 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 560
      Tools/Pozyx/IndoorLoiter/IndoorLoiter.ino

560
Tools/Pozyx/IndoorLoiter/IndoorLoiter.ino
Unescape View File

@ -1,49 +1,67 @@ @@ -1,49 +1,67 @@
#include <Time.h>
#include <TimeLib.h>
#include <Pozyx.h>
#include <Pozyx_definitions.h>
/*//#include <mavlink.h>*/
#include "C:\Users\rmackay9\Documents\GitHub\ardupilot\Build.ArduCopter\libraries\GCS_MAVLink\include\mavlink\v2.0\common\mavlink.h"
#include <SoftwareSerial.h>
#include <Wire.h>
/*#include <Time.h> // download from https://github.com/PaulStoffregen/Time */
//#include "C:\Users\rmackay9\Documents\GitHub\Time\Time.h"
////////////////// Pozyx Prams //////////////////////////////
#define CONFIG_TX_GAIN 33.5f
#define NUM_ANCHORS 4
// the network id of the anchors: change these to the network ids of your anchors.
uint16_t anchors[4] = { 0x601C, // (0,0)
uint16_t anchor_id[4] = { 0x601C, // (0,0)
0x6020, // x-axis
0x6057, // y-axis
0x605E};
// only required for manual anchor calibration.
// Please change this to the coordinates measured for the anchors
int32_t anchors_x[NUM_ANCHORS] = {0, 18600, 0, 18600}; // anchor x-coorindates in mm (horizontal)
int32_t anchors_x[NUM_ANCHORS] = {0, 10000, 0, 10000}; // anchor x-coorindates in mm (horizontal)
int32_t anchors_y[NUM_ANCHORS] = {0, 0, 10000, 10000}; // anchor y-coordinates in mm (vertical)
int32_t heights[NUM_ANCHORS] = {1420, 0, 0, 1450}; // anchor z-coordinates in mm
////////////////// MAVLINK Prams //////////////////////////////
#define LATITUDE_BASE (36.324187 * 1.0e7)
#define LONGITUDE_BASE (138.639212 * 1.0e7)
uint8_t buf[MAVLINK_MSG_ID_GPS_INPUT_LEN];
int32_t latitude = 0;
int32_t longitude = 0;
int32_t heights[NUM_ANCHORS] = {-1200, -1200, -1200, -1200}; // 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(57600);
fcboardSerial.begin(115200);
if (Pozyx.begin() == POZYX_FAILURE) {
Serial.println(("ERR: shield"));
@ -56,22 +74,18 @@ void setup() @@ -56,22 +74,18 @@ void setup()
// clear all previous devices in the device list
Pozyx.clearDevices();
//int status = Pozyx.doAnchorCalibration(POZYX_2_5D, 10, num_anchors, anchors, heights);
/*int status = Pozyx.doAnchorCalibration(POZYX_2D, 10, num_anchors, anchors, heights);
if (status != POZYX_SUCCESS) {
Serial.println(status);
Serial.println(("ERROR: calibration"));
Serial.println(("Reset required"));
delay(100);
abort();
}*/
// configure beacons
while (!configure_beacons()) {
delay(1000);
}
// 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();
printCalibrationResult();
print_anchor_coordinates();
Serial.println(("Waiting.."));
delay(5000);
@ -80,41 +94,62 @@ void setup() @@ -80,41 +94,62 @@ void setup()
void loop()
{
coordinates_t position;
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();
}
int status = Pozyx.doPositioning(&position, POZYX_2_5D, 1000);
//int status = Pozyx.doPositioning(&position);
// 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");
}
}
if (status == POZYX_SUCCESS) {
// print out the result
printCoordinates(position);
// slow down counter
static uint8_t counter = 0;
counter++;
if (counter >= 20) {
counter = 0;
}
// send GPS MAVLINK message
SendGPSMAVLinkMessage(position);
} else {
Serial.println("fail");
// 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++;
}
// function to print the coordinates to the serial monitor
void printCoordinates(coordinates_t coor)
uint32_t time_start_ms;
void timer_start()
{
Serial.print("x:");
Serial.print(coor.x);
print_tab();
Serial.print("y:");
Serial.print(coor.y);
print_tab();
Serial.print("z:");
Serial.print(coor.z);
print_tab();
Serial.print("lat:");
Serial.print(latitude);
print_tab();
Serial.print("lng:");
Serial.print(longitude);
print_tab();
Serial.println();
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()
@ -127,40 +162,169 @@ void print_tab() @@ -127,40 +162,169 @@ void print_tab()
Serial.print("\t");
}
#define LOCATION_SCALING_FACTOR_INV_MM 0.08983204953368922f
#define DEG_TO_RAD (M_PI / 180.0f)
// 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;
}
float longitude_scale(uint32_t lat)
// performs repeated calls to get reliable distance between devices
bool get_remote_range(uint16_t dev1, uint16_t dev2, int32_t& range)
{
static int32_t last_lat = 0;
static float scale = 1.0;
if (labs(last_lat - lat) < 100000) {
// we are within 0.01 degrees (about 1km) of the
// previous latitude. We can avoid the cos() and return
// the same scale factor.
return scale;
// 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++;
}
scale = cosf(lat * 1.0e-7f * DEG_TO_RAD);
if (scale < 0.01f) {
scale = 0.01f;
if (Pozyx.doRemoteRanging(dev2, dev1, &dev_range) == POZYX_SUCCESS) {
range_tot += dev_range.distance;
count++;
}
if (scale > 1.0f) {
scale = 1.0f;
}
last_lat = lat;
return scale;
// 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 location_offset(int32_t &lat, int32_t &lng, int32_t offset_north_mm, int32_t offset_east_mm)
void set_beacon_position(uint8_t index, int32_t x_mm, int32_t y_mm, int32_t z_mm)
{
int32_t dlat = offset_north_mm * LOCATION_SCALING_FACTOR_INV_MM;
int32_t dlng = (offset_east_mm * LOCATION_SCALING_FACTOR_INV_MM) / longitude_scale(lat);
lat += dlat;
lng += dlng;
anchors_x[index] = x_mm;
anchors_y[index] = y_mm;
heights[index] = z_mm;
}
// print out the anchor coordinates (also required for the processing sketch)
void printCalibrationResult()
// 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;
@ -169,17 +333,18 @@ void printCalibrationResult() @@ -169,17 +333,18 @@ void printCalibrationResult()
Serial.print("list: ");
Serial.println(status*list_size);
// print error if no anchors are setup
if (list_size == 0) {
Serial.println("Cal fail.");
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.println(("Cal:"));
Serial.print(("Anchors found: "));
Serial.print("Anchors found: ");
Serial.println(list_size);
coordinates_t anchor_coor;
@ -194,143 +359,116 @@ void printCalibrationResult() @@ -194,143 +359,116 @@ void printCalibrationResult()
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;
}
}
// function to manually set the anchor coordinates
void SetAnchorsManual()
// display errors
if (!success) {
Serial.println("failed to get any ranges");
}
}
// get position of tag
void get_position()
{
int i=0;
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");
}
}
for (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);
// 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");
}
// GPS MAVLink message using Pozyx potision
void SendGPSMAVLinkMessage(coordinates_t position)
// send a beacon's distance to ardupilot
void send_beacon_distance(uint8_t beacon_id, uint32_t distance_mm)
{
// Initialize the required buffers
mavlink_message_t msg;
/**
* @brief Pack a gps_input message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param time_usec Timestamp (micros since boot or Unix epoch)
* @param gps_id ID of the GPS for multiple GPS inputs
* @param ignore_flags Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum). All other fields must be provided.
* @param time_week_ms GPS time (milliseconds from start of GPS week)
* @param time_week GPS week number
* @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
* @param lat Latitude (WGS84), in degrees * 1E7
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (AMSL, not WGS84), in m (positive for up)
* @param hdop GPS HDOP horizontal dilution of position in m
* @param vdop GPS VDOP vertical dilution of position in m
* @param vn GPS velocity in m/s in NORTH direction in earth-fixed NED frame
* @param ve GPS velocity in m/s in EAST direction in earth-fixed NED frame
* @param vd GPS velocity in m/s in DOWN direction in earth-fixed NED frame
* @param speed_accuracy GPS speed accuracy in m/s
* @param horiz_accuracy GPS horizontal accuracy in m
* @param vert_accuracy GPS vertical accuracy in m
* @param satellites_visible Number of satellites visible.
* @return length of the message in bytes (excluding serial stream start sign)
*/
uint16_t ignore_flags = GPS_INPUT_IGNORE_FLAG_VEL_HORIZ|
GPS_INPUT_IGNORE_FLAG_VEL_VERT|
GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY|
GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY|
GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY;
uint32_t time_week_ms = 0;
uint16_t time_week = 0;
make_gps_time(time_week_ms, time_week);
// adjust position
latitude = LATITUDE_BASE;
longitude = LONGITUDE_BASE;
location_offset(latitude, longitude, position.y, position.x);
uint16_t len = mavlink_msg_gps_input_pack(
1,
0,
&msg,
micros(), // time_usec,
0, // gps_id,
ignore_flags,
time_week_ms, // time_week_ms,
time_week, // time_week,
3, // fix_type,
latitude, // latitude,
longitude, // longitude,
10, // altitude,
1.0f, // hdop,
1.0f, // vdop,
0.0f, // vn
0.0f, // ve
0.0f, // vd
0.0f, // speed_accuracy
0.0f, // horiz_accuracy
0.0f, // vert_accuracy,
14 // satellites_visible
);
// Copy the message to send buffer
len = mavlink_msg_to_send_buffer(buf, &msg);
// Send message
fcboardSerial.write(buf, len);
}
// calculate GPS time
// based ardupilot/libraries/AP_GPS/GPS_Backend.cpp
void make_gps_time(uint32_t &time_week_ms, uint16_t &time_week)
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)
{
uint8_t year, mon, day, hour, min, sec;
uint16_t msec;
time_t now_time = now();
year = ::year(now_time);
mon = ::month(now_time);
day = ::day(now_time);
uint32_t v = millis();
msec = v % 1000; v /= 1000;
sec = v % 100; v /= 100;
min = v % 100; v /= 100;
hour = v % 100; v /= 100;
int8_t rmon = mon - 2;
if (0 >= rmon) {
rmon += 12;
year -= 1;
}
// get time in seconds since unix epoch
uint32_t ret = (year/4) - 15 + 367*rmon/12 + day;
ret += year*365 + 10501;
ret = ret*24 + hour;
ret = ret*60 + min;
ret = ret*60 + sec;
// convert to time since GPS epoch
ret -= 272764785UL;
// get GPS week and time
time_week = ret / (7*86400UL);
time_week_ms = (ret % (7*86400UL)) * 1000;
time_week_ms += msec;
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();
}

Write Preview
Loading…
Cancel
Save