Embedded Vicon UDP Receiver [Arduino/C++]

Using a WiFi-enabled microcontroller (Particle Photon) to receive and parse the Vicon UDP on a remote vehicle

Code authors: Zach Olkin (an undergrad researcher from ECE) and Lee Whitcher.

Overview

Many applications require the use of Vicon position and orientation information in real time onboard the remote vehicle, such as trajectory control and synchronized logging. With the need to be onboard a flying vehicle, the requirement to be lightweight and low profile in doing this is critical. Furthermore, many aircraft do not provide a feasible/convenient means of injecting Vicon position information into their flight control system, which is particularly the case for proprietary aircraft like those made by DJI. This project was created to address both of these tasks, and achieves these goals using two repos as follows:

  1. Photon Vicon Raw Data - A Particle Photon WiFi-enabled microcontroller is used to connect to the IFL network, access the Vicon UDP stream, and directly parse it into {x,y,z} position and {roll,pitch,yaw} orientation values. Since the Photon weighs only 5g, operates on 3.6-5.5V @ 100mA, and has an external antenna interface for enabling the best WiFi coverage, it is perfect for this application.

    • Direct repo link here

  2. Photon Vicon GPS Spoofer - this repo is an extension of the former whereby the raw data stream (a flat earth reference frame) is mapped onto a latitude/longitude/altitude (geodetic) reference frame centered on the IFL's real life position on Earth. It is then encoded in either NMEA or UBX format (user-selectable) and sent out over a serial port configured as per almost all off-the-shelf GPS (strictly-speaking, "GNSS") receivers. This has the effect of creating a device that tricks an existing flight control systems into thinking it is communicating with a standard GPS receiver. This is an extremely powerful tool; it enables indoor flight where a solid GPS signal cannot be obtained, it eliminates the need to modify source code which can be extremely inconvenient or indeed impossible in the case of proprietary systems, and it facilitates quick configuration changes in a vehicle that sometimes flies outdoors and sometimes flies in the IFL. One simply needs to unplug the GPS receiver and plug in the Photon in its place. The repo also includes the ability to vary position update rate, accuracy, and variance, for testing aircraft control robustness in outdoor flight conditions.

    • Direct repo link here

Photon Vicon Raw Data

The VICON cameras transmit (x, y, z) location information as well as rotation information about each of those axis. If you would like this information instead of the GPS formatted information, you can use the code at this IFL Codebase Github repo.

The information is sent out over the Serial (USB) and Serial1 port on the photon (which can be found here) at 9600 and 115200 baud respectively. The baud rate is only adjustable by changing the source code.

The data is printed in the following format.

Serial.printf("%f, %f, %f, %f, %f, %f, %d\n", x, y, z, xRot, yRot, zRot, udpPerSec);

x, y, and z are all in meters. The rotation angles are in radians. The last number gives the number of UDP packets processed per second. If the loop rate of the code is set to 100 Hz then the updPerSec field should be ~100.

The loop rate is adjustable in the source code with the variable dt

Photon Vicon GPS Spoofer

The GPS Emulator is designed to provide you with real-time data in a GPS friendly format. The photon has been programmed to have the following functions:

  • Send UBX formatted messages

  • Send NMEA formatted messages

  • Change configuration parameters in real time over a USB serial connection

  • Add normally distributed noise, with adjustable variance, to the VICON data

The GitHub repo can be found here.

The information is sent out over the Serial1 port on the photon (which can be found here) at 115200 baud. The baud rate is only adjustable by changing the source code.

The photon does not respond to messages. If you plug the photon into a flight controller running Ardupilot, it will recognize the photon as a GPS properly.

UBX Messages

The photon can send NAV-PVT and NAV-DOP messages (details of the messages are given in this document). These messages were chosen because they send useful information, and with only these messages the photon can be plugged into a flight controller running Ardupilot and the flight controller will recognize the GPS correctly.

NAV-PVT Messages

The NAV-PVT messages provide position and velocity information.

Time

The time provided in this message is not guaranteed to be accurate. The time provided in the PVT messages is guaranteed to not be accurate to precision beyond the second. The photon does not provide absolute time queries that are more precise than a second. The time messages do report out to the microsecond, and relative to the other messages this will be accurate, but the absolute time is not accurate down to the microsecond. Taking difference measurements down to the microsecond is ok with these messages, but not syncing with external objects. Also, without connecting the photon to the cloud, which cannot happen while receiving VICON data, the day, month, and year are not accurate. The photon normally gets its time stamp from the cloud, but on the local network in the IFL it cannot connect to the cloud.

