From dd15087d0f83c6692798f36a882e1872a32f342a Mon Sep 17 00:00:00 2001 From: MatthewT0809 Date: Thu, 16 Jul 2026 16:38:58 +0100 Subject: [PATCH] Updating Vicon Documentation --- .../common-vicon-for-nongps-navigation.rst | 451 +++++++++++------- 1 file changed, 287 insertions(+), 164 deletions(-) diff --git a/common/source/docs/common-vicon-for-nongps-navigation.rst b/common/source/docs/common-vicon-for-nongps-navigation.rst index d612b95ac2f..44a7dd52651 100644 --- a/common/source/docs/common-vicon-for-nongps-navigation.rst +++ b/common/source/docs/common-vicon-for-nongps-navigation.rst @@ -6,248 +6,371 @@ Using a Vicon indoor positioning system ======================================= +Overview +======== Robotics labs commonly have an indoor flying facility using a Vicon indoor positioning system. These systems use infra-red cameras to give -a high rate (200Hz) position and attitude via a network -connection. ArduPilot can use this positioning information for precise -indoor flight. +a high rate (200Hz) position and attitude information via a network +connection. .. youtube:: XMb4MKi2HSQ :width: 100% -Overview -======== The Vicon system comes with a SDK that provides APIs for accessing the -Vicon positioning data. To use the Vicon data with ArduPilot you need -a tool which can use this API and map it onto MAVLink packets to send -to ArduPilot. +Vicon positioning data. To use the Vicon data with ArduPilot, you need +a tool which can use this API and reformats the data into MAVlink format. This can be done by manipulating the MAVlink packet "GPS_INPUT", creating a pseudo GPS source for ArduPilot. -There are two types of MAVLink packets that can be used: +Hardware Setup +============== - - the GLOBAL_VISION_POSITION_ESTIMATE packet, which provides position - (NED) and attitude (euler angles). +The following hardware is used in this guide: - - the GPS_INPUT packet which provides position as latitude/longitude, - altitude, velocity (3D) and yaw data. + - Vicon camera system with a Host PC + - Vicon Calibration Wand + - Linux laptop (GCS) with both Wi-Fi and Ethernet capability + - Pixhawk6C flight controller + - ESP8266 Wi-Fi module + - Conventional Radio controller and receiver + - Minimum of 4 reflective IR markers per drone + - Ethernet cables + - Drone -Right now the best approach is to use GPS_INPUT with the GPS yaw -extension as the velocity data provided by the GPS packet is more -valuable for good positioning than the attitude information in the -GLOBAL_VISION_POSITION_ESTIMATE packet. In the future we will support -a combined VISION MAVLink message which will provide position, -velocity and attitude in one packet. +System Overview +=============== +A Ground Control Station (GCS) in the form of a Linux laptop receives this positional data by listening to the broadcast from Vicon's Host PC, reformats the positional data into MAVlink's GPS_INPUT format, and sends it wirelessly over a Wi-Fi module to a flight controller. -Hardware Setup -============== -You will need a low-latency network link from your GCS computer to the -copter, and from the GCS to your Vicon server. The recommended method -is to use ethernet to the Vicon server and use a ESP8266 WiFi link -running :ref:`mavest8266 ` on the copter. +GCS Setup +========= +Download the following on the GCS laptop: + + - **Vicon DataStream SDK 1.10.0** - Select *Tracker*. (`Vicon `_) + - **pyvicon** — a Python3 wrapper by MathGaron (`GitHub `_) -Vicon System Setup -================== +Set Up a Conda Virtual Environment (Optional) +--------------------------------------------- +.. code:: bash -- The Vicon system should be correctly calibrated. The origin should be set using the calibration wand. In the Vicon Tracker software, the Z-axis (shown in blue) of the Vicon system world frame should point up. We will call the X-axis (red) as North, and the Y-axis (green) as West. -- The UAV should have a minimum of 4 reflective markers to ensure an accurate fix. Any reflective surfaces on the UAV should be covered with masking tape to avoid spurious detections. -- Place the UAV in the Vicon space so that the forward direction of the autopilot is facing 'North'. (Figure 1) -- In the Vicon Tracker, select all of the markers and create a new object for the UAV. The coordinate frame of the UAV should then be aligned with the Vicon world frame, so that X is forward, Y is Left and Z is up. (Figure 2) + conda create -n VICON python=3.10 + conda activate VICON -.. figure:: ../../../images/vicon-frame-reference.jpg - :target: ../_images/vicon-frame-reference.jpg - - Figure 1: Example of how UAV should be positioned when creating the Vicon Object. The forward direction is aligned with the X-axis. -.. figure:: ../../../images/vicon-alignment.png - :target: ../_images/vicon-alignment.png +Pyvicon installation +-------------------- +Within the downloaded Vicon SDK, move shared library .so files into an accessible location for your OS (In my case, lib inside my "Vicon" venv). Move .cpp and .h files into pyvicon-master/pyvicon folder as well. Below is an example of how to do this: - Figure 2: Screenshot from Vicon Tracker software, showing world coordinate frame (bottom left) aligned with UAV coordinate frame. The origin of the object frame should be approximately where the autopilot is located. +.. code:: bash + # Move .so shared libraries into the venv lib folder + find ~/Downloads/ViconDataStreamSDK_1.10.0.123216h -name "*.so" \ + -exec mv {} ~/miniconda3/envs/VICON/lib/ \; -.. warning:: + # Move .cpp source files into pyvicon + find ~/Downloads/ViconDataStreamSDK_1.10.0.123216h -name "*.cpp" \ + -exec mv {} ~/Downloads/pyvicon-master/pyvicon/ \; - These coordinate frame conventions apply to MAVProxy version 1.8.36 and above. Ensure you are using an up-to-date version. + # Move .h header files into pyvicon + find ~/Downloads/ViconDataStreamSDK_1.10.0.123216h -name "*.h" \ + -exec mv {} ~/Downloads/pyvicon-master/pyvicon/ \; -Software Setup -============== +Install the following packages/dependencies: -You will need a recent version of ArduPilot on your copter. As of -September 2019 it is recommended that you use ArduPilot master. The -recent version is needed to give support for yaw data in the GPS_INPUT -MAVLink message. +.. code:: bash -The setup described below uses the vicon module in MAVProxy to feed -the Vicon data to your vehicle over MAVLink. + sudo apt-get install libgtk-3-dev + sudo apt remove modemmanager -y + python3 -m pip install numpy==2.2.6 + python3 -m pip install pathlib2==2.3.7.post1 + python3 -m pip install matplotlib==3.10.9 + python3 -m pip install opencv-python==4.13.0.92 + python3 -m pip install Cython==3.2.4 + python3 -m pip install future==1.0.0 + python3 -m pip install wxpython==4.2.5 -pyvicon -======= +.. note:: -You will need to install the `pyvicon -`_ python package, using a python -version of at least 3.6. To install pyvicon you will need to also -install the Vicon SDK, version 1.10 or later, from `ViconDataStream -SDK `_ + ``wxpython`` can take up to 20 minutes to install. If the terminal + appears to stall at that package, it is still working — do not + interrupt the process. -Note that pyvicon does not build with python2. -Once you have the SDK setup, you should install pyvicon like this: +From inside the ``pyvicon-master`` directory, build and install pyvicon: .. code:: bash - python3 setup.py build install --user + cd ~/Downloads/pyvicon-master + python3 setup.py install -You will also need to add $HOME/.local/bin to your $PATH. -pymavlink -========= +Network Setup +------------- -You will need a pymavlink version of at least 2.3.8. Install with: +Find a means to connect the Vicon Host PC to your GCS laptop (I am using CAT6 ethernet for lowest latency) and rename its IP address as "vicon" in the hosts file. For example: .. code:: bash - python3 -m pip install --upgrade --user pymavlink + sudo nano /etc/hosts -MAVProxy Setup -============== +.. code:: bash + + # Example IP address + 192.168.0.218 vicon + +Press ``Ctrl+O`` and ``Enter`` to save, then ``Ctrl+X`` to exit. -You will need MAVProxy version of at least 1.8.36. Install with: +Verify connectivity: .. code:: bash - python3 -m pip install --upgrade --user mavproxy + ping vicon -You will also need some other python3 libraries, install with: +A flow of ``64 bytes from vicon (192.168.0.218)`` messages confirms a +successful connection between your GCS and the Vicon Tracker. -.. code:: bash +Drone Setup +=========== + +Connect the Pixhawk6C to your computer and navigate to **Config → Full Parameter List**. Using the search box, set the following parameters: + +.. list-table:: + :header-rows: 1 + :widths: 40 20 + + * - Parameter + - Value + * - AHRS_EKF_TYPE + - 3 + * - EK3_SRC1_POSXY + - 3 + * - EK3_SRC1_POSZ + - 3 + * - EK3_SRC1_VELXY + - 3 + * - EK3_SRC1_VELZ + - 3 + * - EK3_SRC1_YAW + - 2 + * - EK3_SRC2_POSXY (Same for SRC2's VELXY, POSZ, VELZ, YAW) + - 0 + * - EK3_SRC3_POSXY (Same for SRC3's VELXY, POSZ, VELZ, YAW) + - 0 + * - EK3_MAG_CAL + - 5 + * - EK3_POSNE_M_NSE + - 0.002 + * - GPS1_TYPE + - 14 + * - GPS1_DELAY_MS + - 50 + * - COMPASS_USE + - 0 + * - COMPASS_USE2 + - 0 + * - COMPASS_USE3 + - 0 + +These parameters configure the Extended Kalman Filter (EKF3) and GPS +subsystem to accept Vicon data as the main position source and disable +compass reliance. + +.. note:: + An empirical GPS1 delay of 50ms was found to work best with the Vicon system. If using a different network setup, you may need to adjust this value depending on your network latency. + + For remote connection between GCS and the flight controller, I used an ESP8266 Wi-Fi module connected to a Pixhawk6C via the telemetry port. Update the SERIAL1_BAUD parameter to 921600 and SERIAL1_OPTIONS to 2 for this setup. If using a different telemetry module, you may need to adjust this value. + + +IR Marker Placement +------------------- +Mount at least 4 IR markers on the drone using hot glue, positioning +them where they are clearly visible from the camera's perspective. Use an +**asymmetrical arrangement**. + +.. note:: + If flying multiple drones simultaneously, + each drone must have a geometrically distinct marker pattern to prevent + the Vicon system from misidentifying objects. + + +Vicon Setup +=========== +Switch on the Vicon Positioning System and also the Vicon Host PC, allow the system to warm up (~20 minutes). + + +Creating a Camera Mask +---------------------- + +Navigate to the **CALIBRATE** tab. Under *CREATE CAMERA MASK*, click +**START**. This takes a snapshot of the environment and filters out +any ambient IR reflections. + +Calibrating the Cameras +------------------------ + +Under **CALIBRATE CAMERAS**, click **START**. The computer will +verbally confirm ``"Calibration started"``. + +Prepare the Vicon Calibration Wand and switch on its LEDs. Enter the Vicon environment and sweep the wand in a +cyclical arc facing the cameras, covering all camera positions in the +arena. Camera calibration progress is indicated by LED colour change: + + - Small cameras: red/pink → green when calibrated + - Large cameras: red → green, with an on-screen progress bar + +Continue sweeping until all cameras turn from static green to static +red, indicating complete calibration. + +Return to the PC and check the **CAMERA CALIBRATION FEEDBACK** panel. +Once the progress bar completes, inspect the **World Error** and +**Image Error** values for all cameras. + +.. note:: + Any orange or red tiles in the table indicate a poor calibration — repeat the physical sweep in that case. + + Emphasize your sweep in areas your drone is expected to fly. (e.g. if you are flying in the corner, do few more sweeps in that corner). - sudo apt-get install libgtk-3-dev - python3 -m pip install --user pathlib2 wxpython matplotlib +Setting the Origin +------------------ -Next you should put the IP address of your vicon server in your -/etc/hosts file, so that you can do "ping vicon". The name 'vicon' is -used as the default hostname in the MAVProxy vicon module. +From within your Vicon environment, place the wand flat on the ground at your chosen +origin location. -Connecting -========== +The direction convention for the Calibration Wand is usually known as: **The bottom of the handle points towards West, the left handle points towards North.** Align the wand as orthogonally as possible with your Vicon environment. -Setup your copter with the WiFi link running at baudrate 921600 and -with MAVLink2 enabled. If your WiFi adapter is on Telem1, then you -will need to set: +.. warning:: + + A poorly oriented origin will result in a skewed inertial coordinate + frame, which will cause problems for any path-planning algorithms. + +Leave the wand on the ground, return to the PC, and navigate to **SET +VOLUME ORIGIN**. Click **START**. The computer will verbally confirm +``"Origin set"``. + +Defining New Drone Objects +-------------------------- + +The Vicon system identifies objects by recognising the geometric +arrangement of IR markers on a drone. To define a new object: + +1. Place the drone inside the Vicon environment. + +2. Align Autopilot's forward direction with the origin's North axis. + +3. On the PC, click the **OBJECT** tab. + +4. In the viewport (switch to **3D ORTHOGONAL to -Z** if markers are + not visible), use Alt+click to drag-select your IR markers. The + selected markers will show which cameras are tracking them. - - :ref:`SERIAL1_PROTOCOL ` = 2 - - :ref:`SERIAL1_BAUD ` = 921600 +5. At the bottom left of the screen, type a name for your object and + click **CREATE**, then **SHARED**. -Next start MAVProxy like this: +6. Locate your newly created object in the object list and save it. + +7. Select your object in the list and ensure no other objects are selected. + +.. warning:: + + Ensure your Autopilot's North is aligned with Vicon's North when declaring a new object, otherwise you will have a mismatch between body and inertial frame. + +GCS integration +------------------------- + +If you haven't installed MAVProxy and pymavlink already: .. code:: bash - mavproxy.py --master :14550 --aircraft MyQuad --console --map + python3 -m pip install pymavlink==2.4.49 + python3 -m pip install mavproxy==1.8.74 -For more details see the :ref:`MAVProxy documentation ` +You must tell MAVproxy which Vicon object to track. Navigate to the MAVProxy's module folder and find `mavproxy_vicon.py`, for example: -Key Parameters -============== +.. code:: bash -You should set the following key parameters: - - - :ref:`EK3_ENABLE ` = 1 - - :ref:`EK2_ENABLE ` = 0 - - :ref:`AHRS_EKF_TYPE ` = 3 - - :ref:`EK3_SRC1_POSXY ` = 3 - - :ref:`EK3_SRC1_POSZ ` = 3 - - :ref:`EK3_SRC1_VELXY ` = 3 - - :ref:`EK3_SRC1_VELZ ` = 3 - - :ref:`EK3_SRC1_YAW ` = 2 - - :ref:`GPS1_TYPE ` = 14 - - :ref:`GPS1_DELAY_MS ` = 50 - - :ref:`COMPASS_USE ` = 0 - - :ref:`COMPASS_USE2 ` = 0 - - :ref:`COMPASS_USE3 ` = 0 - -After setting these parameters you should reboot your copter. - -Starting Vicon -============== + ~/miniconda3/envs/VICON/lib/python3.10/site-packages/MAVProxy/ -Inside MAVProxy load the vicon module with: +Open ``mavproxy_vicon.py`` and edit lines 29–36: + + - Set ``origin_lat``, ``origin_lon``, and ``origin_alt`` to ``0``. + - Set ``object_name`` to the name of your drone object as defined in + the Vicon Tracker (e.g. ``[TroglodytesDrone]``). + +Save the file. + +Ensure: + - The GCS is connected to the Vicon Host PC. + - The GCS is connected to the flight controller via Wi-Fi module/your means. + - The drone is placed inside the Vicon environment and tracked by the Vicon system. + +Launch MAVProxy: .. code:: bash - module load vicon + mavproxy.py --master : --console --map -If it doesn't load correctly then enable debugging with "set moddebug -3" then try to load the module again. +Within the MAVProxy prompt, load and start the Vicon module: -Once loaded you set set vicon parameters with "vicon set". Once you -are happy with the parameters then do "vicon start" +.. code:: bash -The MAVProxy console will then give status information for the Vicon, -showing position, attitude and messages rates. The vehicle should also -get GPS lock. It will show up at the GPS coordinates in the vicon -module settings. + module load vicon + vicon set + vicon start -Checking Orientations -===================== +A successful connection will print: -Use the Vicon status line in the MAVProxy console to check that you -have the right orientation in the data coming from the Vicon. The VPos -data is in NED format in meters. The VATT data is euler angles of -roll, pitch yaw. Carefully check that these values are consistent -while you rotate and move the vehicle before proceeding. +.. code:: bash -Test Flight -=========== + Connected to subject "" segment "" + +The MAVProxy console should now display live position and Euler angles in North-East-Down and Roll-Pitch-Yaw coordinate system. -You should see ArduPilot first report that EKF3 gets yaw alignment, -and then that it is using the GPS. After that you should be able to -arm and fly. +.. warning:: -For a first test flight :ref:`Stabilize mode ` or :ref:`Alt Hold mode ` mode is -recommended. Then get the flight log from the microSD card and check -that the EKF3 innovations are low. The position, velocity and yaw -normalised innovations should all be low (below 0.1). + Vicon utilizes the NEU (North-East-Up) coordinate system, while ArduPilot uses NED (North-East-Down) coordinate system. Pyvicon automatically converts the coordinates from NEU to NED, but for your own custom integration, you must ensure this conversion is handled correctly (To fly 1m high in MAVlink format, this is -1, not 1). -Once you have confirmed low innovations you can try a flight in :ref:`Loiter mode ` -mode. +.. note:: -Tuning -====== + If the console reports a height of approximately -12cm while the + drone is on the ground, this is expected — it reflects the offset + between the drone's base and the geometric centre of the IR marker + cluster. -If you have significant vibration then you will likely want to -de-weight the accelerometers and instead tell the EKF to use the vicon -data more. Try setting :ref:`EK3_ACC_P_NSE ` to 2.0 to de-weight the -accelerometer data. You may find this helps with position and height -hold. +Checking Orientation +-------------------- -For indoor flight with a Vicon setup and a small quad you can push up -the accelerations and yaw rate to much higher value than are used in a -normal quad. Some suggestions for a small racing quad are: +Monitor the Vicon status output in the MAVProxy console, carefully verify these values are consistent and sensible as you physically rotate and translate the drone before proceeding to flight. - - :ref:`INS_GYRO_FILTER ` = 60 - - :ref:`INS_ACCEL_FILTER ` = 30 - - :ref:`ATC_ACC_Y_MAX ` = 100000 - - :ref:`ATC_RATE_WPY_MAX ` = 15000 +Test Flight +----------- +Set the following flight mode assignments (using a 3-way switch on channel +5 or your preferred channel): -Circle Mode -=========== + - Flight Mode 1: Stabilize + - Flight Mode 4: Alt Hold + - Flight Mode 6: Loiter -A very useful mode for testing accuracy of flight is :ref:`Circle mode -`. To use it indoors try: +Once the EKF3 reports yaw alignment and GPS lock, the drone is ready +to arm. For the first flight, you should take off using Stabilize mode under your manual control. Once the drone is airborne and steady, attempt Alt Hold mode while being fully prepared to switch it back if something goes wrong. Once the drone is stable in Alt Hold using the Vicon coordinates, you can attempt Loiter mode and Guided mode. - - :ref:`CIRCLE_RADIUS_M ` = 100 - - :ref:`CIRCLE_RATE ` = 40 -You should also set your :ref:`RTL_ALT_M ` nice and low in case you hit a -failsafe, or configure a instant disarm switch on your transmitter for -when things go wrong. +.. warning:: + + Exercise great caution when attempting to fly in Alt Hold, Loiter mode for the 1st time. Visually imagine the drone's ability to full throttle into the ceiling or suddenly drops out of the sky as the worst case scenario due to a misconfiguration. The moment you switch the modes you must be capable of switching back to Stabilize mode immediately if the drone is not behaving as expected. + -To fly in :ref:`Circle mode ` take off in :ref:`Loiter mode ` and then move so you are 1m -from the center of the room, pointing towards the middle of the -room. Then switch to Circle mode and the vehicle should start -circling, keeping its nose pointed at the center of the circle. You -can try pushing up :ref:`CIRCLE_RATE ` to higher values (in degrees/second) as -you get more confident. The video at the top of this page has -:ref:`CIRCLE_RATE ` = 150 for a 150 degree/second circle. +Cranfield's Indoor Flight Arena's Approach +------------------------------------------ +It's because configuration differs between the lab environments, this serves as more of a working principle of Vicon integration with Ardupilot. For a more extensive "Dummy" Guide with specific hardware configurations we use in the Cranfield indoor flight arena, it is publicly available at: `Vicon Indoor Positioning System with Pixhawk6C `_. Please use this as a reference for your own setup. + +.. warning:: + + The ESP8266 is reported to have connection issues in swarm flights. + Use a Raspberry Pi Wi-Fi hosting feature for remote communication if conducting multi-drone operations. + + Not all Pixhawk variants behave identically with this configuration — older + Radiolink Pixhawk hardware does not work. + + This guide has not been tested with PX4 firmware. + Do not use Windows Subsystem for Linux (WSL) for the GCS — port communication permissions are difficult to configure correctly.