zero2wfpv_web_logo

Zero2WFpv application

Note: Zero2WFpv application is a software prototype and research work only. The software is not intended for real use and is added to the RapidPixel SDK as bonus additional software without technical support or updates.

v1.2.1

Table of contents

Overview

The Zero2WFpv application implements a video processing pipeline: video capture > tracking > Analog or RTP output streaming, as well as providing auto control for FPV drones. The application combines libraries: VSourceLibCamera (video capture from Libcamera API compatible devices), CvTracker, FormatConverter, VOutputFb, CrsfParser, SerialPort, UdpSocket, RtpPusher, VCodecV4L2 and License. The application shows how to use a combination of the libraries listed above. It is built for Raspberry PI5 and Raspberry PI Zero 2W and has been tested on Raspberry Pi OS (Bookworm x64).

zero2wfpvprinciple

After starting, the application reads a JSON config file that includes video capture parameters, video output parameters, communication parameters, auto control algorithm coefficients, and auto control channel configurations. If there is no config file, the application will create a new one with default parameters. Additionally, the application creates a “Log” folder to write log information.

Versions

Table 1 - Application versions.

Version Release date What’s new
1.0.0 01.02.2025 - First version.
1.0.1 17.02.2025 - Update submodules.
1.0.2 24.02.2025 - Update CvTracker submodule.
1.0.3 18.03.2025 - Update CvTracker submodule.
1.0.4 03.04.2025 - Multiple submodules update.
1.0.5 15.05.2025 - CvTracker submodule update.
1.0.6 22.06.2025 - RtpPusher submodule update.
1.0.7 19.07.2025 - CvTracker submodule update.
1.0.8 27.07.2025 - RtpPusher submodule update.
1.0.9 10.08.2025 - CvTracker submodule update.
1.0.10 17.08.2025 - CvTracker submodule update.
- gitignore file update.
1.0.11 06.10.2025 - RtpPusher submodule update.
1.0.12 25.10.2025 - RtpPusher submodule update.
- SerialPort submodule update.
1.0.13 15.11.2025 - RtpPusher submodule update.
- VCodecV4L2 submodule update.
1.0.14 12.12.2025 - License submodule updated.
1.0.15 27.12.2025 - RtpPusher submodule updated.
1.0.16 17.01.2026 - RtpPusher submodule updated.
- CvTracker submodule updated.
- Parameters initialization changed.
1.0.17 07.02.2026 - RtpPusher submodule updated.
- CvTracker submodule updated.
1.0.18 20.02.2026 - CvTracker submodule updated.
1.0.19 06.03.2026 - RtpPusher submodule updated.
1.0.20 22.03.2026 - RtpPusher submodule updated.
1.0.21 28.03.2026 - CvTracker submodule updated.
- VCodecV4L2 submodule updated.
- Fixed compilation errors with new version of submodules.
1.0.22 14.04.2026 - RtpPusher submodule updated.
1.0.23 25.04.2026 - CvTracker submodule updated.
1.1.0 05.05.2026 - RtpPusher submodule updated.
- UdpSocket submodule added.
1.1.1 05.06.2026 - CvTracker submodule updated.
1.1.2 20.06.2026 - CvTracker submodule updated.
- RtpPusher submodule updated.
1.1.3 13.07.2026 - CvTracker submodule updated.
- RtpPusher submodule updated.
- UdpSocket submodule updated.
1.1.4 26.07.2026 - CvTracker submodule updated.
1.2.0 29.08.2026 - FormatConverterOpenCv submodule replaced by FormatConverter.
- SerialPort submodule updated.
- Fixed PID controller constructor.
- Fixed use of undecoded RC channels.
- Fixed signed/unsigned errors in the auto control channel arithmetic.
- Fixed tracker thread busy loop.
- On screen menu timings are time based instead of loop-iteration based.
- Channel numbers from the config file are validated.
- Comments and documentation corrected.
1.2.1 12.09.2026 - FormatConverter submodule updated.
- License submodule updated. License check always TRUE.

Source code files

The application is provided as source code only. The user is provided with a set of files in the form of a CMake project (repository). The repository structure is shown below:

CMakeLists.txt ------------- Main CMake file.
3rdparty ------------------- Folder with third-party libraries.
    CMakeLists.txt --------- CMake file to include third-party libraries.
    FormatConverter -------- Source code of FormatConverter library.
    VSourceLibCamera ------- Source code of VSourceLibCamera library.
    CvTracker -------------- Source code of CvTracker library.
    VOutputFb -------------- Source code of VOutputFb library.
    License ---------------- Source code of License library.
    CrsfParser ------------- Source code of CrsfParser library.
    SerialPort ------------- Source code of SerialPort library.
    VCodecV4L2 ------------- Source code of VCodecV4L2 library.
    RtpPusher -------------- Source code of RtpPusher library.
    UdpSocket -------------- Source code of UdpSocket library.
src ------------------------ Folder with application source code.
    CMakeLists.txt --------- CMake file.
    Zero2WFpvVersion.h ----- Header file with application version.
    Zero2WFpvVersion.h.in -- File for CMake to generate version header.
    main.cpp --------------- Application source code file.
    utils.h ---------------- Utility functions header file.
    utils.cpp -------------- Utility functions source code file.
    Pid.h ------------------ PID controller header file.
    Pid.cpp ---------------- PID controller source code file.
Compiled ------------------- Prebuilt executables for Raspberry PI.
static --------------------- Images used by this document.

Config file

Zero2WFpv application reads the config file Zero2WFpv.json in the same folder as the application executable file. Config file content:

{
    "Params": 
    {
        "communication": 
        {
            "crsfUdpPort": 5000,
            "isCrsfOverUdp": false,
            "serialPortName": "/dev/ttyAMA0"
        },
        "pidControl": 
        {
            "constantPitch": 1060,
            "isConstantPitchEnabled": true,
            "isPrintThrottle": false,
            "isRollControlEnabled": true,
            "isThrottleControlEnabled": true,
            "isYawControlEnabled": true,
            "rollCenter": 992,
            "rollKd": 0.0,
            "rollKi": 0.0,
            "rollKp": 4.0,
            "rollMax": 1600,
            "rollMin": 300,
            "throttleCenter": 536,
            "throttleKd": 25.0,
            "throttleKi": 0.0,
            "throttleKp": 1.0,
            "throttleMax": 1600,
            "throttleMin": 500,
            "yawCenter": 992,
            "yawKd": 0.0,
            "yawKi": 0.0,
            "yawKp": 1.0,
            "yawMax": 1600,
            "yawMin": 300
        },
        "trackerControl": 
        {
            "armChannel": 8,
            "armChannelTriggerValue": 1000,
            "captureChannel": 9,
            "captureChannelTriggerValue": 1000,
            "pitchChannel": 2,
            "rollChannel": 1,
            "throttleChannel": 0,
            "yawChannel": 3
        },
        "videoOutput": 
        {
            "bandwidthKbps": 1000000,
            "encodingBitrateKpbs": 3000,
            "fps": 30,
            "gopSize": 30,
            "ip": "127.0.0.1",
            "isRtpOutput": false,
            "port": 7032
        },
        "videoSource": 
        {
            "cameraIndex": 0,
            "fps": 30,
            "height": 576,
            "pixelFormat": "YUYV",
            "width": 736
        }
    }
}

Table 2 - Config file parameters description.

