Added functionality to calculate target location.

Added functionality to capture camera frame from RTSP stream.
Refactored code.
Fixed some minor issues.

misc/camera/a8/.gitignore

@@ -72,3 +72,5 @@ CMakeLists.txt.user*
 *.dll
 *.exe
 
+# Folders
+build/

misc/camera/a8/a8.pro

@@ -1,4 +1,6 @@
 QT += core serialport
+QT += widgets
+QT -= gui
 
 CONFIG += c++17 console
 
@@ -16,9 +18,14 @@ linux-g++ {
     QMAKE_CC = clang
 }
 
-#LIBS += $$PWD/some-library.a
-
 SOURCES += *.cpp
 HEADERS += *.h
 
-#INCLUDEPATH += $$QT5_INCLUDES_DIR
+# When using FFmpeg
+#INCLUDEPATH += /usr/include/x86_64-linux-gnu
+#LIBS += -L/usr/lib/x86_64-linux-gnu -lavformat -lavcodec -lavutil -lswscale
+
+# When using OpenCV
+INCLUDEPATH += /usr/include/opencv4
+LIBS += -L/lib/x86_64-linux-gnu
+LIBS += -lopencv_core -lopencv_imgproc -lopencv_highgui -lopencv_imgcodecs -lopencv_videoio

misc/camera/a8/crc16.cpp

@@ -1,52 +0,0 @@
-#include "crc16.h"
-
-static const uint16_t crc16Table[256] = {
-    0x0,    0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108,
-    0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x210,
-    0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b,
-    0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x420,  0x1401,
-    0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee,
-    0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x630,  0x76d7, 0x66f6,
-    0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d,
-    0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x840,  0x1861, 0x2802, 0x3823,
-    0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5,
-    0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0xa50,  0x3a33, 0x2a12, 0xdbfd, 0xcbdc,
-    0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4,
-    0x5cc5, 0x2c22, 0x3c03, 0xc60,  0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd,
-    0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13,
-    0x2e32, 0x1e51, 0xe70,  0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a,
-    0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e,
-    0xe16f, 0x1080, 0xa1,   0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
-    0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x2b1,
-    0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb,
-    0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0,
-    0x481,  0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
-    0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x691,  0x16b0, 0x6657,
-    0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9,
-    0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x8e1,  0x3882,
-    0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
-    0x4a75, 0x5a54, 0x6a37, 0x7a16, 0xaf1,  0x1ad0, 0x2ab3, 0x3a92, 0xfd2e,
-    0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07,
-    0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1,  0xef1f, 0xff3e, 0xcf5d,
-    0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74,
-    0x2e93, 0x3eb2, 0xed1,  0x1ef0};
-
-uint16_t CRC16::calculate(const uint8_t *ptr, uint32_t len, uint16_t crcInit) {
-    uint16_t crc = crcInit;
-    uint8_t temp;
-
-    while (len-- != 0) {
-        temp = (crc >> 8) & 0xFF;
-        crc = (crc << 8) ^ crc16Table[*ptr ^ temp];
-        ptr++;
-    }
-
-    return crc;
-}
-
-void CRC16::getCRCBytes(const QByteArray &data, uint8_t *bytes) {
-    uint16_t crc16 = calculate(
-        reinterpret_cast<const uint8_t *>(data.constData()), data.size(), 0);
-    bytes[0] = crc16 & 0xFF;
-    bytes[1] = crc16 >> 8;
-}

misc/camera/a8/defines.h

@@ -1,12 +1,43 @@
 #pragma once
 
+enum MESSAGE_IDX { STX = 0, CTRL = 2, Data_len = 3, SEQ = 5, CMD_ID = 7, DATA = 8 };
 