Flags and Fix Type

The photon will always report a 3D-fix.

The fix status flags will always be gnssFixOk.

The number of satellites is a configurable property. The default number of satellites is 6.

Position Measurements

The position is calculated through use of the GeographicLib for C++. The library has been ported over to the photon for use in this project. After receiving the VICON information, the code converts the (x, y, z) information into latitude, longitude, and height information. The angle of the walls relative to north was measured with a compass to determine the angle offset between the (x, y) coordinate system and the north-east plane. The converted latitude and longitude values are reported in NAV-PVT.

The height above the ellipsoid was measured on campus at Georgia Tech with a GPS then that number was used as the 0 height in the IFL. Thus all measurements in the z direction are added to that to get the UAV's height above the ellipsoid. The same procedure was used to get the height above mean sea level.

The horizonal and vertical accuracy estimates are reported to be 2x the the square root of the add noise variance. Thus if you change the noise variance, the vertical and horizontal accuracies should adjust.

Calculated Values

The velocities in the North, East, and Down directions are calculated as a simple numerical derivative: the difference in position measurements divided by the change in time. A positive down velocity means the vehicle is going down.

The ground speed is calculated by taking the vehicle's total change in distance in the x-y plane and dividing that by the change in time.

The heading of motion gives the angle at which the UAV is traveling by using 

theta = atan2(dy, dx)

Dilution Of Precision (DOP)

The position DOP (pDOP) is a configurable value. The default is 1.0. Changing the pDOP has no effect on any other calculations.

NAV-DOP

The NAV-DOP message contains the following DOP information:

  • Geometric

  • Position

  • Time

  • Vertical

  • Horizontal

  • Northing

  • Easting

The pDOP, vDOP, gDOP, nDOP, and eDOP can all be adjusted through the serial interface. They can only ever be set to the same value unless the code is manually modified. Changing the DOP values has no effect on the rest of the code.

NMEA Messages

While using the NMEA messages, the following messages are sent:

  • GNGGA

  • GNRMC

These messages were picked because of the information they send and that they constitute enough information for Ardupilot to recognize the GPS.

GNGGA

Time

Once again, due to the same limitations as above, the time presented here is not guaranteed to be accurate.

Position

The position is calculated the same for the NAV-PVT and the GNGGA message. Please see above.

Flags and Other Information

The number of satellites is a configurable number. The default is 6.

The age of differential corrections and the station providing those corrections are not used and thus left blank.

GNRMC

Time

See above. Time is not guaranteed to be accurate.

Position

The same position is reported in the GNRMC message as is the GNGGA message.

Calculated Values

The ground speed uses the same ground speed calculation as the UBX messages.

The course over ground uses the same calculations as does the heading of motion for the UBX messages.

Magnetic Values

The magnetic values (variation and E/W indicator) are not used and thus left blank.

Additional Noise

The VICON cameras provide very accurate measurements. If you would prefer for your GPS measurements to be more noisy, then you can add "fake" noise to your data by using the MCv command shown below. The default variance is 0. This noise will be added to every time step and is approximately normal.

Changing Parameters

You can write a message over USB serial to change the device properties. All messages start with "MC" meaning "Make Change" then a lower case letter to signify what to change followed by the number.

  • MCv1.23456 Will Make a Change to the Variance of the artificial noise added to the data. The new variance will be 1.23456.

  • MCd1.23456 Will Make a Change to the dt of the loop. In this example, the new dt would be 1.23456 seconds.

  • MCp440 Will Make a Change to the dilution of Precision. Changes the pDOP, hDOP, vDOP, gDOP, nDOP, and eDOP to all be the same thing. Changing the dilution of precision has no effect on the any other calculations in the code. Should be expressed as 1e2. In this example, the new DOP would be 4.40.

  • MCs12 Will Make a Change to the number of Satellites.

  • MCu Will toggle the message type being sent. If NMEA is currently being sent then UBX will instead be sent. Make sure your flight controller is ready to accept that message type. When working with Arudpilot, the GPS type may need to be manually set.

The vertical and horizontal accuracy will always be reported as 2x the standard deviation of the noise variance described above.

Important Note: The photon stores this information in volatile memory, thus when the photon loses power you will need to re-configure parameters. The easiest way to configure parameters is to power the photon through the autopilot (by plugging in the battery normally) then connect the USB to your computer and type in the serial commands. This way the photon will never lose power.