Parameter Type Description
Communication:    
crsfUdpPort int CRSF UDP port number. Used only when isCrsfOverUdp is TRUE.
isCrsfOverUdp bool Enable CRSF input over UDP. The serial port is opened in any case: it always carries the CRSF stream to the flight controller.
serialPortName string Serial port name. The port is opened at 420000 baud, 8N1. 420000 is not a standard baudrate, so the SerialPort library programs it through its custom baudrate support (Linux termios2 / BOTHER), which requires a UART driver that accepts arbitrary baudrates.
PID control parameters:    
constantPitch int Constant pitch value. Higher value means higher pitch so drone will move faster towards object.
isConstantPitchEnabled bool Enable constant pitch for auto control.
isPrintThrottle bool Print auto control calculated throttle and raw throttle values.
isRollControlEnabled bool Enable roll control for auto control.
isThrottleControlEnabled bool Enable throttle control for auto control.
isYawControlEnabled bool Enable yaw control for auto control.
rollCenter int Roll center value.
rollKd float Roll Kd value. It is recommended to set this value to 0.0.
rollKi float Roll Ki value. It is recommended to set this value to 0.0.
rollKp float Roll Kp value.
rollMax int Roll max value.
rollMin int Roll min value.
throttleCenter int Throttle center value.
throttleKd float Throttle Kd value.
throttleKi float Throttle Ki value. It is recommended to set this value to 0.0.
throttleKp float Throttle Kp value.
throttleMax int Throttle max value.
throttleMin int Throttle min value. If a diagonal trajectory is desired, this value should be close to throttleCenter. However, if the horizontal distance to the object is not long enough, the drone may pass the object.
yawCenter int Yaw center value.
yawKd float Yaw Kd value. It is recommended to set this value to 0.0.
yawKi float Yaw Ki value. It is recommended to set this value to 0.0.
yawKp float Yaw Kp value.
yawMax int Yaw max value.
yawMin int Yaw min value.
Tracker control parameters:    
armChannel int Arm channel number. Must be inside [0, 15].
armChannelTriggerValue int Arm channel trigger value.
captureChannel int Capture channel number. Must be inside [0, 15].
captureChannelTriggerValue int Capture channel trigger value.
pitchChannel int Pitch channel number. Must be inside [0, 15].
rollChannel int Roll channel number. Must be inside [0, 15].
throttleChannel int Throttle channel number. Must be inside [0, 15].
yawChannel int Yaw channel number. Must be inside [0, 15].
Video output parameters:    
bandwidthKbps int Communication channel bandwidth in Kbps.
encodingBitrateKpbs int H264 encoding bitrate in Kbps. Used only when RTP output is enabled.
fps int H264 encoding frame rate. Used only when RTP output is enabled.
gopSize int H264 group of pictures size. Used only when RTP output is enabled.
ip string Destination IP address for RTP output.
isRtpOutput bool Enable RTP output. If RTP output is enabled, analog output will be disabled.
port int Destination port for RTP output.
Video source parameters:    
cameraIndex int Camera index. If only one camera is connected to the system, it is going to be 0.
fps int FPS of video source.
width int Video source width.
height int Video source height.
pixelFormat string Video source pixel format. (For example : “YUYV” , “NV12”, “BGR24”, “RGB24”)

Note: the application refuses to start if any channel number is outside [0, 15], because the channel numbers are used directly as indexes into the 16 channel CRSF frame. It also refuses to start if any PID center, min or max value or the constant pitch value is outside [0, 1984], the range that fits into an 11 bit CRSF channel.

On screen menu

Zero2WFpv provides an on-screen menu to configure some application parameters without modifying the config file. Access to the menu and parameter configuration is done only through joystick channels.

  1. To open the menu, the joystick position should be held for 5 seconds as follows:
    • Throttle channel should be at minimum value.
    • Yaw channel should be at maximum left position.
  2. To close the menu, the joystick position should be held for 1 second as follows while in the main menu (the parameters are written to the config file at that moment):
    • Throttle channel should be at minimum value.
    • Yaw channel should be at maximum right position.
  3. To navigate the menu:
    • The roll channel should be used to navigate submenus and return to the main menu.
    • The pitch channel should be used to iterate through parameters.
    • The yaw channel should be used to change parameter values.

zero2wfpvmenu

Table 3 - Main menu items.

Item name Description
Throttle PID coefficients for throttle control as well as enable/disable throttle control.
Yaw PID coefficients for yaw control as well as enable/disable yaw control.
Roll PID coefficients for roll control as well as enable/disable roll control.
Pitch Constant pitch value as well as enable/disable constant pitch.
Config Submenu to configure the channel numbers and the throttle overlay.

Table 4 - Controllers menu items.

Item name Description
Center Center value of controller.
Kp Proportional coefficient.
Kd Derivative coefficient.
Ki Integral coefficient.
PID Max Maximum value of controller. Never below PID Min.
PID Min Minimum value of controller. Never above PID Max.
Mode Enable/disable controller.

Note: for the Pitch item only Center (the constant pitch value) and Mode apply. The remaining rows are shown as “-“ because a constant pitch has no PID coefficients or limits.

Table 5 - Config menu items.

Item name Description
Capture Channel Capture channel number.
Arm Channel Arm channel number.
Throttle Channel Throttle channel number.
Roll Channel Roll channel number.
Pitch Channel Pitch channel number.
Yaw Channel Yaw channel number.
Print throttle Print auto control calculated throttle and raw throttle values.

Auto control algorithm

The Zero2WFpv application has 3 independent PID control algorithms to control the FPV drone toward the object being tracked. These PID controllers are for roll, throttle, and yaw control. When auto control mode is enabled, the drone will move toward the object with a constant pitch value. These PID controllers can be independently enabled or disabled, as well as the constant pitch value. Additionally, all parameters related to the controllers can be configured via the config file and on-screen menu.

If auto control is disabled, the Zero2WFpv application re-encodes every received RC channels packet and forwards it to the flight controller unchanged. However, if the auto control algorithm is enabled and the tracker is in tracking mode, the application will calculate new values for roll, throttle, and yaw channels. These calculated CRSF values will be sent to the flight controller via the serial port. The channels the application does not control are forwarded unchanged.

Note: only CRSF RC channels packets are forwarded; telemetry and other CRSF packet types are not relayed. If the RC link goes silent, the application stops writing to the serial port and disarms auto control, so that the flight controller runs its own failsafe.

Additionally, while auto control is on, the operator can give commands to the tracker by using the joystick. Supported commands are:

  • Moving the tracking rectangle by using roll and pitch channels.
  • Changing the tracking rectangle size by using yaw channel.

How to copy Zero2WFpv executable to Raspberry PI

To copy the ready application to Raspberry Pi, follow these steps. Run the commands on your Windows computer where you have the Zero2WFpv file.

cd <directory where you have Zero2WFpv>

For example, if you downloaded the Zero2WFpv repository, there is a compiled version inside the directory “/Zero2WFpv/Compiled”:

cd ./Compiled

Then copy the file to Raspberry Pi:

scp ./Zero2WFpv pi@192.168.1.100:/home/pi

Enter the password.

The file will be copied to Raspberry Pi’s home directory.

Note: The procedure described above can be carried out without the command line. It is much easier. Please check the WinSCP application. It does not require any command line interaction.

Now it needs to be made executable, as it is currently just a raw file for Raspberry Pi. Run the following commands on Raspberry Pi.

Go to the home directory where we have Zero2WFpv:

cd

Make the file executable:

chmod +x ./Zero2WFpv

Now the application is ready to run.

Run application

Copy the application (Zero2WFpv executable and Zero2WFpv.json) to any folder and run:

./Zero2WFpv

Table 6 - Application arguments.

Argument Description
–install Install application as systemd service that will be run automatically after boot. The application exits after the service is installed and started.
-v Enable logger to print logs to console and to the log file.
-vv Alias of -v. The application has a single verbosity level.
-rc Prints decoded CRSF channels to console. It is useful for determining channel numbers.
-h, –help Prints help message that includes application arguments.

When the application is started for the first time, it creates a configuration file named Zero2WFpv.json if the file does not exist (refer to the Config file section) and exits, so that the file can be reviewed before the first real run. The configuration file name follows the name of the executable. If the application is run as a superuser using sudo, the file will be owned by the root user. Therefore, to modify the configuration file, superuser privileges will be necessary. For example:

sudo ./Zero2WFpv --install

Build application

Before building, the user should configure Raspberry Pi (Raspberry PI configuration). Typical build commands:

cd Zero2WFpv
git submodule update --init --recursive
mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Release ..
make

The application links OpenCV (frame scaling and the on screen overlay) and uses OpenMP for the pixel format conversion and for the tracker. Both come with the packages listed in Install dependencies.

Raspberry PI configuration

OS install

  1. Download Raspberry PI imager.

  2. Choose the Operating System: Raspberry Pi OS (other) -> Raspberry Pi OS Lite (64-bit) Debian Bookworm 12.

  3. Choose the storage (MicroSD) card.

  4. Set additional options: do not set hostname “raspberrypi.local”, enable SSH (Use password authentication), set username “pi” and password “pi”, configure wireless LAN according to your settings. You will need wi-fi for software installation, set appropriate time zone and wireless zone.

  5. Save changes and push the “Write” button. After that, push “Yes” to rewrite data on MicroSD. At the end, remove the MicroSD.

  6. Insert the MicroSD into the Raspberry Pi and power it up.

