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.0.1

Table of contents

Overview

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

zero2wfpvprinciple

After start application reads JSON config file which 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 new one with default parameters. Additionally the application creates folder “Log” 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.

Source code files

The library application by source code only. The user would be given 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.
    VSourceLibCamera ------- Source code of VSourceLibCamera library.
    CvTracker -------------- Source code of CvTracker library.
    FormatConverterOpenCv -- Source code of FormatConverterOpenCv library.
    VOutputFb -------------- Source code of VOutputFb library.
    CrsfParser ------------- Source code of CrsfParser library.
    SerialPort ------------- Source code of SerialPort library.
    License ---------------- Source code of License library.
    RtpPusher -------------- Source code of RtpPusher library.
    VCodecV4L2 ------------- Source code of VCodecV4L2 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.

Config file

Zero2WFpv application reads config file Zero2WFpv.json in the same folder with 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.
isCrsfOverUdp bool Enable crsf input over udp.
serialPortName string Serial port name.
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 diagonal trajectory is desired, this value should be close to throttleCenter. However, if horizontal distance to object is not enough long, 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.
armChannelTriggerValue int Arm channel trigger value.
captureChannel int Capture channel number.
captureChannelTriggerValue int Capture channel trigger value.
pitchChannel int Pitch channel number.
rollChannel int Roll channel number.
throttleChannel int Throttle channel number.
yawChannel int Yaw channel number.
Video output parameters:    
bandwidthKbps int Communication channel bandwidth in Kbps.
encodingBitrateKpbs int Video encoding bitrate in Kbps.
fps int Video frames per second.
gopSize int Group of pictures size.
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 connected to 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”)

On screen menu

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

  1. In order to open menu joystick position should be hold for 5 seconds as follows:
    • Throttle channel should be at minimum value.
    • Yaw channel should be at maximum left position.
  2. In order to close menu joystick position should be hold for 1 seconds as follows while in main menu:
    • Throttle channel should be at minimum value.
    • Yaw channel should be at maximum right position.
  3. In order to navigate in menu:
    • Roll channel should be used to navigate submenu and back to main menu.
    • Pitch channel should be used to iterate through parameters.
    • Yaw channel should be used to change parameter value.

zero2wfpvmenu

Table 3 - Main menu items.

Item name Description
Throttle control PID coefficients for throttle control as well as enable/disable throttle control.
Roll control PID coefficients for roll control as well as enable/disable roll control.
Yaw control PID coefficients for yaw control as well as enable/disable yaw control.
Pitch control Constant pitch value as well as enable/disable constant pitch.
Config Submenu to configure communication additional parameters.

Table 4 - Controllers menu items.

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

Table 5 - Config menu items.

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

Auto control algorithm

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

If auto control is disabled Zero2WFpv application will bypass input CRSF data to Flight controller directly without modification on it. However, if auto control algorithm is enabled and tracker is in tracking mode, application will calculate new values for roll, throttle and yaw channels. These calculated CRSF values will be sent to Flight controller via serial port. All others CRSF input data will be bypassed to Flight controller without modification.

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

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

How to copy Zero2WFpv executable to Raspberry PI

In order to copy ready application to raspberry pi follow steps. Run commands on your windows computer where you have Zero2WFpv file.

cd <directory where you have Zero2WFpv>

for example if you downloaded Zero2WFpv repository, there is compiled version inside. In directory “/Zero2WFpv/Compiled”

cd ./Compiled

then copy file to raspberry pi

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

Enter password

It will be copied to raspberry pi’s home directory.

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

Now it should become executable, it is just a raw file for raspberry yet. Run following commands on raspberry pi.

Go to home directory where we have Zero2WFpv.

cd

Make file executable.

chmod +x ./Zero2WFpv

Now application is ready to run.

Run application

Copy 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.
-v Enable logger to print logs to console.
-rc Prints raw CRSF data 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 file does not exist (refer to the Config file section). 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 build user should configure raspberry (Raspberry PI configuration). Typical build commands:

cd Zero2WFpv
mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Release ..
make

Raspberry PI configuration

OS install

  1. Download Raspberry PI imager.

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

  3. Choose 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 your settings. You will need wi-fi for software installation, set appropriate time zone and wireless zone.

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

  6. Insert MicroSD in Raspberry and power up Raspberry.

