From 2b9bda1ff0dcf938ec4c1d4728291ed2cb5c5657 Mon Sep 17 00:00:00 2001 From: Nffj84 Date: Tue, 16 Jul 2024 18:15:10 +0300 Subject: [PATCH] Added new target location algorithm. Fixed issue with target altitude calculation. --- misc/camera/a8/config.cpp | 3 + misc/camera/a8/config.hpp | 2 +- misc/camera/a8/localControl.cpp | 3 +- misc/camera/a8/localControl.hpp | 4 +- misc/camera/a8/main.cpp | 6 ++ misc/camera/a8/remoteControl.cpp | 9 +-- misc/camera/a8/serialCommand.cpp | 4 +- misc/camera/a8/serialPort.cpp | 2 +- misc/camera/a8/serialResponse.cpp | 2 +- misc/camera/a8/utilsTargetLocation.cpp | 98 ++++++++++++++++++-------- misc/camera/a8/utilsTargetLocation.hpp | 12 ++-- 11 files changed, 98 insertions(+), 47 deletions(-) diff --git a/misc/camera/a8/config.cpp b/misc/camera/a8/config.cpp index 1810b87..5462164 100644 --- a/misc/camera/a8/config.cpp +++ b/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; diff --git a/misc/camera/a8/config.hpp b/misc/camera/a8/config.hpp index efaacf9..4d47ac8 100644 --- a/misc/camera/a8/config.hpp +++ b/misc/camera/a8/config.hpp @@ -1,8 +1,8 @@ #pragma once +#include #include "serialCommand.hpp" #include "serialPort.hpp" -#include class Config { diff --git a/misc/camera/a8/localControl.cpp b/misc/camera/a8/localControl.cpp index 8529027..3e7121f 100644 --- a/misc/camera/a8/localControl.cpp +++ b/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; diff --git a/misc/camera/a8/localControl.hpp b/misc/camera/a8/localControl.hpp index 8daa857..4872292 100644 --- a/misc/camera/a8/localControl.hpp +++ b/misc/camera/a8/localControl.hpp @@ -1,7 +1,7 @@ -#include - #pragma once +#include + class LocalControl : public QObject { Q_OBJECT diff --git a/misc/camera/a8/main.cpp b/misc/camera/a8/main.cpp index 7b3b505..ac9d923 100644 --- a/misc/camera/a8/main.cpp +++ b/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"; diff --git a/misc/camera/a8/remoteControl.cpp b/misc/camera/a8/remoteControl.cpp index 85d8a30..da76822 100644 --- a/misc/camera/a8/remoteControl.cpp +++ b/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; diff --git a/misc/camera/a8/serialCommand.cpp b/misc/camera/a8/serialCommand.cpp index 1b48867..60102e9 100644 --- a/misc/camera/a8/serialCommand.cpp +++ b/misc/camera/a8/serialCommand.cpp @@ -1,8 +1,8 @@ -#include "serialCommand.hpp" +#include #include +#include "serialCommand.hpp" #include "config.hpp" #include "utilsTargetLocation.hpp" -#include SerialCommand::SerialCommand() { diff --git a/misc/camera/a8/serialPort.cpp b/misc/camera/a8/serialPort.cpp index 396988d..7e0f16b 100644 --- a/misc/camera/a8/serialPort.cpp +++ b/misc/camera/a8/serialPort.cpp @@ -1,7 +1,7 @@ -#include "serialPort.hpp" #include #include #include +#include "serialPort.hpp" #include "defines.hpp" #include "utilsCRC16.hpp" diff --git a/misc/camera/a8/serialResponse.cpp b/misc/camera/a8/serialResponse.cpp index 638b8e9..7b36914 100644 --- a/misc/camera/a8/serialResponse.cpp +++ b/misc/camera/a8/serialResponse.cpp @@ -1,5 +1,5 @@ -#include "serialResponse.hpp" #include +#include "serialResponse.hpp" #include "defines.hpp" #include "utilsCRC16.hpp" diff --git a/misc/camera/a8/utilsTargetLocation.cpp b/misc/camera/a8/utilsTargetLocation.cpp index 579435c..fd33045 100644 --- a/misc/camera/a8/utilsTargetLocation.cpp +++ b/misc/camera/a8/utilsTargetLocation.cpp @@ -1,32 +1,27 @@ -#include "utilsTargetLocation.hpp" #include #include +#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(M_PI); + newLocation.longitude = newLonRad * 180.0f / static_cast(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; } diff --git a/misc/camera/a8/utilsTargetLocation.hpp b/misc/camera/a8/utilsTargetLocation.hpp index 591b351..e2bfee4 100644 --- a/misc/camera/a8/utilsTargetLocation.hpp +++ b/misc/camera/a8/utilsTargetLocation.hpp @@ -2,11 +2,13 @@ #include +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); };