LAN configuration

  1. Configure LAN IP address on your PC. For Windows 11, go to Settings -> Network & Internet -> Advanced Network Settings -> More network adapter options. Right-click on the LAN connection used to connect to Raspberry Pi and choose “Properties”. Double-click on “Internet Protocol Version 4 (TCP/IPv4)”. Set static IP address 192.168.0.1 and mask 255.255.255.0.

  2. Connect the Raspberry Pi via LAN cable. After power up, you don’t know the IP address of the Raspberry Pi board, but you can connect to it via SSH using the “raspberrypi” name. In Windows 11, open Windows Terminal or PowerShell terminal and type the command ssh pi@raspberrypi. After connection, type yes to establish authenticity. WARNING: If you work with Windows, we recommend deleting information about previous connections. Go to folder C:/Users/[your user name]/.ssh. Open file known_hosts if it exists in a text editor. Delete all lines containing ”raspberrypi”.

  3. You need to set a static IP address on the Raspberry Pi, but not NOW. After setting a static IP, you will lose the Wi-Fi connection, which you need for software installation.

Install dependencies

  1. Connect to the Raspberry Pi via SSH: ssh pi@raspberrypi. WARNING: If you work with Windows, we recommend deleting information about previous connections. Go to folder C:/Users/[your user name]/.ssh. Open file known_hosts if it exists in a text editor. Delete all lines containing ”raspberrypi”.

  2. Install libraries:

     sudo apt-get install cmake build-essential libopencv-dev libcamera-dev -y

  3. Reboot the system.

     sudo reboot

Configuration of analog video output and serial port

To use the serial port and analog video output, Raspberry Pi should be configured. Please follow the steps below:

  1. Open the configuration file with the following command:

     sudo nano /boot/firmware/config.txt

  2. Add the following lines to the end of the file:

For Raspberry Pi 5: bash sdtv_mode=2 sdtv_aspect=1 hdmi_force_hotplug=0 hdmi_ignore_hotplug=1 enable_tvout=1 dtparam=uart0=on

For Raspberry Pi Zero 2W: bash sdtv_mode=2 sdtv_aspect=1 hdmi_force_hotplug=0 hdmi_ignore_hotplug=1 enable_tvout=1 dtoverlay=pi3-disable-bt enable_uart=1 init_uart_clock=16000000 dtparam=krnbt=off dtoverlay=uart0

Also, find the line dtoverlay=vc4-fkms-v3d and append “,composite” to the end of the line. It should look like this:

dtoverlay=vc4-fkms-v3d,composite

Alternatively, this configuration can be done using the raspi-config tool.

  1. Save the file and exit.

  2. Open the cmdline.txt file with the following command:

     sudo nano /boot/firmware/cmdline.txt

  3. Add the following line to the beginning of the file:

     video=Composite-1:720x576-24@25,tv_mode=PAL vc4.tv_norm=PAL

This kernel configuration will force composite video output. However, the resolution of the frame buffer can be different due to unstable firmware. Therefore, please check the frame buffer resolution using the following command:

```bash
fbset
```

Also, ensure that there is nothing like “console=serial0,115200” in the file. If it exists, remove it. There can be console=ttyX but not serial0. This will ensure that the serial port is not used for the console.

The file should look like this:

```bash
video=Composite-1:720x576-24@25,tv_mode=PAL vc4.tv_norm=PAL console=tty1 ........
```

  1. Save the file and exit.

  2. Reboot the Raspberry Pi with the following command:

     sudo reboot

How to set static IP

A static IP can be set using the /boot/firmware/cmdline.txt file. Open the file with the following command:

sudo nano /boot/firmware/cmdline.txt

Add the following line to the end of the file:

ip=192.168.0.5::192.168.0.6:255.255.255.0:rpi:eth0:off

The format of the ip parameter is:

ip=<client-ip>::<gateway-ip>:<netmask>:<hostname>:<device>:<autoconf>

Also, be aware that this file is located in the /boot partition of the SD card, which means it can be accessed from any computer and modified. This configuration can be done while inserting the SD card into any computer.

After setting the static IP, reboot the Raspberry Pi with the following command:

sudo reboot

Hardware installation

The following figure shows how to connect Raspberry PI to an FPV drone.

zero2wfpvhardwareconnections

This figure shows analog video output with CRSF being read from the RC receiver. If CRSF over UDP and/or RTP output is enabled, the following connections should be made:

zero2wfpvhardwareconnectionsudp

NOTE: Raspberry Pi should be powered from a stable source. If Raspberry Pi Zero 2W is used, it is possible to power it from the FC since its current draw will not exceed 1 Amp. However, for Raspberry Pi 5, it is recommended to use a separate DC-DC converter to power the Raspberry Pi.

Source code 

#include <iostream>
#include <string>
#include <fstream>
#include <cstdlib>
#include <unistd.h>
#include <limits.h>
#include <algorithm>
#include <cstring>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <chrono>
#include <opencv2/opencv.hpp>

#include "VSourceLibCamera.h"
#include "VOutputFb.h"
#include "FormatConverter.h"
#include "CvTracker.h"
#include "Logger.h"
#include "CrsfParser.h"
#include "SerialPort.h"
#include "UdpSocket.h"
#include "VCodecV4L2.h"
#include "RtpPusher.h"
#include "utils.h"
#include "Pid.h"
#include "Zero2WFpvVersion.h"



/// Log folder.
#define LOG_FOLDER "Log"



// Link namespaces.
using namespace cv;
using namespace std;
using namespace cr::clib;
using namespace cr::video;
using namespace cr::utils;
using namespace std::chrono;
using namespace cr::vtracker;
using namespace cr::rtp;


/// Application params.
Params g_params;
/// Logger.
Logger g_log;
/// Log flag.
PrintFlag g_logFlag{PrintFlag::DISABLE};
/// Size of frame buffer.
const int g_frameBufferSize{4};
/// Shared frame buffer.
Frame g_sharedFrame[g_frameBufferSize];
/// Shared frame index.
atomic<uint8_t> g_sharedFrameIndex{0};
/// Shared frame condition variable.
condition_variable g_sharedFrameCond;
/// Shared frame condition variable mutex.
mutex g_sharedFrameCondMutex;
/// Shared frame condition variable flag. Set by the capture loop when a new
/// frame is published, cleared by the tracker thread when it takes it.
atomic<bool> g_sharedFrameCondFlag{false};
/// Tracking rectangle X coordinate.
atomic<int> g_trackerResultX;
/// Tracking rectangle Y coordinate.
atomic<int> g_trackerResultY;
/// Tracking rectangle width.
atomic<int> g_trackerResultWidth;
/// Tracking rectangle height.
atomic<int> g_trackerResultHeight;
/// Tracking rectangle mode.
/// 0 - free mode, 1 - tracking mode, 2 - inertial mode, 3 - static mode.
atomic<int> g_trackerResultMode;
/// Tracker processing time.
atomic<int> g_trackerProcessingTime;
/// Tracker.
CvTracker g_tracker;
/// Flag that indicates if auto control is enabled.
atomic<bool> g_isArmed;
/// Throttle value.
atomic<int> g_throttleValue{0};
/// Flag for printing RC channels.
bool g_printRcChannels{false};

/// CRSF baudrate. 420000 is not one of the standard POSIX baudrates, so the
/// SerialPort library programs it through its custom baudrate path
/// (termios2 + BOTHER). The default "8N1" mode is what CRSF requires.
const unsigned int g_crsfBaudrate{420000};
/// CRSF input read timeout, milliseconds. The same value is used for the
/// serial port and for the UDP socket so that both input paths are paced
/// identically when no data arrives.
const int g_crsfReadTimeoutMs{100};
/// Number of CPU cores the pixel format converters are allowed to occupy.
/// The remaining cores are left to the tracker thread and to the H264 encoder.
const int g_converterThreads{2};

/// Hold time to open the on screen menu.
const milliseconds g_menuOpenHoldTime{5000};
/// Hold time to close the on screen menu and save the config file.
const milliseconds g_menuCloseHoldTime{1000};
/// Minimum interval between two on screen menu actions (stick debounce).
const milliseconds g_menuActionInterval{200};
/// Minimum interval between two tracker capture/reset commands.
const milliseconds g_captureInterval{400};
/// Delay after arming before stick commands start moving the tracking rectangle.
const milliseconds g_trackingCommandDelay{300};
/// Time without a decoded RC channels packet after which the RC link is
/// considered lost and auto control is disarmed. A single read does not have
/// to end on a packet boundary, so the window has to cover several packets.
const milliseconds g_rcLinkTimeout{500};