-enum MESSAGE_IDX
+enum COMMAND_ID {
-{
+    TURN_TO_X = 2, // Set first to 2, because 0 is reserved for EXIT_PROGRAM and 1 for running target location test
-    STX = 0,
+    ZOOM_TO_X,
-    CTRL = 2,
+    ACQUIRE_CAMERA_CODEC_SPECS,
-    Data_len = 3,
+    ACQUIRE_CURRENT_ZOOM,
-    SEQ = 5,
+    ACQUIRE_ATTITUDE_DATA,
-    CMD_ID = 7,
+    AUTO_CENTER,
-    DATA = 8
+    ZOOM_MOST,
+    ZOOM_LEAST,
+    FOCUS_MOST,
+    FOCUS_LEAST,
+    FOCUS_AUTO,
+    ROTATE_UP,
+    ROTATE_DOWN,
+    ROTATE_RIGHT,
+    ROTATE_LEFT,
+    ROTATE_STOP,
+    ACQUIRE_MAX_ZOOM_VALUE,
+    TAKE_PICTURES,
+    TAKE_VIDEO,
+    ROTATE_100_100,
+    ACQUIRE_GIMBAL_STATUS,
+    ACQUIRE_HW_INFO,
+    ACQUIRE_FIRMWARE_VERSION,
+    MODE_LOCK,
+    MODE_FOLLOW,
+    MODE_FPV,
+    ENABLE_HDMI,
+    ENABLE_CVBS,
+    DISABLE_HDMI_CVBS,
+    ACQUIRE_RANGE_DATA,
 };
+
+#define CAMERA_FIELD_OF_VIEW_DIAGONAL 93.0
+#define CAMERA_FIELD_OF_VIEW_HORIZONTAL 81.0
+#define CAMERA_RESOLUTION_HEIGHT 1080
+#define CAMERA_RESOLUTION_WIDTH 1920
+#define SERIAL_RESPONSE_WAIT_TIME 500
+#define RTSP_ADDRESS "rtsp://192.168.144.25:8554/main.264"

misc/camera/a8/main.cpp

@@ -1,11 +1,13 @@
+#include <QCoreApplication>
+#include <QThread>
 #include "serialCommand.h"
 #include "serialPort.h"
 #include "serialResponse.h"
-#include <QCoreApplication>
+#include "utilsTargetLocation.h"
-#include <QThread>
 #include <iostream>
 
-int main(int argc, char *argv[]) {
+int main(int argc, char *argv[])
+{
     QCoreApplication app(argc, argv);
 
     // Replace with your actual port name
@@ -26,30 +28,38 @@ int main(int argc, char *argv[]) {
         commands.printCommands();
 
         // Get user input
-        std::cout << "Enter a command (0 to exit): ";
+        qInfo() << "Enter a command (0 to exit, 1 to run test): ";
         int16_t number;
         std::cin >> number;
 
         // Check if the input is within the valid range for uint8_t
         if (number < 0 || number > commands.getCommandCount() - 1) {
-            qWarning() << "Number (" << qPrintable(QString::number(number))
+            qWarning() << "Number (" << qPrintable(QString::number(number)) << ") out of range 0 -" << qPrintable(QString::number(commands.getCommandCount() - 1));
-                       << ") out of range 0 -"
-                       << qPrintable(QString::number(commands.getCommandCount() - 1));
             continue;
         }
 
         // Exit loop if user enters 0
-        if (number == 0)
+        if (number == 0) {
             break;
+        } else if (number == 1) {
+            qInfo() << "Running target location test";
 
-        // Example command to send (replace with actual Siyi A8 mini camera
+            UtilsTargetLocation locateTarget(&serial, &commands);
-        // commands)
+            GPSData gpsData = locateTarget.getLocation(200.0, 63.16122286887124, 23.822053704379698, 180.0, 0.0, 0.0, 5.0, 20);
-        QByteArray command = commands.getCommand((uint8_t)number);
+            qInfo() << "Altitude:  " << gpsData.altitude;
-        serial.sendCommand(command);
+            qInfo() << "Latitude:  " << gpsData.latitude;
+            qInfo() << "Longitude: " << gpsData.longitude;
+        } else {
+            COMMAND_ID commandId = (COMMAND_ID) number;
 
-        // Read response from the camera
+            // Example command to send (replace with actual Siyi A8 mini camera commands)
-        QByteArray response = serial.readResponse();
+            QByteArray command = commands.getCommandByID(commandId);
-        SerialResponse::printResponse(response);
+            serial.sendCommand(command);
+
+            // Read response from the camera
+            QByteArray response = serial.readResponse();
+            SerialResponse::printResponse(response);
+        }
     }
 
     // Close the serial port

misc/camera/a8/serialCommand.cpp

@@ -1,119 +1,65 @@
 #include "serialCommand.h"
-#include "crc16.h"
 #include <QDebug>
+#include "utilsCRC16.h"
 #include <iostream>
 
-SerialCommand::SerialCommand() {
+SerialCommand::SerialCommand()
+{
     /*
-  Field    Index   Bytes       Description
+Field    Index   Bytes       Description
-  STX      0       2           0x6655: starting mark. Low byte in the front
+STX      0       2           0x6655: starting mark. Low byte in the front
-  CTRL     2       1           0: need_ack (if the current data pack need “ack”)
+CTRL     2       1           0: need_ack (if the current data pack need “ack”)
-  1: ack_pack (if the current data pack is an “ack” package) 2-7: reserved
+1: ack_pack (if the current data pack is an “ack” package) 2-7: reserved
-  Data_len 3       2           Data field byte length. Low byte in the front
+Data_len 3       2           Data field byte length. Low byte in the front
-  SEQ      5       2           Frame sequence (0 ~ 65535). Low byte in the front
+SEQ      5       2           Frame sequence (0 ~ 65535). Low byte in the front
-  CMD_ID   7       1           Command ID
+CMD_ID   7       1           Command ID
-  DATA     8       Data_len    Data
+DATA     8       Data_len    Data
-  CRC16            2           CRC16 check to the complete data package. Low
+CRC16            2           CRC16 check to the complete data package. Low
-  byte in the front
+byte in the front
-  */
+*/
 
-    mSerialCommands.append({createByteArray({0x00, 0x00}), "Exit program"});
+    mSerialCommands.push_back({COMMAND_ID::TURN_TO_X, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to X"});
-    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00,
+    mSerialCommands.push_back({COMMAND_ID::ZOOM_TO_X, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x01, 0x00, 0x0F, 0x00, 0x00}), "Zoom to X"});
-                                             0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}),
+    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x20, 0x00}), "Acquire Camera Codec Specs"});
-                            "Degrees"});
+    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_CURRENT_ZOOM, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x18}), "Acquire current zoom"});
-    mSerialCommands.append(
+    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_ATTITUDE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0d}), "Acquire Attitude Data"});
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}),
+    mSerialCommands.push_back({COMMAND_ID::AUTO_CENTER, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}), "Auto Centering"});
-         "Auto Centering"});
+    mSerialCommands.push_back({COMMAND_ID::ZOOM_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x01}), "Zoom +1"});
-    mSerialCommands.append(
+    mSerialCommands.push_back({COMMAND_ID::ZOOM_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0xFF}), "Zoom -1"});
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x01}),
+    mSerialCommands.push_back({COMMAND_ID::FOCUS_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01}), "Manual Focus +1"});
-         "Zoom +1"});
+    mSerialCommands.push_back({COMMAND_ID::FOCUS_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xff}), "Manual Focus -1"});
-    mSerialCommands.append(
+    mSerialCommands.push_back({COMMAND_ID::FOCUS_AUTO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01}), "Auto Focus"});
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0xFF}),
+    mSerialCommands.push_back({COMMAND_ID::ROTATE_UP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x2D}), "Rotate Up"});
-         "Zoom -1"});
+    mSerialCommands.push_back({COMMAND_ID::ROTATE_DOWN, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, -0x2D}), "Rotate Down"});
-    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x01,
+    mSerialCommands.push_back({COMMAND_ID::ROTATE_RIGHT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x2D, 0x00}), "Rotate Right"});
-                                             0x00, 0x0F, 0x04, 0x05}),
+    mSerialCommands.push_back({COMMAND_ID::ROTATE_LEFT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, -0x2D, 0x00}), "Rotate Left"});
-                            "4.5x"});
+    mSerialCommands.push_back({COMMAND_ID::ROTATE_STOP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}), "Stop rotation"});
-    mSerialCommands.append(
+    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x16}), "Acquire the Max Zoom Value"});
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01}),
+    mSerialCommands.push_back({COMMAND_ID::TAKE_PICTURES, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00}), "Take Pictures"});
-         "Manual Focus +1"});
+    mSerialCommands.push_back({COMMAND_ID::TAKE_VIDEO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x02}), "Record Video"});
-    mSerialCommands.append(
+    mSerialCommands.push_back({COMMAND_ID::ROTATE_100_100, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x64, 0x64}), "Rotate 100 100"});
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xff}),
+    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_GIMBAL_STATUS, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0a}), "Gimbal Status Information"});
-         "Manual Focus -1"});
+    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_HW_INFO, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x02}), "Acquire Hardware ID"});
-    mSerialCommands.append(
+    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_FIRMWARE_VERSION, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01}), "Acquire Firmware Version"});
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01}),
+    mSerialCommands.push_back({COMMAND_ID::MODE_LOCK, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x03}), "Lock Mode"});
-         "Auto Focus"});
+    mSerialCommands.push_back({COMMAND_ID::MODE_FOLLOW, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x04}), "Follow Mode"});
-    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
+    mSerialCommands.push_back({COMMAND_ID::MODE_FPV, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x05}), "FPV Mode"});
-                                             0x00, 0x07, 0x00, 0x2D}),
+    mSerialCommands.push_back({COMMAND_ID::ENABLE_HDMI, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x06}), "Set Video Output as HDMI (Only available on A8 mini, restart to take effect)"});
-                            "Rotate Up"});
+    mSerialCommands.push_back({COMMAND_ID::ENABLE_CVBS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x07}), "Set Video Output as CVBS (Only available on A8 mini, restart to take effect)"});
-    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
+    mSerialCommands.push_back({COMMAND_ID::DISABLE_HDMI_CVBS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x08}), "Turn Off both CVBS and HDMI Output (Only available on A8 mini, restart to take effect)"});
-                                             0x00, 0x07, 0x00, -0x2D}),
+    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_RANGE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x15}), "Read Range from Laser Rangefinder(Low byte in the front, high byte in the back, available on ZT30)"});
-                            "Rotate Down"});
+
-    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
+    // Sort vector by COMMAND_ID
-                                             0x00, 0x07, 0x2D, 0x00}),
+    std::sort(mSerialCommands.begin(), mSerialCommands.end(), [](const Command& a, const Command& b) { return a.id < b.id; });
-                            "Rotate Right"});
-    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
-                                             0x00, 0x07, -0x2D, 0x00}),
-                            "Rotate Left"});
-    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
-                                             0x00, 0x07, 0x00, 0x00}),
-                            "Stop rotation"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x16}),
-         "Acquire the Max Zoom Value"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00}),
-         "Take Pictures"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x02}),
-         "Record Video"});
-    mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
-                                             0x00, 0x07, 0x64, 0x64}),
-                            "Rotate 100 100"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0a}),
-         "Gimbal Status Information"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x02}),
-         "Acquire Hardware ID"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01}),
-         "Acquire Firmware Version"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x03}),
-         "Lock Mode"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x04}),
-         "Follow Mode"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x05}),
-         "FPV Mode"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0d}),
-         "Acquire Attitude Data"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x06}),
-         "Set Video Output as HDMI (Only available on A8 mini, restart to take "
-         "effect)"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x07}),
-         "Set Video Output as CVBS (Only available on A8 mini, restart to take "
-         "effect)"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x08}),
-         "Turn Off both CVBS and HDMI Output (Only available on A8 mini, restart "
-         "to take effect)"});
-    mSerialCommands.append(
-        {createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x15}),
-         "Read Range from Laser Rangefinder(Low byte in the front, high byte in "
-         "the back, available on ZT30)"});
 }
 