Configuring The Photon

We need to "claim" the photon before we can use it at all. Although in the future we will not need to have the photon connected to the internet, for the claiming step, we need internet connection.

  1. Plug your photon into your computer over USB. (right now we only need power, so if you have a different way of supplying power, you can use that. USB is suggested).

  2. Before we can do anything, we need to claim our photon. Go here and navigate to step 2A. Follow the steps there.

    1. Select any network that allows you to reach the internet. Getting onto eduroam is not suggested, as the credentials to login are not trivial to overcome. For this, I would suggest using a hotspot or home network.

    2. We only need this connection to claim the photon, we will configure the WiFi credentials for the lab later

Setting Up a Local Development Environment

We need to be able to flash the correct code and re-configure the WiFi credentials, so here we will set up your local machine for this. You need the Command Line Interface (CLI) for future commands. If you prefer to use a graphical interface then you can download the Particle Workbench.

Download the Command Line Interface (CLI)

  1. Follow the instructions here.

    1. This is required to be able to run the command line commands below.

Download the Particle Workbench

  1. To use the Particle Workbench you will need Visual Studio Code (which can be downloaded on Linux, Mac, and Windows). Please download Visual Studio Code

  2. Now go to the extensions search (normally located on the left of the screen) and search for Particle Workbench Core or Photon. Install the Workbench core extension.

  3. Now we can compile and flash code locally!

Connect the Photon to the IFL Network

The commands here have only been tested on an Ubuntu machine.

  1. Make sure the photon is plugged in over USB to your computer.

  2. Go to your terminal and type particle usb list You should see your device listed. If it is not listed make sure your USB connection is secure and the installation of the CLI is correct. Also check that the light on the photon is flashing any color (to show it has power).

  3. Now we need to put the photon into listening mode. Type particle usb listen and you should see the LED on the photon change to flashing blue.

  4. Let’s configure the WiFi settings now. Type particle serial wifi into the CLI and you should be prompted with a list of networks, select the SSID shown on the IFL router and use the associated password.

  5. The light should blink a fast green then eventually a slow green to show that it is connected to the network, but not the WiFi.

  6. Now the photon is connected to the IFL network.

Flash the Code to the Photon

Before we can gather data we need to flash the correct code onto the photon. The photon will run the most recent code that has been flashed on boot.

  • The raw data repo can be found here.

  • The GPS emulator repo can be found here.

Using the Particle Workbench

  1. Get the code locally on your computer and put it into a photon project in the workbench.

  2. To check if it compiles, start by pressing the “check” button in the top right of the screen (the button is only located there while you have the .ino or .cpp file selected).

  3. Now to flash the application, press the “lighting bolt” button in the top right. It should put the device into DFU (device firmware update) mode before it flashes. If that fails then in the CLI you can type particle usb dfu which should put the device into DFU mode manually. Then you can re-try the flash button.

Using the CLI

  1. Navigate to the folder that holds the src folder then run the following commands

  2. particle compile p --saveTo test.bin

  3. particle usb dfuThis line is needed to but the photon into DFU mode so it can be flashed

  4. particle flash --usb test.bin

Configuring UDP

As per the instructions laid out in the "Connecting to the IFL Network" section of this Gitbook, it is essential that the Photon network settings are in line with how Vicon Tracker is set up. This is hard coded in the Photon firmware, with the following parameters requiring configuration:

  • IP address (IPAddress myAddress) = 192.168.10.xxx where xxx is the user-selected Host ID that needs to conform to the constraints laid out in the networking instructions

  • Subnet mask (IPAddress netmask) = 255.255.255.0

  • Default gateway (IPAddress gateway) = 0.0.0.0

  • Router IP (IPAddress router) = 192.168.10.3

  • UDP port (Udp.begin.port()) = Whatever is configured in the Vicon Tracker UDP Object Stream setting (or 801 if the DataStream default is sufficient)

  • UDP Packet Size (int packet_size) = Whatever is configured in the Vicon Tracker UDP Object Stream setting

Troubleshooting

This section holds miscellaneous tips for troubleshooting.

  • If the autopilot software claims to be waiting on a GPS config message, you can change the autopilot to skip GPS configuration by changing GPS_AUTO_CONFIG to 0.

  • If the photon is having a hard time connecting to the network, you can try runningparticle device doctor which should update the device. After running this command you may need to repeat the network connection steps and re-flash the code.

PreviousVicon UDP Parsing [Python]NextBuilding multi-rotor UAVs

Last updated