/// Yaw PID controller.
Pid g_pidYaw;
/// Throttle PID controller.
Pid g_pidThrottle;
/// Roll PID controller.
Pid g_pidRoll;
/// Pitch "controller". Only the center and the enable flag are used: pitch is
/// held at a constant value instead of being driven by a PID loop.
Pid g_pidPitch;

/// Executable name.
string g_executableName;

/**
 * @brief Crsf communication thread function.
 */
void CrsfThreadFunc();

/**
 * @brief Tracker thread function.
 */
void trackerThreadFunc();

/// Config mode flag.
std::atomic<bool> g_configMode{false};
/// Menu index. 0 - Main menu, 1 - Sub menu.
std::atomic<int> g_MenuIndex{0};
/// Cursor index for main menu. 0 - Throttle, 1 - Yaw, 2 - Roll, 3 - Constant Pitch, 4 - Config.
std::atomic<int> g_cursorIndex{0};
/// Cursor index for sub menu. Values:
///         For cursor 0 - 3 (Throttle, Yaw, Roll, Pitch):
///         0 - Center, 1 - Kp, 2 - Kd, 3 - Ki, 4 - PID Max, 5 - PID Min, 6 - Mode.
///         For cursor 4 (Config):
///         0 - Capture Channel, 1 - Arm Channel, 2 - Throttle Channel,
///         3 - Roll Channel, 4 - Pitch Channel, 5 - Yaw Channel,
///         6 - Print throttle.
std::atomic<int> g_subCursorIndex{0};

/// Output resolution. With RTP output it is the camera resolution, otherwise
/// it is the resolution reported by the frame buffer. The values below are
/// only defaults until one of the two is known.
int g_outputWidth = 704;
int g_outputHeight = 576;