-SerialCommand::~SerialCommand() {
+SerialCommand::~SerialCommand()
+{
     // Do something?
 }
 
-QByteArray
+QByteArray SerialCommand::createByteArray(const std::initializer_list<int>& bytes)
-SerialCommand::createByteArray(const std::initializer_list<int> &bytes) {
+{
     QByteArray byteArray;
     for (int byte : bytes) {
         byteArray.append(static_cast<char>(byte));
@@ -121,45 +67,92 @@ SerialCommand::createByteArray(const std::initializer_list<int> &bytes) {
     return byteArray;
 }
 
-void SerialCommand::printCommands() {
+void SerialCommand::printCommands()
-    uint8_t index = 0;
+{
-    foreach (Command command, mSerialCommands) {
+    for (uint8_t i = 0; i < mSerialCommands.size(); i++) {
-        qInfo().noquote() << QString::number(index) + ": " + command.description;
+        qInfo().noquote() << QString::number(mSerialCommands.at(i).id) + ": " + mSerialCommands.at(i).description;
-        index++;
     }
 }
 
-void SerialCommand::setExtraValues(uint8_t number) {
+void SerialCommand::setExtraValues(COMMAND_ID commandId)
-    if (number == 1) {
+{
-        QByteArray command = mSerialCommands.at(number).command;
+    uint16_t commandIndex = getCommandIndex(commandId);
-        int16_t degrees;
+    QByteArray command = mSerialCommands.at(commandIndex).command;
 
-        std::cout << "Enter yaw degrees (-135 -> 135): ";
+    if (commandId == COMMAND_ID::TURN_TO_X) {
+        float degrees;
+
+        qInfo() << "Enter yaw degrees (-135.0 -> 135.0): ";
         std::cin >> degrees;
-        degrees = degrees * 10;
-        command[8] = degrees & 0xFF;
-        command[9] = degrees >> 8;
 
-        std::cout << "Enter pitch degrees (-90 -> 25): ";
+        if (std::cin.fail() || degrees < -135.0f || degrees > 135.0f) {
+            qWarning() << "Invalid value, using 0.0";
+            degrees = 0.0f;
+        }
+
+        int16_t degreesVal = degrees * 10;
+        command[8] = degreesVal & 0xFF;
+        command[9] = degreesVal >> 8;
+
+        qInfo() << "Enter pitch degrees (-90.0 -> 25.0): ";
         std::cin >> degrees;
-        degrees = degrees * 10;
-        command[10] = degrees & 0xFF;
-        command[11] = degrees >> 8;
 
-        mSerialCommands[number].command = command;
+        if (std::cin.fail() || degrees < -90.0f || degrees > 25.0f) {
+            qWarning() << "Invalid value, using 0.0";
+            degrees = 0.0f;
+        }
+
+        degreesVal = degrees * 10;
+        command[10] = degreesVal & 0xFF;
+        command[11] = degreesVal >> 8;
+    } else if (commandId == COMMAND_ID::ZOOM_TO_X) {
+        float zoom;
+
+        qInfo() << "Enter zoom (1.0 -> 6.0): ";
+        std::cin >> zoom;
+
+        if (std::cin.fail() || zoom < 1.0 || zoom > 6.0) {
+            qWarning() << "Invalid value, using 1.0";
+            zoom = 1.0f;
+        }
+
+        uint8_t integerPart = static_cast<uint8_t>(zoom);
+        float fractionalPart = zoom - integerPart;
+        uint8_t scaledFractional = int(fractionalPart * 10);
+        command[8] = integerPart;
+        command[9] = scaledFractional;
     }
+
+    mSerialCommands[commandIndex].command = command;
 }
 
-QByteArray SerialCommand::getCommand(uint8_t number) {
+uint8_t SerialCommand::getCommandCount()
-    setExtraValues(number);
+{
+    return mSerialCommands.size();
+}
 
-    QByteArray command = mSerialCommands.at(number).command;
+int16_t SerialCommand::getCommandIndex(COMMAND_ID commandId)
+{
+    for (uint16_t i = 0; i < mSerialCommands.size(); i++) {
+        if (mSerialCommands.at(i).id == commandId) {
+            return i;
+        }
+    }
 
-    uint8_t crcBytes[2];
+    return -1;
-    CRC16::getCRCBytes(command, crcBytes);
+}
 
-    command.resize(command.size() +
+QByteArray SerialCommand::getCommandByID(COMMAND_ID commandId)
-                   2); // Increase array size to accommodate CRC bytes
+{
+    setExtraValues(commandId);
+
+    uint16_t commandIndex = getCommandIndex(commandId);
+    QByteArray command = mSerialCommands.at(commandIndex).command;
+
+    int8_t crcBytes[2];
+    UtilsCRC16::getCRCBytes(command, crcBytes);
+
+    command.resize(command.size() + 2);        // Increase array size to accommodate CRC bytes
     command[command.size() - 2] = crcBytes[0]; // Set LSB
     command[command.size() - 1] = crcBytes[1]; // Set MSB
 
@@ -171,5 +164,3 @@ QByteArray SerialCommand::getCommand(uint8_t number) {
 
     return command;
 }
-
-uint8_t SerialCommand::getCommandCount() { return mSerialCommands.size(); }

misc/camera/a8/serialCommand.h

@@ -1,9 +1,12 @@
 #pragma once
 
+#include <QByteArray>
 #include <QList>
 #include <QString>
+#include "defines.h"
 
 struct Command {
+    COMMAND_ID id;
     QByteArray command;
     QString description;
 };
@@ -12,12 +15,13 @@ class SerialCommand {
 public:
     SerialCommand();
     ~SerialCommand();
-    QByteArray createByteArray(const std::initializer_list<int> &bytes);
     void printCommands(void);
-    QByteArray getCommand(uint8_t number);
+    QByteArray getCommandByID(COMMAND_ID commandId);
-    void setExtraValues(uint8_t number);
     uint8_t getCommandCount();
 
 private:
-    QList<Command> mSerialCommands;
+    void setExtraValues(COMMAND_ID commandId);
+    QByteArray createByteArray(const std::initializer_list<int> &bytes);
+    int16_t getCommandIndex(COMMAND_ID commandId);
+    std::vector<Command> mSerialCommands;
 };

misc/camera/a8/serialPort.cpp

@@ -1,7 +1,10 @@
 #include "serialPort.h"
 #include <QDebug>
+#include "defines.h"
 
-SerialPort::SerialPort(const QString &portName) : mSerialPort(portName) {
+SerialPort::SerialPort(const QString &portName)
+    : mSerialPort(portName)
+{
     mSerialPort.setPortName(portName);
     mSerialPort.setBaudRate(QSerialPort::Baud115200);
     mSerialPort.setDataBits(QSerialPort::Data8);
@@ -9,11 +12,13 @@ SerialPort::SerialPort(const QString &portName) : mSerialPort(portName) {
     mSerialPort.setFlowControl(QSerialPort::NoFlowControl);
 }
 
-SerialPort::~SerialPort() {
+SerialPort::~SerialPort()
+{
     closePort(); // Close port if open on destruction
 }
 
-bool SerialPort::openPort() {
+bool SerialPort::openPort()
+{
     if (mSerialPort.isOpen()) {
         qDebug() << "Port already open";
         return true;
@@ -22,13 +27,15 @@ bool SerialPort::openPort() {
     return mSerialPort.open(QIODevice::ReadWrite);
 }
 
-void SerialPort::closePort() {
+void SerialPort::closePort()
+{
     if (mSerialPort.isOpen()) {
         mSerialPort.close();
     }
 }
 
-void SerialPort::sendCommand(const QByteArray &command) {
+void SerialPort::sendCommand(const QByteArray &command)
+{
     if (!mSerialPort.isOpen()) {
         qDebug() << "Error: Port not open";
         return;
@@ -37,7 +44,8 @@ void SerialPort::sendCommand(const QByteArray &command) {
     mSerialPort.write(command);
 }
 
-QByteArray SerialPort::readResponse() {
+QByteArray SerialPort::readResponse()
+{
     if (!mSerialPort.isOpen()) {
         qDebug() << "Error: Port not open";
         return QByteArray();
@@ -45,7 +53,7 @@ QByteArray SerialPort::readResponse() {
 
     // Read data from serial port until timeout or specific criteria met
     QByteArray response;
-    while (mSerialPort.waitForReadyRead(5000)) { // Adjust timeout as needed
+    while (mSerialPort.waitForReadyRead(SERIAL_RESPONSE_WAIT_TIME)) { // Adjust timeout as needed
         response.append(mSerialPort.readAll());
     }
 

misc/camera/a8/serialPort.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <QString>
+#include <QByteArray>
 #include <QSerialPort>
 
 class SerialPort {

misc/camera/a8/serialResponse.cpp

@@ -1,48 +1,110 @@
 #include "serialResponse.h"
-#include "crc16.h"
-#include "defines.h"
 #include <QDebug>
-#include <QString>
+#include "defines.h"
+#include "utilsCRC16.h"
 
-void SerialResponse::printResponse(QByteArray response) {
+void SerialResponse::printResponse(QByteArray response)
-    QString responseStr;
+{
-    for (int i = 0; i < response.size(); i++) {
+    if (response.size() == 0) {
-        responseStr +=
+        qWarning().noquote() << "Response is empty";
-            QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
-    }
-    qDebug() << "Response: " << responseStr;
-
-    responseStr = "";
-    uint8_t command = response.at(MESSAGE_IDX::CMD_ID);
-
-    // Check response data validity
-    uint8_t crcCheck[2];
-    CRC16::getCRCBytes(response.mid(0, response.size() - 2), crcCheck);
-
-    // Data not OK
-    if (crcCheck[0] != response.at(response.size() - 2) ||
-        crcCheck[1] != response.at(response.size() - 1)) {
-        qWarning() << "Response data not valid";
         return;
     }
-    qInfo() << "Response data is valid";
 
-    if (command == 0x0E) {
+    QHash<QString, QVariant> results = getResponceValues(response);
-        int16_t yaw = (static_cast<uint8_t>(response.at(9)) << 8) |
-                      static_cast<uint8_t>(response.at(8));
-        int16_t pitch = (static_cast<uint8_t>(response.at(11)) << 8) |
-                        static_cast<uint8_t>(response.at(10));
-        int16_t roll = (static_cast<uint8_t>(response.at(13)) << 8) |
-                       static_cast<uint8_t>(response.at(12));
 
-        qInfo().noquote() << "Yaw:   " << QString::number(yaw / 10) + "°";
+    QList<QString> keys = results.keys();
-        qInfo().noquote() << "Pitch: " << QString::number(pitch / 10) + "°";
+    std::sort(keys.begin(), keys.end());
-        qInfo().noquote() << "Roll:  " << QString::number(roll / 10) + "°";
+
-    } else {
+    // Loop through the sorted keys
-        for (int i = 0; i < response.size(); i++) {
+    for (const QString &key : keys) {
-            responseStr +=
+        if (results.value(key).typeId() == QMetaType::UInt) {
-                QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
+            qInfo().noquote().nospace() << key << ": " << results.value(key).toUInt();
+        } else if (results.value(key).typeId() == QMetaType::Int) {
+            qInfo().noquote().nospace() << key << ": " << results.value(key).toInt();
+        } else if (results.value(key).typeId() == QMetaType::Double) {
+            qInfo().noquote().nospace() << key << ": " << results.value(key).toDouble();
+        } else if (results.value(key).typeId() == QMetaType::Float) {
+            qInfo().noquote().nospace() << key << ": " << results.value(key).toFloat();
+        } else if (results.value(key).typeId() == QMetaType::Bool) {
+            qInfo().noquote().nospace() << key << ": " << results.value(key).toBool();
+        } else if (results.value(key).typeId() == QMetaType::QString) {
+            qInfo().noquote().nospace() << key << ": " << results.value(key).toString();
+        } else {
+            qInfo().noquote().nospace() << key << ": " << results.value(key);
         }
-        qInfo() << "Response: " << responseStr;
     }
 }
+
+QHash<QString, QVariant> SerialResponse::getResponceValues(QByteArray response)
+{
+    // Check response data validity
+    int8_t crcCheck[2];
+    uint8_t desiredLength = response.size() - 2;
+    QByteArray subData(response.data(), desiredLength);
+    UtilsCRC16::getCRCBytes(subData, crcCheck);
+
+    int8_t crcOriginal[2];
+    crcOriginal[0] = response.at(response.size() - 2);
+    crcOriginal[1] = response.at(response.size() - 1);
+
+    // Data not OK
+    if (crcCheck[0] != crcOriginal[0] || crcCheck[1] != crcOriginal[1]) {
+        qWarning() << "Response data INVALID";
+        QString responseCRC = QString("0x%1,0x%2").arg(crcOriginal[0], 2, 16, QLatin1Char('0')).arg(crcOriginal[1], 2, 16, QLatin1Char('0')).toUpper();
+        QString recalcCRC = QString("0x%1,0x%2").arg(crcCheck[0], 2, 16, QLatin1Char('0')).arg(crcCheck[1], 2, 16, QLatin1Char('0')).toUpper();
+        qWarning().noquote() << responseCRC << "!=" << recalcCRC;
+    }
+
+    QHash<QString, QVariant> results;
+    uint8_t command = response.at(MESSAGE_IDX::CMD_ID);
+
+    if (command == 0x0E) {
+        int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8);
+        results.insert("yaw", (float) (yaw / 10));
+
+        int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
+        results.insert("pitch", (float) (pitch / 10));
+
+        int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
+        results.insert("roll", (float) (roll / 10));
+    } else if (command == 0x0D) {
+        int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8);
+        results.insert("yaw", (float) (yaw / 10));
+
+        int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
+        results.insert("pitch", (float) (pitch / 10));
+
+        int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
+        results.insert("roll", (float) (roll / 10));
+
+        int16_t yaw_speed = ((uint8_t) response.at(15) << 8) | (uint8_t) response.at(14);
+        results.insert("yaw_speed", (float) (yaw_speed / 10));
+
+        int16_t pitch_speed = ((uint8_t) response.at(17) << 8) | (uint8_t) response.at(16);
+        results.insert("pitch_speed", (float) (pitch_speed / 10));
+
+        int16_t roll_speed = ((uint8_t) response.at(19) << 8) | (uint8_t) response.at(18);
+        results.insert("roll_speed", (float) (roll_speed / 10));
+    } else if (command == 0x0F) {
+        int8_t zoom = (int8_t) response.at(8);
+        results.insert("zoom", zoom);
+    } else if (command == 0x18) {
+        float zoomInt = (float) response.at(8);
+        float zoomFloat = (float) ((float) response.at(9) / 10);
+        results.insert("zoom", zoomInt + zoomFloat);
+    } else if (command == 0x20) {
+        uint16_t width = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
+        results.insert("width", width);
+
+        uint16_t height = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
+        results.insert("height", height);
+    } else {
+        QString responseStr;
+        for (int i = 0; i < response.size(); i++) {
+            responseStr += QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
+        }
+        results.insert("Response", responseStr);
+    }
+
+    return results;
+}

misc/camera/a8/serialResponse.h

@@ -1,8 +1,11 @@
 #pragma once
 
 #include <QByteArray>
+#include <QVariant>
 
-class SerialResponse {
+class SerialResponse
+{
 public:
     static void printResponse(QByteArray response);
+    static QHash<QString, QVariant> getResponceValues(QByteArray response);
 };

misc/camera/a8/utilsCRC16.cpp

@@ -0,0 +1,28 @@
+#include "utilsCRC16.h"
+
+static const uint16_t crc16Table[256] = {0x0,    0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x210,  0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x420,  0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x630,
+                                         0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x840,  0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0xa50,  0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0xc60,  0x1c41,
+                                         0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0xe70,  0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0xa1,   0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x2b1,  0x1290, 0x22f3, 0x32d2,
+                                         0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x481,  0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x691,  0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x8e1,  0x3882, 0x28a3,
+                                         0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0xaf1,  0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1,  0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0xed1,  0x1ef0};
+
+uint16_t UtilsCRC16::calculate(const uint8_t *ptr, uint32_t len, uint16_t crcInit)
+{
+    uint16_t crc = crcInit;
+    uint8_t temp;
+
+    while (len-- != 0) {
+        temp = (crc >> 8) & 0xFF;
+        crc = (crc << 8) ^ crc16Table[*ptr ^ temp];
+        ptr++;
+    }
+
+    return crc;
+}
+
+void UtilsCRC16::getCRCBytes(const QByteArray &data, int8_t *bytes)
+{
+    uint16_t crc16 = calculate(reinterpret_cast<const uint8_t *>(data.constData()), data.size(), 0);
+    bytes[0] = crc16 & 0xFF; // Set LSB
+    bytes[1] = crc16 >> 8;   // Set MSB
+}

misc/camera/a8/utilsCRC16.h

@@ -1,11 +1,11 @@
 #pragma once
 
 #include <QByteArray>
-#include <cstdint>
 
-class CRC16 {
+class UtilsCRC16
+{
 public:
-    static void getCRCBytes(const QByteArray &data, uint8_t *bytes);
+    static void getCRCBytes(const QByteArray &data, int8_t *bytes);
 
 private:
     static uint16_t calculate(const uint8_t *ptr, uint32_t len, uint16_t crcInit);

misc/camera/a8/utilsTargetLocation.cpp

@@ -0,0 +1,284 @@
+#include "utilsTargetLocation.h"
+#include <QDebug>
+#include <QImage>
+#include "serialCommand.h"
+#include "serialResponse.h"
+
+#define USE_OPENCV
+//#define USE_FFMPEG
+
+#ifdef USE_OPENCV
+#include <opencv2/opencv.hpp>
+#endif
+
+#ifdef USE_FFMPEG
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libavutil/imgutils.h>
+#include <libswscale/swscale.h>
+}
+#endif
+
+UtilsTargetLocation::UtilsTargetLocation(SerialPort *serial, SerialCommand *command)
+    : mSerial(serial)
+    , mCommand(command)
+{
+    // ... Optional: Additional initialization logic here
+}
+
+GPSData UtilsTargetLocation::getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize)
+{
+    if (mSerial == nullptr) {
+        qWarning() << "Cannot get geo location: Serial connection not available.";
+    }
+
+    if (mCommand == nullptr) {
+        qWarning() << "Cannot get geo location: Commands not available.";
+    }
+
+    // Capture image from rtsp stream
+    captureImageFromStream();
+
+    // TODO?
+    // Send image to AI for target identification and/or target choosing
+
+    // From the drone and camera
+    CameraData cameraData = getCameraData();
+    GPSData gpsData = {latitude, lognitude, altitude};
+    DroneData droneData = {gpsData, yaw, pitch, roll}; // GPS (latitude, longitude, altitude) and heading
+
+    // Calculate altitude and distance to the target
+    float targetDistance = calculateTargetDistance(targetTrueSize, targetPixelSize, cameraData.width, degreesToRadians(cameraData.fow));
+
+    // Calculate the bearing from the drone orientation and camera orientation
+    float targetBearing = fmod(droneData.yaw + cameraData.yaw, 360.0f);
+
+    // Calculate the GPS location of the target
+    return calculateTargetLocation(droneData, cameraData, targetDistance, targetBearing);
+}
+
+CameraData UtilsTargetLocation::getCameraData()
+{
+    uint16_t height = CAMERA_RESOLUTION_HEIGHT;
+    uint16_t width = CAMERA_RESOLUTION_WIDTH;
+    float pitch = 0;
+    float yaw = 0;
+    float fov = CAMERA_FIELD_OF_VIEW_HORIZONTAL;
+
+    QByteArray response;
+    QHash<QString, QVariant> responseValues;
+    mSerial->sendCommand(mCommand->getCommandByID(COMMAND_ID::ACQUIRE_CURRENT_ZOOM)); // Field of view in degrees
+    response = mSerial->readResponse();
+    responseValues = SerialResponse::getResponceValues(response);
+    SerialResponse::printResponse(response);
+    fov = CAMERA_FIELD_OF_VIEW_HORIZONTAL / (responseValues["zoom"].toFloat() == 0.0 ? 1.0 : responseValues["zoom"].toFloat());
+
+    mSerial->sendCommand(mCommand->getCommandByID(COMMAND_ID::ACQUIRE_ATTITUDE_DATA)); // Yaw and pitch in degrees
+    response = mSerial->readResponse();
+    responseValues = SerialResponse::getResponceValues(response);
+    SerialResponse::printResponse(response);
+    yaw = responseValues["yaw"].toFloat();
+    pitch = responseValues["pitch"].toFloat();
+
+    mSerial->sendCommand(mCommand->getCommandByID(COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS)); // Camera resolution in pixels
+    response = mSerial->readResponse();
+    responseValues = SerialResponse::getResponceValues(response);
+    SerialResponse::printResponse(response);
+    height = responseValues["height"].toUInt();
+    width = responseValues["width"].toUInt();
+
+    return {height, width, pitch, yaw, fov};
+}
+
+// Function to calculate distance from pixel size and target size
+float UtilsTargetLocation::calculateTargetDistance(float targetTrueSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov)
+{
+    float focalLength = (imageWidth / 2) / tan(fov / 2);
+    return (targetTrueSize * focalLength) / targetPixelSize;
+}
+
+// Function to convert degrees to radians
+float UtilsTargetLocation::degreesToRadians(float degrees)
+{
+    return degrees * M_PI / 180.0f;
+}
+
+// Function to calculate the new GPS location
+GPSData UtilsTargetLocation::calculateTargetLocation(DroneData drone, CameraData camera, float distance, float bearing)
+{
+    constexpr float R = 6371000.0; // Earth radius in meters
+
+    float bearingRad = degreesToRadians(bearing);
+    float latRad = degreesToRadians(drone.gps.latitude);
+    float lonRad = degreesToRadians(drone.gps.longitude);
+
+    float newLatRad = asin(sin(latRad) * cos(distance / R) + cos(latRad) * sin(distance / R) * cos(bearingRad));
+    float newLonRad = lonRad + atan2(sin(bearingRad) * sin(distance / R) * cos(latRad), cos(distance / R) - sin(latRad) * sin(newLatRad));
+
+    float angleRad = camera.pitch * M_PI / 180.0;
+    float horizontalDistance = distance * cos(angleRad);
+    float newAltitude = drone.gps.altitude + (horizontalDistance * tan(angleRad));
+
+    GPSData newLocation;
+    newLocation.latitude = newLatRad * 180.0f / M_PI;
+    newLocation.longitude = newLonRad * 180.0f / M_PI;
+    newLocation.altitude = newAltitude;
+
+    return newLocation;
+}
+
+void UtilsTargetLocation::captureImageFromStream()
+{
+#ifdef USE_OPENCV
+    cv::VideoCapture cap;
+    cap.open(RTSP_ADDRESS);
+
+    if (!cap.isOpened()) {
+        qWarning() << "Error: Could not open RTSP stream";
+        return;
+    }
+
+    // Hack to capture proper image
+    // For some reason first frames get corrupted
+    uint8_t frameCount = 0;
+
+    while (true) {
+        cv::Mat frame;
+
+        if (cap.grab() && cap.retrieve(frame)) {
+            if (frame.empty() || frame.total() == 0) {
+                qInfo() << "Warning: Invalid frame captured, retrying...";
+            } else {
+                // Convert the frame to BGR if needed
+                if (frame.channels() == 1) {
+                    qInfo() << "Captured frame is grayscale, converting to BGR";
+                    cv::cvtColor(frame, frame, cv::COLOR_GRAY2BGR);
+                } else if (frame.channels() == 4) {
+                    qInfo() << "Captured frame is RGBA, converting to BGR";
+                    cv::cvtColor(frame, frame, cv::COLOR_RGBA2BGR);
+                }
+
+                frameCount++;
+                if (frameCount > 10) {
+                    // Save the captured frame as an image
+                    std::string filename = "output_opencv.jpg";
+                    if (cv::imwrite(filename, frame)) {
+                        qInfo() << "Image captured and saved as" << QString::fromStdString(filename);
+                        break;
+                    } else {
+                        qInfo() << "Error: Failed to save the captured image";
+                    }
+                } else if (frameCount > 100) {
+                    qInfo() << "Error: Tried 100 times and still fails, giving up";
+                    break;
+                }
+            }
+        } else {
+            qInfo() << "Warning: Could not grab or retrieve frame, retrying...";
+        }
+    }
+
+    // Release the video capture object
+    cap.release();
+#endif
+
+#ifdef USE_FFMPEG
+    // Initialize FFmpeg
+    avformat_network_init();
+
+    // Open RTSP stream
+    AVFormatContext *formatContext = avformat_alloc_context();
+    if (avformat_open_input(&formatContext, RTSP_ADDRESS, NULL, NULL) != 0) {
+        qDebug() << "Error: Couldn't open RTSP stream";
+        return;
+    }
+
+    // Find stream info
+    if (avformat_find_stream_info(formatContext, NULL) < 0) {
+        qDebug() << "Error: Couldn't find stream info";
+        return;
+    }
+
+    // Find video stream
+    int videoStreamIndex = -1;
+    for (uint i = 0; i < formatContext->nb_streams; i++) {
+        if (formatContext->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_VIDEO) {
+            videoStreamIndex = i;
+            break;
+        }
+    }
+    if (videoStreamIndex == -1) {
+        qDebug() << "Error: Couldn't find video stream";
+        return;
+    }
+
+    // Initialize video decoder
+    AVCodecParameters *codecParameters = formatContext->streams[videoStreamIndex]->codecpar;
+    AVCodec *codec = avcodec_find_decoder(codecParameters->codec_id);
+    AVCodecContext *codecContext = avcodec_alloc_context3(codec);
+    avcodec_parameters_to_context(codecContext, codecParameters);
+    avcodec_open2(codecContext, codec, NULL);
+
+    // Allocate frame
+    AVFrame *frame = av_frame_alloc();
+
+    bool shouldBreak = false;
+
+    // Read frames
+    AVPacket packet;
+
+    while (shouldBreak == false && av_read_frame(formatContext, &packet) >= 0) {
+        if (packet.stream_index == videoStreamIndex) {
+            if (avcodec_send_packet(codecContext, &packet) >= 0) {
+                while (avcodec_receive_frame(codecContext, frame) >= 0) {
+                    // Convert frame to RGB if necessary
+                    if (codecParameters->format != AV_PIX_FMT_RGB24) {
+                        AVFrame *rgbFrame = av_frame_alloc();
+                        int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, codecParameters->width, codecParameters->height, 1);
+                        uint8_t *buffer = (uint8_t *) av_malloc(numBytes * sizeof(uint8_t));
+                        if (av_image_fill_arrays(rgbFrame->data, rgbFrame->linesize, buffer, AV_PIX_FMT_RGB24, codecParameters->width, codecParameters->height, 1) > 0) {
+                            // Convert format to enum AVPixelFormat
+                            const AVPixFmtDescriptor *pixDesc = av_pix_fmt_desc_get((AVPixelFormat) codecParameters->format);
+                            if (!pixDesc) {
+                                qDebug() << "Error: Unsupported pixel format";
+                                return;
+                            }
+
+                            // Get the AVPixelFormat enum value directly from the
+                            descriptor AVPixelFormat pixelFormat = (AVPixelFormat) codecParameters->format;
+
+                            // Create the sws context
+                            struct SwsContext *swsContext = sws_getContext(codecParameters->width, codecParameters->height, pixelFormat, codecParameters->width, codecParameters->height, AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL);
+                            sws_scale(swsContext, frame->data, frame->linesize, 0, codecParameters->height, rgbFrame->data, rgbFrame->linesize);
+                            sws_freeContext(swsContext);
+
+                            // Save frame as JPEG
+                            QImage image(rgbFrame->data[0], codecParameters->width, codecParameters->height, QImage::Format_RGB888);
+                            image.save("output_ffmpeg.jpg");
+
+                            av_free(buffer);
+                            av_frame_free(&rgbFrame);
+                            shouldBreak = true;
+                            break;
+                        }
+                    } else {
+                        // Save frame as JPEG
+                        QImage image(frame->data[0], codecParameters->width, codecParameters->height, QImage::Format_RGB888);
+                        image.save("output_ffmpeg.jpg");
+                        shouldBreak = true;
+                        break;
+                    }
+                }
+            }
+        }
+
+        av_packet_unref(&packet);
+    }
+
+    // Clean up
+    avformat_close_input(&formatContext);
+    avcodec_free_context(&codecContext);
+    av_frame_free(&frame);
+#endif
+}

misc/camera/a8/utilsTargetLocation.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#include "serialCommand.h"
+#include "serialPort.h"
+
+#include <cstdint>
+
+struct GPSData
+{
+    float latitude;  // Decimal degrees
+    float longitude; // Decimal degrees
+    float altitude;  // Meters
+};
+
+struct CameraData
+{
+    uint16_t height; // Pixels
+    uint16_t width;  // Pixels
+    float pitch;     // Degrees
+    float yaw;       // Degrees
+    float fow;       // Degrees
+};
+
+struct DroneData
+{
+    GPSData gps;
+    float yaw;   // Degrees
+    float pitch; // Degrees
+    float roll;  // Degrees
+};
+
+class UtilsTargetLocation
+{
+public:
+    UtilsTargetLocation(SerialPort *serial, SerialCommand *command);
+    GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize);
+
+private:
+    CameraData getCameraData();
+    float calculateTargetDistance(float targetSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov);
+    float degreesToRadians(float degrees);
+    GPSData calculateTargetLocation(DroneData drone, CameraData camera, float distance, float bearing);
+    void captureImageFromStream();
+
+    SerialPort *mSerial;
+    SerialCommand *mCommand;
+};