LAN configuration

  1. Configure LAN IP address on your PC. For Windows 11 got to Settings -> Network & Internet -> Advanced Network Settings -> More network adapter options. Click right button on LAN connection used to connect to Raspberry PI and chose “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 raspberry via LAN cable. After power up you don’t know IP address of Raspberry board but you can connect to raspberry via SSH using “raspberrypi” name. In Windows 11 open windows terminal or power shell terminal and type command ssh pi@raspberrypi. After connection type yes to establish authenticity. WARNING: If you work with Windows we recommend delete information about previous connections. Go to folder C:/Users/[your user name]/.ssh. Open file known_hosts is exists in text editor. Delete all lines “raspberrypi”.

  3. You have to set static IP address in Raspberry PI but not NOW. After static IP set you will lose wi-fi connection which you need for software installation.

Install dependencies

  1. Connect to raspberry via SSH: ssh pi@raspberrypi. WARNING: If you work with Windows we recommend delete information about previous connections. Go to folder C:/Users/[your user name]/.ssh. Open file known_hosts is exists in text editor. Delete all lines “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

In order to use serial port and analog video output, RaspberryPi 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 RaspberryPi 5: bash sdtv_mode=2 sdtv_aspect=1 hdmi_force_hotplug=0 hdmi_ignore_hotplug=1 enable_tvout=1 dtparam=uart0=on

For RaspberryPi 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 be like this:

dtoverlay=vc4-fkms-v3d,composite

As alternative these configuration can be done by using 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 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, resolution of frame buffer can be different because of unstable firmware. So, please check frame buffer resolution by using 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 serial port is not used for console.

File should be 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 RaspberryPi with the following command:

     sudo reboot

How to set static IP

Static IP can be set by using /boot/firmware/cmdline.txt file. Open the file with 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

format of ip parameter is :

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

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

After setting static IP, reboot the RaspberryPi with the following command:

sudo reboot

Hardware installation

Following figure shows how to connect Raspberry PI to Fpv drone.

zero2wfpvhardwareconnections

This figure shows for analog video output and CRSF is read from RC receiver. If CRSF over UDP and/or RTP output is enabled following connections should be done:

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 FC since it will not exceed 1 Amp power. However for Raspberry Pi 5, it is recommended to use a separate DC-DC converter to power 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 <opencv2/opencv.hpp>

#include "VSourceLibCamera.h"
#include "VOutputFb.h"
#include "FormatConverterOpenCv.h"
#include "CvTracker.h"
#include "Logger.h"
#include "CrsfParser.h"
#include "SerialPort.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.
atomic<bool> g_sharedFrameCondFlag;
/// 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};

/// Yaw PID controller.
Pid g_pidYaw;
/// Throttle PID controller.
Pid g_pidThrottle;
/// Roll PID controller.
Pid g_pidRoll;
/// Pitch PID controller. It is not actually a PID controller, but a constant value.
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 : 
///         0 - Center, 1 - Kp, 2 - Kd, 3 - Ki, 4 - PID Max, 5 - PID Min, 6 - PID Enable.
///         For cursor 4 :
///         0 - Capture Channel, 1 - Arm Channel, 2 - Throttle Channel, 3 - Roll Channel, 4 - Pitch Channel, 5 - Yaw Channel.
std::atomic<int> g_subCursorIndex{0};