int main(int argc, char **argv)
{
    //checkLicense();

    // Configure folder to save logs.
    Logger::setSaveLogParams(LOG_FOLDER, "log", 20, 1);

    // Check if the executable name starts with "./"
    g_executableName = argv[0];
    if (g_executableName.rfind("./", 0) == 0)
        g_executableName = g_executableName.substr(2); // Remove the "./" prefix

    // Welcome message.
    g_log.print(PrintColor::YELLOW, PrintFlag::CONSOLE) <<
    "[" << __LOGFILENAME__ << "][" << __LINE__ << "] " <<
    g_executableName << " application v" << ZERO2W_FPV_VERSION << std::endl;

    // Parse arguments.
    if (argc > 1)
    {
        // Parse the arguments
        for (int i = 1; i < argc; ++i) 
        {
            std::string arg = argv[i];

            // "-vv" is accepted as an alias of "-v": the application has a
            // single verbosity level.
            if (arg == "-v" || arg == "-vv") 
                g_logFlag = PrintFlag::CONSOLE_AND_FILE;
            else if (arg == "--install") 
            {
                // The service is started by systemd, so this process must not
                // continue into the capture loop as a second instance.
                createSystemdService(g_executableName);
                return 0;
            }
            else if (arg == "-h" || arg == "--help")
                printUsage(g_executableName);
            else if (arg == "-rc")
                g_printRcChannels = true;
            else 
            {
                std::cerr << "See -h or --help option for usage." << std::endl;
                return -1;
            }
        }
    }

    // Load config file.
    if (!loadConfig(g_executableName + ".json"))
    {
        g_log.print(PrintColor::RED, startupLogFlag()) <<
        "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
        "Can't load config file." << std::endl;
        return -1;
    }
    g_log.print(PrintColor::CYAN, g_logFlag) <<
    "[" << __LOGFILENAME__ << "][" << __LINE__ << "] " <<
    "Config file loaded." << std::endl;

    // Channel numbers from the config file are used directly as indexes into
    // the 16 element CRSF channel array, so they must be validated before use.
    if (!checkChannelNumbers())
    {
        g_log.print(PrintColor::RED, startupLogFlag()) <<
        "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
        "Channel number out of range [0, 15] in config file." << std::endl;
        return -1;
    }

    // Centers and limits are encoded into 11 bit CRSF channels, so a value the
    // encoder would reject must be caught before the controllers are built.
    if (!checkPidLimits())
    {
        g_log.print(PrintColor::RED, startupLogFlag()) <<
        "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
        "PID center/min/max out of range [0, 1984] in config file." << std::endl;
        return -1;
    }

    // Init pid controllers with config params.
    g_pidThrottle = Pid(g_params.pidControl.throttleKp, g_params.pidControl.throttleKd, g_params.pidControl.throttleKi,
                        g_params.pidControl.throttleCenter, g_params.pidControl.throttleMin, g_params.pidControl.throttleMax, g_params.pidControl.isThrottleControlEnabled);

    g_pidYaw = Pid(g_params.pidControl.yawKp, g_params.pidControl.yawKd, g_params.pidControl.yawKi,
                    g_params.pidControl.yawCenter, g_params.pidControl.yawMin, g_params.pidControl.yawMax, g_params.pidControl.isYawControlEnabled);

    g_pidRoll = Pid(g_params.pidControl.rollKp, g_params.pidControl.rollKd, g_params.pidControl.rollKi,
                    g_params.pidControl.rollCenter, g_params.pidControl.rollMin, g_params.pidControl.rollMax, g_params.pidControl.isRollControlEnabled);

    g_pidPitch = Pid(0, 0, 0, g_params.pidControl.constantPitch, 0, 0, g_params.pidControl.isConstantPitchEnabled);

    // Rtp pusher.
    RtpPusher rtpPusher;

    // Frame buffer output.
    VOutputFb frameBufferOutput;

    // Init frame buffer output if RTP output is disabled. RtpPusher has no
    // init() in the new interface — resources are lazily created on first send().
    if (!g_params.videoOutput.isRtpOutput)
    {
        if (!frameBufferOutput.init())
        {
            g_log.print(cr::utils::PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Can't initialise video output." << std::endl;
            return -1;
        }
    }

    if(g_params.videoOutput.isRtpOutput)
    {
        // If rtp output is enabled, set output resolution to directly to camera resolution.
        g_outputWidth = g_params.videoSource.width;
        g_outputHeight = g_params.videoSource.height;
    }
    else
    {
        // If frame buffer output is enabled, set output resolution to frame buffer resolution.
        frameBufferOutput.getResolution(g_outputWidth, g_outputHeight);
    }

    // Init video source.
    VSource* videoSource = new VSourceLibCamera();

    // Set video source parameters.
    videoSource->setParam(VSourceParam::ROI_X, (g_params.videoSource.width - g_outputWidth) / 2);
    videoSource->setParam(VSourceParam::ROI_Y, (g_params.videoSource.height - g_outputHeight) / 2);
    videoSource->setParam(VSourceParam::ROI_WIDTH, g_outputWidth);
    videoSource->setParam(VSourceParam::ROI_HEIGHT, g_outputHeight);
    // Frame rate, exposure mode and gain mode are not run time parameters of
    // VSourceLibCamera. The frame rate is part of the init string below, the
    // exposure and the gain are left to the automatic control of the camera.

    // Init string for video source: index;width;height;fps;pixel format.
    string initString = to_string(g_params.videoSource.cameraIndex) +  ";" + to_string(g_params.videoSource.width) + ";" +
                    to_string(g_params.videoSource.height) + ";" + to_string(g_params.videoSource.fps) + ";" + g_params.videoSource.pixelFormat;

    // Init video source.
    if (!videoSource->openVSource(initString))
    {
        g_log.print(PrintColor::RED, g_logFlag) <<
        "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
        "Can't initialise video source." << std::endl;
        return -1;
    }

    g_log.print(PrintColor::CYAN, g_logFlag) <<
    "[" << __LOGFILENAME__ << "][" << __LINE__ << "] INFO: " <<
    "Video source init." << std::endl;

    // Wait for camera to init.
    this_thread::sleep_for(seconds(1));

    // Start Crsf communication thread.
    thread crsfThread(CrsfThreadFunc);
    crsfThread.detach();

    // Start tracker thread.
    thread trackerThread(trackerThreadFunc);
    trackerThread.detach();

    /// Source frame.
    Frame sourceFrame;
    Frame sourceFrameYuv;
    sourceFrameYuv.fourcc = Fourcc::YUV24;
    /// Bgr frame for frame buffer output.
    Frame bgrFrame;
    bgrFrame.fourcc = Fourcc::BGR24;
    /// NV12 frame for H264 encoding.
    Frame nv12Frame;
    nv12Frame.fourcc = Fourcc::NV12;
    /// H264 frame for RTP output.
    Frame h264Frame (g_outputWidth, g_outputHeight, Fourcc::H264);

    // Error message when there is no frame from camera.
    cr::video::Frame emptyFrame (g_outputWidth, g_outputHeight, cr::video::Fourcc::BGR24);
    Mat errorMessage(emptyFrame.height, emptyFrame.width, CV_8UC3, emptyFrame.data);
    string text = "No video from camera";
    int fontFace = FONT_HERSHEY_SIMPLEX;
    double fontScale = 1.5;
    int thickness = 2;
    // Calculate text size
    int baseline = 0;
    Size textSize = getTextSize(text, fontFace, fontScale, thickness, &baseline);
    baseline += thickness;
    // Center the text
    Point textOrg((errorMessage.cols - textSize.width) / 2, (errorMessage.rows + textSize.height) / 2);
    // Render the text on the frame
    putText(errorMessage, text, textOrg, fontFace, fontScale, Scalar(0, 0, 255), thickness, 8);

    // Init the frames shared with the tracker thread. The tracker runs on a
    // copy downscaled by 2 to keep up with the camera on a Zero 2W.
    for (int i = 0; i < g_frameBufferSize; i++)
        g_sharedFrame[i] = Frame(g_outputWidth / 2, g_outputHeight / 2, Fourcc::YUV24);

    // Init frame converters. FormatConverter is single threaded until
    // setMaxThreads(...) is called, and the limit is per converter object.
    FormatConverter converter;
    converter.setMaxThreads(g_converterThreads);
    // Converter for NV12.
    FormatConverter converterNV12;
    converterNV12.setMaxThreads(g_converterThreads);

    // Codec for H264 encoding.
    VCodecV4L2 codec;

    // Set codec parameters.
    codec.setParam(VCodecParam::BITRATE_KBPS, g_params.videoOutput.encodingBitrateKpbs);
    codec.setParam(VCodecParam::GOP, g_params.videoOutput.gopSize);
    codec.setParam(VCodecParam::FPS, g_params.videoOutput.fps);

    /// Ring slot the capture loop writes next. g_sharedFrameIndex holds the
    /// slot that was published last, so it cannot be reused as write cursor.
    uint8_t sharedFrameWriteIndex = 0;

    while(true)
    {
        // Get frame from video source. 100 msec timeout.
        if (!videoSource->getFrame(sourceFrame, 100))
        {
            g_log.print(PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Can't get frame from video source." << std::endl;

            // Put empty frame to output. The frame buffer is only initialised
            // when RTP output is disabled, so it must not be written otherwise.
            if (!g_params.videoOutput.isRtpOutput && !frameBufferOutput.write(emptyFrame))
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't write empty frame to frame buffer." << std::endl;
            }

            /// @todo Error frame to rtp pusher should be added.
            continue;
        }

        // Convert source frame to YUV.
        if (!converter.convert(sourceFrame, sourceFrameYuv))
        {
            g_log.print(PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Can't convert frame to YUV24." << std::endl;
            continue;
        }

        // Downscale the frame into the next slot of the ring shared with the
        // tracker thread.
        const uint8_t index = sharedFrameWriteIndex;

        Mat sourceFrameYuvMat(sourceFrameYuv.height, sourceFrameYuv.width, CV_8UC3, sourceFrameYuv.data);
        Mat sourceFrameYuvMatScaled(g_sharedFrame[index].height, g_sharedFrame[index].width, CV_8UC3, g_sharedFrame[index].data);
        resize(sourceFrameYuvMat, sourceFrameYuvMatScaled, Size(g_sharedFrame[index].width, g_sharedFrame[index].height), 0.0, 0.0, INTER_LINEAR);

        sharedFrameWriteIndex = (uint8_t)((index + 1) % g_frameBufferSize);

        // Publish the slot and notify the tracker thread. The index is stored
        // under the mutex so that the pixels written above are visible to the
        // tracker thread before it observes the new index. The ring gives the
        // tracker g_frameBufferSize - 1 frame periods to finish with a slot
        // before the capture loop comes back to it.
        unique_lock<mutex> lock(g_sharedFrameCondMutex);
        g_sharedFrameIndex.store(index);
        g_sharedFrameCondFlag.store(true);
        g_sharedFrameCond.notify_one();
        lock.unlock();

        // Draw tracker results.
        Mat drawingMat(sourceFrameYuv.height, sourceFrameYuv.width, CV_8UC3, sourceFrameYuv.data);
        cv::Scalar colorDark(0, 128, 128); // Black in YUV24.
        cv::Scalar colorWhite(255, 128, 128); // White in YUV24.
        cv::Scalar colorRed(76, 84, 255); // Red in YUV24.

        // Draw config menu.
        if (g_configMode.load())
        {
            drawConfigMenu(drawingMat, g_MenuIndex.load(), g_cursorIndex.load(), g_subCursorIndex.load(), g_outputWidth, g_outputHeight, g_pidThrottle, g_pidYaw, g_pidRoll, g_pidPitch);
        }
        else
        {
            // Draw tracking rectangle.
            if (g_trackerResultMode.load() != 0)
            {
                drawRectangle(drawingMat, sourceFrameYuv.width, sourceFrameYuv.height, g_trackerResultWidth.load() / 2, g_trackerResultHeight.load() / 2, g_trackerResultX.load(), g_trackerResultY.load(), 32, colorDark);
                drawRectangle(drawingMat, sourceFrameYuv.width, sourceFrameYuv.height, g_trackerResultWidth.load() / 2 - 4, g_trackerResultHeight.load() / 2 - 4, g_trackerResultX.load(), g_trackerResultY.load(), 28, colorWhite);
            }
            

            if (g_isArmed)
            {
                std::string isArmed = "Auto";
                // Horizontally centered, above the bottom edge of the screen.
                int x = g_outputWidth / 2 - 15; // Almost center.
                int y = g_outputHeight - 100; // Bottom of the screen.
                putText(drawingMat, isArmed, Point(x, y), FONT_HERSHEY_SIMPLEX, 0.9, colorDark, 1);
                putText(drawingMat, isArmed, Point(x + 1, y), FONT_HERSHEY_SIMPLEX, 0.9, colorWhite, 1);
            }

            if (g_params.pidControl.isPrintThrottle)
            {
                std::string throttleValue = "Throttle (current) : " + to_string(g_throttleValue.load());
                putText(drawingMat, throttleValue, Point(240, (g_outputHeight - 46)), FONT_HERSHEY_SIMPLEX, 0.9, colorRed, 2);

                std::string throttleCenter = "Throttle center : " + to_string(g_pidThrottle.center);
                putText(drawingMat, throttleCenter, Point(240, (g_outputHeight - 16)), FONT_HERSHEY_SIMPLEX, 0.9, colorRed, 2);
            }

            // Dark circle in the center of the screen.
            circle(drawingMat, Point(g_outputWidth / 2, g_outputHeight / 2), 4, colorDark, -2);
            circle(drawingMat, Point(g_outputWidth / 2, g_outputHeight / 2), 2, colorWhite, -2);

            // Draw only if tracker is in free mode.
            if (g_trackerResultMode.load() == 0)
            {
                drawRectangle(drawingMat, sourceFrameYuv.width, sourceFrameYuv.height, 64, 64, g_outputWidth / 2, g_outputHeight / 2, 8, colorDark);
                drawRectangle(drawingMat, sourceFrameYuv.width, sourceFrameYuv.height, 60, 60, g_outputWidth / 2, g_outputHeight / 2, 4, colorWhite);
            }
        }

        if (g_params.videoOutput.isRtpOutput)
        {
            // Convert YUV frame to NV12 frame
            if (!converterNV12.convert(sourceFrameYuv, nv12Frame))
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't convert frame to NV12." << std::endl;
                continue;
            }

            // Encode NV12 frame to H264.
            if (!codec.transcode(nv12Frame, h264Frame))
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't encode frame to H264." << std::endl;
                continue;
            }

            // Push H264 frame to RTP output.
            int rtpSendResult = rtpPusher.send(
                h264Frame.data,
                static_cast<size_t>(h264Frame.size),
                "H264",
                g_params.videoOutput.ip,
                static_cast<uint16_t>(g_params.videoOutput.port),
                0,
                static_cast<float>(g_params.videoOutput.fps),
                1420,
                g_params.videoOutput.encodingBitrateKpbs);
            if (rtpSendResult != RtpPusher::SEND_OK &&
                rtpSendResult != RtpPusher::SEND_FRAME_DROP)
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't push frame to RTP (code " << rtpSendResult << ")." << std::endl;
            }
        }
        else // Frame buffer output.
        {
            // Convert the frame with the overlay to BGR24 for the frame buffer.
            if (!converter.convert(sourceFrameYuv, bgrFrame))
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't convert frame to BGR24." << endl;
                continue;
            }

            // Write frame to output.
            if (!frameBufferOutput.write(bgrFrame))
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't write frame to frame buffer." << std::endl;
            }
        }
    }

    return 0;
}



