Added new target location algorithm.

Fixed issue with target altitude calculation.

misc/camera/a8/config.cpp

@@ -50,6 +50,9 @@ void Config::setInitalValues(SerialPort *serial, SerialCommand *command)
     responseValues = SerialResponse::getResponceValues(tempResponse);
     mMaxZoom = responseValues["zoom"].toInt();
 
+    // Also update
+    updateState();
+
     // Debug printing
     qDebug().noquote().nospace() << "Camera resolution: " << mResolutionX << "*" << mResolutionY;
     qDebug().noquote().nospace() << "Camera max zoom:   " << mMaxZoom;

misc/camera/a8/config.hpp

@@ -1,8 +1,8 @@
 #pragma once
 
+#include <cstdint>
 #include "serialCommand.hpp"
 #include "serialPort.hpp"
-#include <cstdint>
 
 class Config
 {

misc/camera/a8/localControl.cpp

@@ -39,7 +39,8 @@ void LocalControl::run()
 
             if (commandId == COMMAND_ID::RUN_TARGET_LOCATION_TEST) {
                 qInfo().noquote().nospace() << "Running target location test";
-                GPSData gpsData = UtilsTargetLocation::getLocation(200.0f, 63.16122286887124f, 23.822053704379698f, 180.0f, 0.0f, 0.0f, 5.0f, 20);
+                //GPSData gpsData = UtilsTargetLocation::getLocation(200.0f, 63.16122286887124f, 23.822053704379698f, 180.0f, 0.0f, 0.0f, 5.0f, 20);
+                GPSData gpsData = UtilsTargetLocation::getLocation(200.0f, 63.16122286887124f, 23.822053704379698f, 180.0f, 0.0f, 0.0f);
                 qInfo().noquote().nospace() << "Altitude:  " << gpsData.altitude;
                 qInfo().noquote().nospace() << "Latitude:  " << gpsData.latitude;
                 qInfo().noquote().nospace() << "Longitude: " << gpsData.longitude;

misc/camera/a8/localControl.hpp

@@ -1,7 +1,7 @@
-#include <QObject>
-
 #pragma once
 
+#include <QObject>
+
 class LocalControl : public QObject
 {
     Q_OBJECT

misc/camera/a8/main.cpp

@@ -6,6 +6,7 @@
 #include "remoteControl.hpp"
 #include "serialCommand.hpp"
 #include "serialPort.hpp"
+#include "utilsTargetLocation.hpp"
 
 int main(int argc, char *argv[])
 {
@@ -31,6 +32,11 @@ int main(int argc, char *argv[])
     SerialCommand serialCommand;
     Config::setInitalValues(&serialPort, &serialCommand);
 
+    GPSData test = UtilsTargetLocation::getLocation(100.0f, 49.8397, 24.0319, 180.0f, 0.0f, 0.0f);
+    qInfo().noquote().nospace() << "altitude: " << test.altitude;
+    qInfo().noquote().nospace() << "latitude: " << test.latitude;
+    qInfo().noquote().nospace() << "longitude: " << test.longitude;
+
     // Remote mode will read commands from pipe
     if (useRemoteMode == true) {
         qDebug() << "Creating new RemoteControl object";

misc/camera/a8/remoteControl.cpp

@@ -141,17 +141,18 @@ void RemoteControl::calculateTargetPosition(QJsonObject &commandObject)
 {
     qDebug().noquote().nospace() << "Getting target location";
 
+    float altitude = commandObject["altitude"].toDouble();
     float latitude = commandObject["latitude"].toDouble();
     float longitude = commandObject["longitude"].toDouble();
-    float altitude = commandObject["altitude"].toDouble();
     float yaw = commandObject["yaw"].toDouble();
     float pitch = commandObject["pitch"].toDouble();
-    float targetPixelWidth = commandObject["target_pixel_width"].toInt();
+    //float targetPixelWidth = commandObject["target_pixel_width"].toInt();
     //float targetPixelHeight = commandObject["target_pixel_height"].toInt();
-    float targetRealWidth = commandObject["target_real_width"].toDouble();
+    //float targetRealWidth = commandObject["target_real_width"].toDouble();
     //float targetRealHeight = commandObject["target_real_height"].toDouble();
 
-    GPSData gpsData = UtilsTargetLocation::getLocation(altitude, latitude, longitude, yaw, pitch, 0.0f, targetRealWidth, targetPixelWidth);
+    //GPSData gpsData = UtilsTargetLocation::getLocation(altitude, latitude, longitude, yaw, pitch, 0.0f, targetRealWidth, targetPixelWidth);
+    GPSData gpsData = UtilsTargetLocation::getLocation(altitude, latitude, longitude, yaw, pitch, 0.0f);
     mResponseObject["altitude"] = gpsData.altitude;
     mResponseObject["latitude"] = gpsData.latitude;
     mResponseObject["longitude"] = gpsData.longitude;

misc/camera/a8/serialCommand.cpp

@@ -1,8 +1,8 @@
-#include "serialCommand.hpp"
+#include <iostream>
 #include <QDebug>
+#include "serialCommand.hpp"
 #include "config.hpp"
 #include "utilsTargetLocation.hpp"
-#include <iostream>
 
 SerialCommand::SerialCommand()
 {

misc/camera/a8/serialPort.cpp

@@ -1,7 +1,7 @@
-#include "serialPort.hpp"
 #include <QCoreApplication>
 #include <QDebug>
 #include <QTimer>
+#include "serialPort.hpp"
 #include "defines.hpp"
 #include "utilsCRC16.hpp"
 

misc/camera/a8/serialResponse.cpp

@@ -1,5 +1,5 @@
-#include "serialResponse.hpp"
 #include <QDebug>
+#include "serialResponse.hpp"
 #include "defines.hpp"
 #include "utilsCRC16.hpp"
 

misc/camera/a8/utilsTargetLocation.cpp

@@ -1,32 +1,27 @@
-#include "utilsTargetLocation.hpp"
 #include <QDebug>
 #include <QImage>
+#include "utilsTargetLocation.hpp"
 #include "config.hpp"
 #include "defines.hpp"
 
-GPSData UtilsTargetLocation::getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize)
+//GPSData UtilsTargetLocation::getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize)
+GPSData UtilsTargetLocation::getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll)
 {
     // From the drone and camera
     CameraData cameraData = getCameraData();
-    GPSData gpsData = {latitude, lognitude, altitude};
+    GPSData gpsData = {altitude, latitude, lognitude};
     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);
+    return calculateTargetLocation(droneData, cameraData);
 }
 
 CameraData UtilsTargetLocation::getCameraData()
 {
     uint16_t height = Config::getResolutionHeight();
     uint16_t width = Config::getResolutionWidth();
-    float yaw = Config::getCurrentYaw();
-    float pitch = Config::getCurrentPitch();
+    float yaw = 0 - Config::getCurrentYaw(); // Reverse value for calculation purposes
+    float pitch = 0 - Config::getCurrentPitch(); // Reverse value for calculation purposes
     float zoom = Config::getCurrentZoom();
     float fov = CAMERA_FIELD_OF_VIEW_HORIZONTAL / zoom;
 
@@ -34,11 +29,35 @@ CameraData UtilsTargetLocation::getCameraData()
 }
 
 // Function to calculate distance from pixel size and target size
-float UtilsTargetLocation::calculateTargetDistance(float targetTrueSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov)
+/*
+float UtilsTargetLocation::calculateTargetDistanceFromTargetSize(float targetTrueSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov)
 {
     float focalLength = (imageWidth / 2) / tan(fov / 2);
     return (targetTrueSize * focalLength) / targetPixelSize;
 }
+*/
+
+void UtilsTargetLocation::calculateDistancesToTarget(float altitude, float cameraPitch, float &slantDistance, float &horizontalDistance)
+{
+    // Convert pitch angle from degrees to radians
+    float cameraPitchRadians = degreesToRadians(cameraPitch);
+
+    // Value has to be between 0-90
+    cameraPitchRadians = std::clamp((double)cameraPitchRadians, (double)0.0f, M_PI_2);
+
+    // Calculate horizontal distance
+    horizontalDistance = altitude / tan(cameraPitchRadians);
+
+    // Adjust for Earth's curvature:
+    // We need to find the horizontal distance on the curved Earth surface that corresponds to this flat distance
+    double centralAngle = horizontalDistance / EARTH_RADIUS; // in radians
+
+    // Calculate the arc length on Earth's surface
+    horizontalDistance = EARTH_RADIUS * centralAngle;
+
+    // Calculate slant distance considering the Earth's curvature
+    slantDistance = sqrt(altitude * altitude + horizontalDistance * horizontalDistance);
+}
 
 // Function to convert degrees to radians
 float UtilsTargetLocation::degreesToRadians(float degrees)
@@ -47,50 +66,67 @@ float UtilsTargetLocation::degreesToRadians(float degrees)
 }
 
 // Function to calculate the new GPS location
-GPSData UtilsTargetLocation::calculateTargetLocation(DroneData drone, CameraData camera, float distance, float bearing)
+GPSData UtilsTargetLocation::calculateTargetLocation(DroneData drone, CameraData camera)
 {
-    constexpr float R = 6371000.0f; // Earth radius in meters
+    // Calculate altitude and distance to the target
+    //float targetDistance = calculateTargetDistanceFromTargetSize(targetTrueSize, targetPixelSize, cameraData.width, degreesToRadians(cameraData.fow));
+    float slantDistance = 0;
+    float horizontalDistance = 0;
+    calculateDistancesToTarget(drone.gps.altitude, camera.pitch, slantDistance, horizontalDistance);
+    qInfo().noquote().nospace() << "horizontalDistance: " << horizontalDistance;
+    qInfo().noquote().nospace() << "slantDistance: " << slantDistance;
 
-    float bearingRad = degreesToRadians(bearing);
+    // Calculate new altitude using the slant distance and angle
+    float pitchRad = degreesToRadians(camera.pitch);
+    float sinPitchRad = std::sin(pitchRad);
+    float altitudeDifference = std::round(slantDistance * sinPitchRad * 100.0) / 100.0; // Rounding to avoid weird targetAltitude
+    float targetAltitude = (std::round(drone.gps.altitude * 100.0) / 100.0) - altitudeDifference; // Rounding to avoid weird targetAltitude
+
+    // Calculate the bearing from the drone orientation and camera orientation
+    float targetBearing = std::fmod(drone.yaw + camera.yaw, 360.0f);
+
+    // Convert bearing and drone's latitude/longitude to radians
+    float bearingRad = degreesToRadians(targetBearing);
     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));
+    // Calculate new latitude using Haversine formula
+    float newLatRad = std::asin(std::sin(latRad) * std::cos(horizontalDistance / EARTH_RADIUS) + std::cos(latRad) * std::sin(horizontalDistance / EARTH_RADIUS) * std::cos(bearingRad));
 
-    float angleRad = camera.pitch * M_PI / 180.0f;
-    float horizontalDistance = distance * cos(angleRad);
-    float newAltitude = drone.gps.altitude + (horizontalDistance * tan(angleRad));
+    // Calculate new longitude using Haversine formula
+    float newLonRad = lonRad + std::atan2(std::sin(bearingRad) * std::sin(horizontalDistance / EARTH_RADIUS) * std::cos(latRad), std::cos(horizontalDistance / EARTH_RADIUS) - std::sin(latRad) * std::sin(newLatRad));
 
+    // Convert back to degrees for latitude and longitude
     GPSData newLocation;
-    newLocation.latitude = newLatRad * 180.0f / M_PI;
-    newLocation.longitude = newLonRad * 180.0f / M_PI;
-    newLocation.altitude = newAltitude;
+    newLocation.altitude = targetAltitude;
+    newLocation.latitude = newLatRad * 180.0f / static_cast<float>(M_PI);
+    newLocation.longitude = newLonRad * 180.0f / static_cast<float>(M_PI);
 
     return newLocation;
 }
 
 void UtilsTargetLocation::getAnglesToOnScreenTarget(uint16_t targetX, uint16_t targetY, float &resultYaw, float &resultPitch)
 {
+    // Get current yaw and pitch
     resultYaw = Config::getCurrentYaw();
     resultPitch = Config::getCurrentPitch();
 
     // Normalize target pixel location to [-0.5, 0.5] range
-    double normPixelX = (targetX - Config::getResolutionWidth() / 2.0f) / (Config::getResolutionWidth() / 2.0f);
-    double normPixelY = (targetY - Config::getResolutionHeight() / 2.0f) / (Config::getResolutionHeight() / 2.0f);
+    float normPixelX = (targetX - Config::getResolutionWidth() / 2.0f) / (Config::getResolutionWidth() / 2.0f);
+    float normPixelY = (targetY - Config::getResolutionHeight() / 2.0f) / (Config::getResolutionHeight() / 2.0f);
 
     // Adjust horizontal field of view for zoom
-    double horizontalFov = CAMERA_FIELD_OF_VIEW_HORIZONTAL * (1.0f + (Config::getCurrentZoom() - 1.0f) / 5.0f);
+    float horizontalFov = CAMERA_FIELD_OF_VIEW_HORIZONTAL * (1.0f + (Config::getCurrentZoom() - 1.0f) / 5.0f);
 
     // Calculate image plane dimensions based on focal length and aspect ratio
-    double imagePlaneWidth = 2.0f * CAMERA_FOCAL_LENGTH * tan(horizontalFov / 2.0f * M_PI / 180.0f);
-    double imagePlaneHeight = imagePlaneWidth / CAMERA_ASPECT_RATIO;
+    float imagePlaneWidth = 2.0f * CAMERA_FOCAL_LENGTH * tan(degreesToRadians(horizontalFov) / 2.0f);
+    float imagePlaneHeight = imagePlaneWidth / CAMERA_ASPECT_RATIO;
 
-    // Calculate angle offsets based on normalized pixel location and image plane
-    // dimensions
+    // Calculate angle offsets based on normalized pixel location and image plane dimensions
     float turnX = atan2(normPixelX * imagePlaneWidth / 2.0f, CAMERA_FOCAL_LENGTH) * 180.0f / M_PI;
     float turnY = atan2(normPixelY * imagePlaneHeight / 2.0f, CAMERA_FOCAL_LENGTH) * 180.0f / M_PI;
 
+    // Make alterations to current angles
     resultYaw -= turnX;
     resultPitch -= turnY;
 }

misc/camera/a8/utilsTargetLocation.hpp

@@ -2,11 +2,13 @@
 
 #include <cstdint>
 
+const double EARTH_RADIUS = 6371000.0; // Earth's radius in meters
+
 struct GPSData
 {
+    float altitude;  // Meters
     float latitude;  // Decimal degrees
     float longitude; // Decimal degrees
-    float altitude;  // Meters
 };
 
 struct CameraData
@@ -29,12 +31,14 @@ struct DroneData
 class UtilsTargetLocation
 {
 public:
-    static GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize);
+    //static GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize);
+    static GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll);
     static void getAnglesToOnScreenTarget(uint16_t targetX, uint16_t targetY, float &resultYaw, float &resultPitch);
 
 private:
     static CameraData getCameraData();
-    static float calculateTargetDistance(float targetSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov);
+    //static float calculateTargetDistanceFromTargetSize(float targetSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov);
+    static void calculateDistancesToTarget(float altitude, float cameraPitch, float &slantDistance, float &horizontalDistance);
     static float degreesToRadians(float degrees);
-    static GPSData calculateTargetLocation(DroneData drone, CameraData camera, float distance, float bearing);
+    static GPSData calculateTargetLocation(DroneData drone, CameraData camera);
 };