/// Output resolution. If rtp output is enabled, set output resolution to directly to camera resolution else to frame buffer resolution.
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];

            if (arg == "-v" || arg == "-vv") 
                g_logFlag = PrintFlag::CONSOLE_AND_FILE;
            else if (arg == "--install") 
                createSystemdService(g_executableName);
            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, g_logFlag) <<
        "[" << __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;

    // 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 rtp pusher or frame buffer output based on video output config params.
    if (g_params.videoOutput.isRtpOutput)
    {
        if (!rtpPusher.init(g_params.videoOutput.ip, g_params.videoOutput.port, g_params.videoOutput.bandwidthKbps))
        {
            g_log.print(PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Can't initialise rtp pusher." << std::endl;
            return -1;
        }
    }
    else
    {
        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);
    videoSource->setParam(VSourceParam::FPS, 60);
    videoSource->setParam(VSourceParam::EXPOSURE_MODE, 1); // Auto exposure.
    videoSource->setParam(VSourceParam::GAIN_MODE, 1); // Auto gain.

    // Init string for video source.
    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 frame buffer scaled by 2.
    for (int i = 0; i < g_frameBufferSize; i++)
        g_sharedFrame[i] = Frame(g_outputWidth / 2, g_outputHeight / 2, Fourcc::YUV24);

    // Init frame converter.
    FormatConverterOpenCv converter;
    // Converter for NV12
    FormatConverterOpenCv converterNV12;

    // 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);
    codec.setDevice("/dev/video11");

    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. 
            if (!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;
        }

        // Copy frame to frame buffer.
        uint8_t index = g_sharedFrameIndex.load();

        Mat sourceFrameYuvMat(sourceFrame.height, sourceFrame.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);

        // Change index.
        index++;
        index = index & 0x03;
        g_sharedFrameIndex.store(index);

        // Notify tracker thread about new frame.
        unique_lock<mutex> lock(g_sharedFrameCondMutex);
        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 YUV
        cv::Scalar colorWhite(255, 128, 128); // White in YUV

        // 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"; ;
                // Put to center 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, Scalar(0, 0, 255), 2);

                std::string throtteCenter = "Throttle center : " + to_string(g_pidThrottle.center);
                putText(drawingMat, throtteCenter, Point(240, (g_outputHeight - 16)), FONT_HERSHEY_SIMPLEX, 0.9, Scalar(0, 0, 255), 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;
            }

            // Write H264 frame to V4L2 output.
            if (!rtpPusher.sendFrame(h264Frame))
            {
                g_log.print(PrintColor::RED, g_logFlag) <<
                "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
                "Can't write frame to V4L2 output." << std::endl;
            }
        }
        else // Frame buffer output.
        {
            // Convert source frame to BGR24. Use frame from previous iteration.
            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, 3);


    while(true)
    {
        // Wait frame from main thread.
        if(!g_sharedFrameCondFlag.load())
        {
            std::unique_lock<std::mutex> lock(g_sharedFrameCondMutex);
            while(!g_sharedFrameCondFlag.load())
            {
                g_sharedFrameCond.wait(lock);
            }
            lock.unlock();
        }

        // Get frame index.
        uint8_t index = g_sharedFrameIndex.load();
        index--;
        index = index & 0x03;

        // Process frame. Use frame from previous iteration.
        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(g_params.communication.crsfUdpPort, true))
        {
            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, 420000, 1000))
    {
        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;

    /// Capture channel last value.
    uint16_t captureChannelLastValue = 0;
    /// Arm channel last value.
    uint16_t armChannelLastValue = 0;

    /// Capture counter to prevent multiple capture.
    int captureCounter = 0;
    /// Cursor movement counter to prevent multiple cursor movement.
    int cursorIndexCounter = 0;
    /// Menu enable counter to prevent menu enable easily.
    int menuEnableCounter = 0;
    /// Moving tracker result counter.
    int trackingCommandCounter = 0;
    /// Throttle config mode counter.
    int throttleConfigModeCounter = 0;

    while(true)
    {
        // Read incoming data from serial port.
        const int bufferSize = 256;
        uint8_t rxBuffer[bufferSize];
        int bytesRead = 0;

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

        if (bytesRead <= 0 && !g_isArmed)
            continue;

        // Decode incoming data.
        uint32_t crsfChannels[16];
        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);
                break;
            }
        }

        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;
        }

        // Close config mode and save params to config file.
        if (crsfChannels[g_params.trackerControl.throttleChannel] < 300 && g_MenuIndex == 0 && g_configMode.load() && crsfChannels[g_params.trackerControl.yawChannel] > 1500 && cursorIndexCounter >= 50)
        {
            cursorIndexCounter = 0;
            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;
            }
        }
        else if (crsfChannels[g_params.trackerControl.throttleChannel] < 300 && !g_configMode.load() && crsfChannels[g_params.trackerControl.yawChannel] < 200 && menuEnableCounter >= 1000 && cursorIndexCounter >= 50)
        {
            // Enter config mode.
            menuEnableCounter = 0;
            cursorIndexCounter = 0;
            g_configMode.store(true);
        }
        else if (crsfChannels[g_params.trackerControl.throttleChannel] < 300 && !g_configMode.load() && crsfChannels[g_params.trackerControl.yawChannel] < 200 && menuEnableCounter <= 1000)
        {
            menuEnableCounter++;
        }

        // Update menu cursor with pitch channel max and min values.
        if (crsfChannels[g_params.trackerControl.pitchChannel] > 1500 && cursorIndexCounter >= 50 && g_MenuIndex == 0)
        {
            cursorIndexCounter = 0;
            g_cursorIndex = (g_cursorIndex - 1 + 5) % 5;
        }
        else if (crsfChannels[g_params.trackerControl.pitchChannel] < 200 && cursorIndexCounter >= 50  && g_MenuIndex == 0)
        {
            cursorIndexCounter = 0;
            g_cursorIndex = (g_cursorIndex + 1) % 5;
        }
        // Update sub menu cursor with pitch channel max and min values.
        else if (crsfChannels[g_params.trackerControl.pitchChannel] > 1500 && cursorIndexCounter >= 50 && g_MenuIndex == 1)
        {
            cursorIndexCounter = 0;
            g_subCursorIndex = (g_subCursorIndex - 1 + 7) % 7;
        }
        else if (crsfChannels[g_params.trackerControl.pitchChannel] < 200 && cursorIndexCounter >= 50 && g_MenuIndex == 1)
        {
            cursorIndexCounter = 0;
            g_subCursorIndex = (g_subCursorIndex + 1) % 7;
        }

        // Check we enter sub menu by using roll channel.
        if (crsfChannels[g_params.trackerControl.rollChannel] > 1500 && cursorIndexCounter >= 50)
        {
            cursorIndexCounter = 0;
            g_MenuIndex = 1; // Enter sub menu.
            g_subCursorIndex = 0;
        }
        else if (crsfChannels[g_params.trackerControl.rollChannel] < 200 && cursorIndexCounter >= 50)
        {
            cursorIndexCounter = 0;
            g_MenuIndex = 0; // Exit sub menu.
            g_subCursorIndex = 0;
            g_cursorIndex = 0;
        }

        // PID coefficients adjustment with 0 throttle and sub menu is open.
        if (crsfChannels[g_params.trackerControl.throttleChannel] < 300 && g_MenuIndex == 1 && g_configMode.load() && cursorIndexCounter >= 50)
        {
            // Adjust PID coefficients with in current menu item and cursor index.
            // Check if we are in sub menu.
            cursorIndexCounter = 0;

            // Update PID controller values.
            if (g_cursorIndex < 4)
            {
                switch (g_cursorIndex)
                {
                case 0: // Throttle
                    g_pidThrottle.update(g_subCursorIndex.load(), crsfChannels[g_params.trackerControl.yawChannel]);
                    break;
                case 1: // Yaw
                    g_pidYaw.update(g_subCursorIndex.load(), crsfChannels[g_params.trackerControl.yawChannel]);
                    break;
                case 2: // Roll
                    g_pidRoll.update(g_subCursorIndex.load(), crsfChannels[g_params.trackerControl.yawChannel]);
                    break;
                case 3: // Pitch
                    g_pidPitch.update(g_subCursorIndex.load(), crsfChannels[g_params.trackerControl.yawChannel]);
                    break;
                }
            }
            else if (g_cursorIndex == 4) // Config channels
            {
                switch (g_subCursorIndex)
                {
                case 0: // Capture channel
                {
                    if (crsfChannels[g_params.trackerControl.yawChannel] > 1500)
                        g_params.trackerControl.captureChannel = (g_params.trackerControl.captureChannel + 1) % 16;
                    else if (crsfChannels[g_params.trackerControl.yawChannel] < 200)
                        g_params.trackerControl.captureChannel = (g_params.trackerControl.captureChannel - 1 + 16) % 16;

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

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

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

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

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

                    break;
                }
                case 6: // is print throttle value
                {
                    if (crsfChannels[g_params.trackerControl.yawChannel] > 1500)
                        g_params.pidControl.isPrintThrottle = !g_params.pidControl.isPrintThrottle;
                    else if (crsfChannels[g_params.trackerControl.yawChannel] < 200)
                        g_params.pidControl.isPrintThrottle = !g_params.pidControl.isPrintThrottle;

                    break;
                }
                }
            }
        }

        cursorIndexCounter++;

        // Check if tracker capture channel is changed. This is just an example.
        if (crsfChannels[g_params.trackerControl.captureChannel] > g_params.trackerControl.captureChannelTriggerValue)
        {
            captureCounter++;
            uint8_t commandBuffer[32];
            int commandSize = 0;
            if (g_trackerResultMode.load() == 1 && captureCounter >= 40) // Capture and reset commands can be send min 40 cycle period
            {
                captureCounter = 0;
                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 && captureCounter >= 40)
            {
                captureCounter = 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;
                }
            }
        }

        // Arming (Auto control). Only if throttle config mode is not enabled.
        if (abs(crsfChannels[g_params.trackerControl.armChannel] - armChannelLastValue) > 500)
        {
            if (crsfChannels[g_params.trackerControl.armChannel] > g_params.trackerControl.armChannelTriggerValue && g_trackerResultMode.load() == 1)
            {
                g_isArmed = true;
                trackingCommandCounter = 0;
            }
            else if (crsfChannels[g_params.trackerControl.armChannel] < g_params.trackerControl.armChannelTriggerValue && g_isArmed)
            {
                g_isArmed = false;
            }
            armChannelLastValue = crsfChannels[g_params.trackerControl.armChannel];
        }

        // If it is armed and tracker is in tracking mode then control the drone.
        if (g_isArmed && g_trackerResultMode.load() == 1)
        {
            if (trackingCommandCounter >= 75)
            {
                // Control tracking rectangle with channels.
                if (crsfChannels[g_params.trackerControl.rollChannel] < 500)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::MOVE_RECT, -1, 0);
                else if (crsfChannels[g_params.trackerControl.rollChannel] > 1400)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::MOVE_RECT, 1, 0);

                if (crsfChannels[g_params.trackerControl.pitchChannel] < 500)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::MOVE_RECT, 0, 1);
                else if (crsfChannels[g_params.trackerControl.pitchChannel] > 1400)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::MOVE_RECT, 0, -1);

                // Change tracking rectangle size with yaw channel.
                if (crsfChannels[g_params.trackerControl.yawChannel] < 500)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::CHANGE_RECT_SIZE, -1, -1);
                else if (crsfChannels[g_params.trackerControl.yawChannel] > 1500)
                    g_tracker.executeCommand(cr::vtracker::VTrackerCommand::CHANGE_RECT_SIZE, 1, 1);
            }
            else
            {
                trackingCommandCounter++;
            }

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

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

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

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

        g_throttleValue = crsfChannels[g_params.trackerControl.throttleChannel];

        // Encoder RC channels.
        parser.encodeRcChannels(crsfChannels, packet, size);

        // Write data to serial port.
        if (serialPort.write(packet, size) != size)
        {
            g_log.print(PrintColor::RED, g_logFlag) <<
            "[" << __LOGFILENAME__ << "][" << __LINE__ << "] ERROR: " <<
            "Can't write data to serial port." << std::endl;
        }
    }
}

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: Betaflight antigravity feature should be configured properly for the drone. Throttle controller may activate antigravity feature in Betaflight. If drone configuration is not done properly, antigravity may make drone unstable under throttle controll. 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 to be copied it to different hardware. Current licensing mechanism uses 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.txt file in same directory of Zero2WFpv, it will create a token.txt file based on these hardware dependent parameters. And license.txt file can be obtained by using License library from the token that is in token.txt. License file should be placed in same directory of Zero2WFpv executable.

Overlay examples

Zero2WFpv can be in mainly 3 different states. These are:

  • Free mode: In this mode, tracker is waiting 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, tracker is tracking the object. Overlay rectangle corners are bigger than the rectangle in free mode. This state indicates that Zero2WFpv is ready to start auto control towards the object.

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

Following image shows the overlay examples for these 3 different states with same order.

zero2fpvoverlaystates

Table of contents