void trackerThreadFunc()
{
    // Set default tracker params.
    g_tracker.setParam(VTrackerParam::RECT_HEIGHT, 32);
    g_tracker.setParam(VTrackerParam::RECT_WIDTH, 32);
    g_tracker.setParam(VTrackerParam::SEARCH_WINDOW_HEIGHT, 128);
    g_tracker.setParam(VTrackerParam::SEARCH_WINDOW_WIDTH, 128);
    g_tracker.setParam(VTrackerParam::MULTIPLE_THREADS, 1);
    g_tracker.setParam(VTrackerParam::NUM_CHANNELS, 2); // 2 color channels.
    g_tracker.setParam(VTrackerParam::TYPE, 0); // Daylight.


    while(true)
    {
        // Wait for the next frame from the main thread and latch the slot it
        // published. The flag is cleared here so that the next iteration waits
        // again instead of reprocessing the same frame.
        uint8_t index = 0;
        {
            std::unique_lock<std::mutex> lock(g_sharedFrameCondMutex);
            while(!g_sharedFrameCondFlag.load())
            {
                g_sharedFrameCond.wait(lock);
            }
            g_sharedFrameCondFlag.store(false);
            index = g_sharedFrameIndex.load();
        }

        // Process the frame the main thread has just published.
        if (!g_tracker.processFrame(g_sharedFrame[index]))
        {
            g_log.print(cr::utils::PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Can't process frame." << std::endl;
            continue;
        }

        // Get tracker result.
        VTrackerParams params;
        g_tracker.getParams(params);

        // Set tracker rectangle size in FREE mode.
        if (params.mode == 0)
        {
            g_tracker.setParam(VTrackerParam::RECT_HEIGHT, 64);
            g_tracker.setParam(VTrackerParam::RECT_WIDTH, 64);
        }

        g_trackerResultX.store(params.rectX * 2);
        g_trackerResultY.store(params.rectY * 2);
        g_trackerResultWidth.store(params.rectWidth * 2);
        g_trackerResultHeight.store(params.rectHeight * 2);
        g_trackerResultMode.store(params.mode);
        g_trackerProcessingTime.store(params.processingTimeMks);
    }
}



void CrsfThreadFunc()
{
    cr::clib::SerialPort serialPort;
    cr::clib::UdpSocket udpSocket;

    if (g_params.communication.isCrsfOverUdp)
    {
        if (!udpSocket.open((uint16_t)g_params.communication.crsfUdpPort, true,
                            "127.0.0.1", g_crsfReadTimeoutMs))
        {
            g_log.print(PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Can't open udp socket." << std::endl;
            exit(-1);
        }
    }

    // Serial port is always used. Even if UDP is enabled for reading CRSF, serial port is used to write CRSF data to FC.
    if (!serialPort.open(g_params.communication.serialPortName, g_crsfBaudrate, g_crsfReadTimeoutMs))
    {
        g_log.print(PrintColor::RED, g_logFlag) <<
        "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
        "Can't open serial port : " << g_params.communication.serialPortName << std::endl;
        exit(-1);
    }

    /// CRSF parser.
    cr::clib::CrsfParser parser;

    /// RC channels of the last decoded RC_CHANNELS packet.
    uint32_t crsfChannels[16] = {};
    /// Capture channel value of the previous decoded packet.
    uint32_t captureChannelLastValue = 0;
    /// Arm channel value of the previous decoded packet.
    int armChannelLastValue = 0;
    /// The first decoded packet only seeds the values above: a switch that is
    /// already in the active position at startup must not trigger anything.
    bool isFirstPacket = true;

    /// Time of the last tracker capture/reset command.
    steady_clock::time_point lastCaptureTime = steady_clock::now();
    /// Time of the last on screen menu action.
    steady_clock::time_point lastMenuActionTime = steady_clock::now();
    /// Time the "open menu" stick combination has been held since.
    steady_clock::time_point menuOpenHoldStart = steady_clock::now();
    /// Time the "close menu" stick combination has been held since.
    steady_clock::time_point menuCloseHoldStart = steady_clock::now();
    /// Time the auto control was armed.
    steady_clock::time_point armTime = steady_clock::now();
    /// Time of the last decoded RC channels packet.
    steady_clock::time_point lastRcPacketTime = steady_clock::now();
    /// FALSE forces the arm channel to be re-evaluated on the next packet
    /// instead of waiting for the value to change.
    bool isArmChannelValid = false;

    while(true)
    {
        // Read incoming CRSF data. Depending on the configuration it comes
        // either from the serial port or from the UDP socket.
        const int bufferSize = 64;
        uint8_t rxBuffer[bufferSize];
        int bytesRead = 0;

        if (g_params.communication.isCrsfOverUdp)
            bytesRead = udpSocket.read(rxBuffer, bufferSize);
        else
            bytesRead = serialPort.read(rxBuffer, bufferSize);

        // A negative result is a port error, not a timeout: reopen the port
        // instead of spinning on it. read(...) returns 0 when the timeout
        // expires, which is self pacing and needs no delay here.
        if (bytesRead < 0 && !g_params.communication.isCrsfOverUdp)
        {
            g_log.print(PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Serial port read error. Reopening : " << g_params.communication.serialPortName << std::endl;

            serialPort.close();
            this_thread::sleep_for(milliseconds(500));
            serialPort.open(g_params.communication.serialPortName, g_crsfBaudrate, g_crsfReadTimeoutMs);
            continue;
        }

        // Decode incoming data. Every byte of the chunk is fed to the parser:
        // a chunk can hold more than one packet and the parser is a byte
        // oriented state machine, so nothing may be dropped.
        uint8_t packet[64];
        int size = 0;
        bool isDetected = false;
        for (int i = 0; i < bytesRead; i++)
        {
            if (parser.detect(rxBuffer[i], packet, size) == cr::clib::CrsfPacket::RC_CHANNELS)
            {
                parser.getRcChannels(crsfChannels);
                isDetected = true;
            }
        }

        // A read does not have to end on a packet boundary, so a chunk that
        // does not complete an RC channels packet is normal. There is nothing
        // to react to and nothing to forward in that case: staying silent lets
        // the flight controller run its own link loss failsafe instead of
        // receiving stale channels. Auto control is only given up when the
        // link has been quiet for the whole failsafe window.
        if (!isDetected)
        {
            if (g_isArmed.load() && (steady_clock::now() - lastRcPacketTime) > g_rcLinkTimeout)
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "No RC channels data. Auto control disarmed." << std::endl;
                g_isArmed = false;
                // The arm switch has not moved, so the hysteresis below would
                // never look at it again. Force a re-evaluation instead.
                isArmChannelValid = false;
            }
            continue;
        }

        const steady_clock::time_point now = steady_clock::now();
        lastRcPacketTime = now;

        if (isFirstPacket)
        {
            isFirstPacket = false;
            isArmChannelValid = true;
            captureChannelLastValue = crsfChannels[g_params.trackerControl.captureChannel];
            armChannelLastValue = (int)crsfChannels[g_params.trackerControl.armChannel];
        }

        if (g_printRcChannels)
        {
            std::cout << "\r" 
            << "Ch0:  " << crsfChannels[0] << " "
            << "Ch1:  " << crsfChannels[1] << " "
            << "Ch2:  " << crsfChannels[2] << " "
            << "Ch3:  " << crsfChannels[3] << " "
            << "Ch4:  " << crsfChannels[4] << " "
            << "Ch5:  " << crsfChannels[5] << " "
            << "Ch6:  " << crsfChannels[6] << " "
            << "Ch7:  " << crsfChannels[7] << " "
            << "Ch8:  " << crsfChannels[8] << " "
            << "Ch9:  " << crsfChannels[9] << " "
            << "Ch10: " << crsfChannels[10] << " "
            << "Ch11: " << crsfChannels[11] << " "
            << "Ch12: " << crsfChannels[12] << " "
            << "Ch13: " << crsfChannels[13] << " "
            << "Ch14: " << crsfChannels[14] << " "
            << "Ch15: " << crsfChannels[15] << " "
            << std::flush;
        }

        const uint32_t throttleChannelValue = crsfChannels[g_params.trackerControl.throttleChannel];
        const uint32_t rollChannelValue = crsfChannels[g_params.trackerControl.rollChannel];
        const uint32_t pitchChannelValue = crsfChannels[g_params.trackerControl.pitchChannel];
        const uint32_t yawChannelValue = crsfChannels[g_params.trackerControl.yawChannel];

        // The menu is opened and closed by holding a stick combination. The
        // hold is measured in time so that it does not depend on how fast RC
        // packets arrive.
        const bool isMenuOpenCombination = throttleChannelValue < 300 && yawChannelValue < 200;
        const bool isMenuCloseCombination = throttleChannelValue < 300 && yawChannelValue > 1500;

        // A hold timer only runs while its action is actually available, so
        // that leaving a sub menu or entering config mode cannot carry an
        // already elapsed hold over into the next action.
        if (!isMenuOpenCombination || g_configMode.load())
            menuOpenHoldStart = now;
        if (!isMenuCloseCombination || g_MenuIndex != 0 || !g_configMode.load())
            menuCloseHoldStart = now;

        // Close config mode and save params to config file.
        if (isMenuCloseCombination && g_MenuIndex == 0 && g_configMode.load() &&
            (now - menuCloseHoldStart) >= g_menuCloseHoldTime)
        {
            menuCloseHoldStart = now;
            lastMenuActionTime = now;
            g_configMode.store(false);

            // Save PID values to config file.
            g_params.pidControl.throttleKp = g_pidThrottle.kP;
            g_params.pidControl.throttleKd = g_pidThrottle.kD;
            g_params.pidControl.throttleKi = g_pidThrottle.kI;
            g_params.pidControl.throttleCenter = g_pidThrottle.center;
            g_params.pidControl.throttleMin = g_pidThrottle.min;
            g_params.pidControl.throttleMax = g_pidThrottle.max;
            g_params.pidControl.isThrottleControlEnabled = g_pidThrottle.isEnabled;

            g_params.pidControl.yawKp = g_pidYaw.kP;
            g_params.pidControl.yawKd = g_pidYaw.kD;
            g_params.pidControl.yawKi = g_pidYaw.kI;
            g_params.pidControl.yawCenter = g_pidYaw.center;
            g_params.pidControl.yawMin = g_pidYaw.min;
            g_params.pidControl.yawMax = g_pidYaw.max;
            g_params.pidControl.isYawControlEnabled = g_pidYaw.isEnabled;

            g_params.pidControl.rollKp = g_pidRoll.kP;
            g_params.pidControl.rollKd = g_pidRoll.kD;
            g_params.pidControl.rollKi = g_pidRoll.kI;
            g_params.pidControl.rollCenter = g_pidRoll.center;
            g_params.pidControl.rollMin = g_pidRoll.min;
            g_params.pidControl.rollMax = g_pidRoll.max;
            g_params.pidControl.isRollControlEnabled = g_pidRoll.isEnabled;

            g_params.pidControl.constantPitch = g_pidPitch.center;
            g_params.pidControl.isConstantPitchEnabled = g_pidPitch.isEnabled;

            // Config reader.
            ConfigReader config;
            if (!config.set(g_params, "Params"))
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't set config reader." << std::endl;
            }

            // Save config file.
            if (!config.writeToFile(g_executableName + ".json"))
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't save config file." << std::endl;
            }
        }
        // Enter config mode.
        else if (isMenuOpenCombination && !g_configMode.load() &&
                 (now - menuOpenHoldStart) >= g_menuOpenHoldTime)
        {
            menuOpenHoldStart = now;
            lastMenuActionTime = now;
            g_configMode.store(true);
        }

        // Menu navigation. It is only active while the menu is displayed, so
        // that flight stick input never moves the cursor in flight.
        if (g_configMode.load() && (now - lastMenuActionTime) >= g_menuActionInterval)
        {
            // Update menu cursor with pitch channel max and min values.
            if (pitchChannelValue > 1500 && g_MenuIndex == 0)
            {
                lastMenuActionTime = now;
                g_cursorIndex = (g_cursorIndex - 1 + 5) % 5;
            }
            else if (pitchChannelValue < 200 && g_MenuIndex == 0)
            {
                lastMenuActionTime = now;
                g_cursorIndex = (g_cursorIndex + 1) % 5;
            }
            // Update sub menu cursor with pitch channel max and min values.
            else if (pitchChannelValue > 1500 && g_MenuIndex == 1)
            {
                lastMenuActionTime = now;
                g_subCursorIndex = (g_subCursorIndex - 1 + 7) % 7;
            }
            else if (pitchChannelValue < 200 && g_MenuIndex == 1)
            {
                lastMenuActionTime = now;
                g_subCursorIndex = (g_subCursorIndex + 1) % 7;
            }

            // Check we enter sub menu by using roll channel.
            if (rollChannelValue > 1500)
            {
                lastMenuActionTime = now;
                g_MenuIndex = 1; // Enter sub menu.
                g_subCursorIndex = 0;
            }
            else if (rollChannelValue < 200)
            {
                lastMenuActionTime = now;
                g_MenuIndex = 0; // Exit sub menu.
                g_subCursorIndex = 0;
                g_cursorIndex = 0;
            }
        }

        // PID coefficients adjustment with 0 throttle and sub menu is open.
        if (throttleChannelValue < 300 && g_MenuIndex == 1 && g_configMode.load() &&
            (now - lastMenuActionTime) >= g_menuActionInterval)
        {
            // Adjust PID coefficients within the current menu item and cursor index.
            lastMenuActionTime = now;

            // Update PID controller values.
            if (g_cursorIndex < 4)
            {
                switch (g_cursorIndex)
                {
                case 0: // Throttle
                    g_pidThrottle.update(g_subCursorIndex.load(), yawChannelValue);
                    break;
                case 1: // Yaw
                    g_pidYaw.update(g_subCursorIndex.load(), yawChannelValue);
                    break;
                case 2: // Roll
                    g_pidRoll.update(g_subCursorIndex.load(), yawChannelValue);
                    break;
                case 3: // Pitch. Only the constant value (Center) and the
                        // enable flag (Mode) apply to a constant pitch.
                    if (g_subCursorIndex == 0 || g_subCursorIndex == 6)
                        g_pidPitch.update(g_subCursorIndex.load(), yawChannelValue);
                    break;
                }
            }
            else if (g_cursorIndex == 4) // Config channels
            {
                switch (g_subCursorIndex)
                {
                case 0: // Capture channel
                {
                    if (yawChannelValue > 1500)
                        g_params.trackerControl.captureChannel = (g_params.trackerControl.captureChannel + 1) % 16;
                    else if (yawChannelValue < 200)
                        g_params.trackerControl.captureChannel = (g_params.trackerControl.captureChannel - 1 + 16) % 16;

                    break;
                }
                case 1: // Arm channel
                {
                    if (yawChannelValue > 1500)
                        g_params.trackerControl.armChannel = (g_params.trackerControl.armChannel + 1) % 16;
                    else if (yawChannelValue < 200)
                        g_params.trackerControl.armChannel = (g_params.trackerControl.armChannel - 1 + 16) % 16;

                    break;
                }
                case 2: // Throttle channel
                {
                    if (yawChannelValue > 1500)
                        g_params.trackerControl.throttleChannel = (g_params.trackerControl.throttleChannel + 1) % 16;
                    else if (yawChannelValue < 200)
                        g_params.trackerControl.throttleChannel = (g_params.trackerControl.throttleChannel - 1 + 16) % 16;

                    break;
                }
                case 3: // Roll channel
                {
                    if (yawChannelValue > 1500)
                        g_params.trackerControl.rollChannel = (g_params.trackerControl.rollChannel + 1) % 16;
                    else if (yawChannelValue < 200)
                        g_params.trackerControl.rollChannel = (g_params.trackerControl.rollChannel - 1 + 16) % 16;

                    break;
                }
                case 4: // Pitch channel
                {
                    if (yawChannelValue > 1500)
                        g_params.trackerControl.pitchChannel = (g_params.trackerControl.pitchChannel + 1) % 16;
                    else if (yawChannelValue < 200)
                        g_params.trackerControl.pitchChannel = (g_params.trackerControl.pitchChannel - 1 + 16) % 16;

                    break;
                }
                case 5: // Yaw channel
                {
                    if (yawChannelValue > 1500)
                        g_params.trackerControl.yawChannel = (g_params.trackerControl.yawChannel + 1) % 16;
                    else if (yawChannelValue < 200)
                        g_params.trackerControl.yawChannel = (g_params.trackerControl.yawChannel - 1 + 16) % 16;

                    break;
                }
                case 6: // Print throttle value. The stick direction selects the
                        // state instead of toggling, so the value is predictable.
                {
                    if (yawChannelValue > 1500)
                        g_params.pidControl.isPrintThrottle = true;
                    else if (yawChannelValue < 200)
                        g_params.pidControl.isPrintThrottle = false;

                    break;
                }
                }
            }
        }

        // Capture/reset the tracker when the capture channel crosses its
        // trigger value. The edge check keeps a held switch from toggling
        // capture and reset over and over.
        const uint32_t captureChannelValue = crsfChannels[g_params.trackerControl.captureChannel];
        const uint32_t captureTriggerValue = (uint32_t)g_params.trackerControl.captureChannelTriggerValue;
        if (captureChannelValue > captureTriggerValue &&
            captureChannelLastValue <= captureTriggerValue &&
            (now - lastCaptureTime) >= g_captureInterval)
        {
            lastCaptureTime = now;

            if (g_trackerResultMode.load() == 1)
            {
                if (!g_tracker.executeCommand(cr::vtracker::VTrackerCommand::RESET))
                {
                    g_log.print(PrintColor::RED, g_logFlag) <<
                    "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                    "Can't send tracker command (RESET)." << std::endl;
                }

                g_isArmed = false;
            }
            else if (g_trackerResultMode.load() == 0)
            {
                if (!g_tracker.executeCommand(cr::vtracker::VTrackerCommand::CAPTURE_PERCENTS, 50, 50))
                {
                    g_log.print(PrintColor::RED, g_logFlag) <<
                    "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                    "Can't send tracker command (CAPTURE)." << std::endl;
                }
            }
        }
        captureChannelLastValue = captureChannelValue;

        // Arming (Auto control). The channel is only evaluated when it really
        // moves, so the arithmetic has to be signed.
        const int armChannelValue = (int)crsfChannels[g_params.trackerControl.armChannel];
        if (!isArmChannelValid || std::abs(armChannelValue - armChannelLastValue) > 500)
        {
            isArmChannelValid = true;
            if (armChannelValue > g_params.trackerControl.armChannelTriggerValue && g_trackerResultMode.load() == 1)
            {
                g_isArmed = true;
                armTime = now;
            }
            else if (armChannelValue < g_params.trackerControl.armChannelTriggerValue && g_isArmed)
            {
                g_isArmed = false;
            }
            armChannelLastValue = armChannelValue;
        }

        // If it is armed and tracker is in tracking mode then control the drone.
        if (g_isArmed && g_trackerResultMode.load() == 1)
        {
            // Give the operator a moment to release the sticks after arming
            // before they start moving the tracking rectangle.
            if ((now - armTime) >= g_trackingCommandDelay)
            {
                // Control tracking rectangle with channels.
                if (rollChannelValue < 500)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::MOVE_RECT, -1, 0);
                else if (rollChannelValue > 1400)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::MOVE_RECT, 1, 0);

                if (pitchChannelValue < 500)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::MOVE_RECT, 0, 1);
                else if (pitchChannelValue > 1400)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::MOVE_RECT, 0, -1);

                // Change tracking rectangle size with yaw channel.
                if (yawChannelValue < 500)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::CHANGE_RECT_SIZE, -1, -1);
                else if (yawChannelValue > 1500)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::CHANGE_RECT_SIZE, 1, 1);
            }

            // Yaw control.
            if (g_pidYaw.isEnabled)
            {
                int yawSetPoint = (g_outputWidth / 2);
                int yawInput = g_trackerResultX.load();
                int yawControl = (int)g_pidYaw.compute((float)yawInput, (float)yawSetPoint);
                yawControl = clamp(yawControl, -500, 500); // May be removed in future by testing.
                crsfChannels[g_params.trackerControl.yawChannel] = g_pidYaw.clampToRange((int)g_pidYaw.center + yawControl);
            }

            // Throttle control.
            if (g_pidThrottle.isEnabled)
            {
                int throttleSetPoint = (g_outputHeight / 2);
                int throttleInput = g_trackerResultY.load();
                int throttleControl = (int)g_pidThrottle.compute((float)throttleInput, (float)throttleSetPoint);
                throttleControl = clamp(throttleControl, -400, 400); // May be removed in future by testing.
                crsfChannels[g_params.trackerControl.throttleChannel] = g_pidThrottle.clampToRange((int)g_pidThrottle.center - throttleControl);
            }

            // Roll control.
            if (g_pidRoll.isEnabled)
            {
                int rollSetPoint = (g_outputWidth / 2);
                int rollInput = g_trackerResultX.load();
                int rollControl = (int)g_pidRoll.compute((float)rollInput, (float)rollSetPoint);
                rollControl = clamp(rollControl, -500, 500); // May be removed in future by testing.
                crsfChannels[g_params.trackerControl.rollChannel] = g_pidRoll.clampToRange((int)g_pidRoll.center + rollControl);
            }

            // Constant pitch control.
            if (g_pidPitch.isEnabled)
                crsfChannels[g_params.trackerControl.pitchChannel] = g_pidPitch.center;
        }

        g_throttleValue = (int)crsfChannels[g_params.trackerControl.throttleChannel];

        // The encoder rejects a packet that holds a channel above PID_LIMIT_MAX
        // and leaves the buffer untouched. Clamping here keeps one bad channel
        // from silencing the whole link to the flight controller.
        for (int i = 0; i < 16; i++)
            if (crsfChannels[i] > PID_LIMIT_MAX)
                crsfChannels[i] = PID_LIMIT_MAX;

        // Encode RC channels.
        if (!parser.encodeRcChannels(crsfChannels, packet, size))
        {
            g_log.print(PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Can't encode RC channels." << std::endl;
            continue;
        }

        // Write data to serial port. The port is non blocking, so a single
        // write may accept only a part of the packet.
        int bytesSent = 0;
        while (bytesSent < size)
        {
            int result = serialPort.write(packet + bytesSent, size - bytesSent);
            if (result <= 0)
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't write data to serial port." << std::endl;
                break;
            }
            bytesSent += result;
        }
    }
}

Betaflight related information

Zero2WFpv application is tested with AT32 and STM32 flight controllers with Betaflight 4.3 and 4.4 versions. Zero2WFpv works only with ANGLE and HORIZON modes, ACRO mode is not supported.

NOTE: The Betaflight antigravity feature should be configured properly for the drone. The throttle controller may activate the antigravity feature in Betaflight. If the drone configuration is not done properly, antigravity may make the drone unstable under throttle control. This is especially important for the drones with high power motors and heavy payloads.

Licensing mechanism

Zero2WFpv has an integrated licensing mechanism to protect it from being copied to different hardware. The current licensing mechanism uses the following hardware-dependent parameters to validate the license:

  • Unique ID of Raspberry Pi
  • UUID of SD card
  • Serial number of SD card

If there is no license.lic file in the same directory as Zero2WFpv, it will create a Token.txt file based on these hardware-dependent parameters. The license.lic file can be obtained by using the License library from the token in Token.txt. The license file should be placed in the same directory as the Zero2WFpv executable.

Note: the license check is disabled in the source code that is shipped with this prototype - the call to checkLicense() at the top of main(…) is commented out. Uncomment it to enable the mechanism described above.

Overlay examples

Zero2WFpv can be in 3 main states. These are:

  • Free mode: In this mode, the tracker is waiting for a capture command from the center of the screen. Overlay rectangle corners are smaller than the rectangle in tracking mode. This mode is used to capture the object to be tracked.

  • Tracking mode: In this mode, the tracker is tracking the object. Overlay rectangle corners are larger than the rectangle in free mode. This state indicates that Zero2WFpv is ready to start auto control toward the object.

  • Auto control mode: In this mode, Zero2WFpv is controlling the drone toward the object. Overlay rectangle corners are the same as the rectangle in tracking mode, but a circle in the center indicates the drone’s current heading.

The following image shows the overlay examples for these 3 different states in the same order.

zero2fpvoverlaystates

Table of contents