From 6b95f9da6e67fee92f61643b91b20b4808bd19f8 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Fri, 22 Nov 2024 08:44:55 -0700 Subject: [PATCH 1/3] AP_Soaring: Use constexpr instead of define for constants Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- libraries/AP_Soaring/AP_Soaring.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index 4d13ef8e797d6..f9dd8b17c8073 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -23,10 +23,10 @@ #include "Variometer.h" #include "SpeedToFly.h" -#define INITIAL_THERMAL_RADIUS 80.0 -#define INITIAL_STRENGTH_COVARIANCE 0.0049 -#define INITIAL_RADIUS_COVARIANCE 400.0 -#define INITIAL_POSITION_COVARIANCE 400.0 +static constexpr float INITIAL_THERMAL_RADIUS = 80.0; +static constexpr float INITIAL_STRENGTH_COVARIANCE = 0.0049; +static constexpr float INITIAL_RADIUS_COVARIANCE = 400.0; +static constexpr float INITIAL_POSITION_COVARIANCE = 400.0; class SoaringController { From 5254cbe6710938f10b43f640e291f1cab113fe8f Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Fri, 22 Nov 2024 14:20:39 -0700 Subject: [PATCH 2/3] Implement AP_TopoLift library Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- Tools/ardupilotwaf/ardupilotwaf.py | 1 + libraries/AP_Terrain/AP_Terrain.h | 6 +++ libraries/AP_TopoLift/AP_TopoLift.h | 22 ++++++++ .../AP_TopoLiftSinglePointEstimate.cpp | 54 +++++++++++++++++++ .../AP_TopoLiftSinglePointEstimate.h | 27 ++++++++++ libraries/AP_TopoLift/AP_TopoLift_config.h | 3 ++ libraries/AP_TopoLift/README.md | 9 ++++ libraries/AP_Vehicle/AP_Vehicle.h | 9 ++++ libraries/SITL/SIM_Aircraft.cpp | 15 +++++- libraries/SITL/SIM_Aircraft.h | 12 +++++ 10 files changed, 157 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_TopoLift/AP_TopoLift.h create mode 100644 libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp create mode 100644 libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h create mode 100644 libraries/AP_TopoLift/AP_TopoLift_config.h create mode 100644 libraries/AP_TopoLift/README.md diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 880d1978dff09..2a286c9d9405d 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -118,6 +118,7 @@ 'AP_VideoTX', 'AP_FETtecOneWire', 'AP_TemperatureSensor', + 'AP_TopoLift', 'AP_Torqeedo', 'AP_CustomRotations', 'AP_AIS', diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index b0cbe826ab761..df9eb458e184b 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -125,6 +125,12 @@ class AP_Terrain { */ bool height_amsl(const Location &loc, float &height, bool corrected = true); + /* + find the terrain gradient (slope) for a location + + return false if not available + */ + bool gradient(const Location &loc); /* find difference between home terrain height and the terrain height at the current location in meters. A positive result diff --git a/libraries/AP_TopoLift/AP_TopoLift.h b/libraries/AP_TopoLift/AP_TopoLift.h new file mode 100644 index 0000000000000..38156d5f076ff --- /dev/null +++ b/libraries/AP_TopoLift/AP_TopoLift.h @@ -0,0 +1,22 @@ +#pragma once + +#include +#include + + +// The front end for the AP_TopoLift library. +class AP_TopoLift { + public: + + // Get the terrain height of a point. + virtual bool get_terrain_height(const Location loc, float& height_amsl) = 0; + + // // Calculate the wind rate of topographic influenced lift at the current location. + virtual bool calc_lift(const Location loc, float& lift) = 0; + + // Get the wind estimate at the current location. + virtual bool get_wind_at_current_loc(Vector3f& wind) = 0; + + // // Update state. + // virtual void update() = 0; +}; \ No newline at end of file diff --git a/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp b/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp new file mode 100644 index 0000000000000..9cb3c120ff6e4 --- /dev/null +++ b/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp @@ -0,0 +1,54 @@ +#include "AP_TopoLiftSinglePointEstimate.h" +#include +#include +#include + + +bool AP_TopoLiftSinglePointEstimate::get_terrain_height(const Location loc, float& height_amsl) { + + AP_Terrain *terrain = AP::terrain(); + if (terrain == nullptr) { + return false; + } + + // status() handles enable check. + if (terrain->status() != AP_Terrain::TerrainStatusOK) { + return false; + } + + const auto corrected = true; + return terrain->height_amsl(loc, height_amsl, corrected); + + + return true; +} + +bool AP_TopoLiftSinglePointEstimate::get_wind_at_current_loc(Vector3f& wind) { + const AP_AHRS &ahrs = AP::ahrs(); + + return ahrs.wind_estimate(wind); +} + +bool AP_TopoLiftSinglePointEstimate::calc_lift(const Location loc, float& lift) { + float height_amsl; + if (!get_terrain_height(loc, height_amsl)) { + return false; + } + + Vector3f wind; + if (!get_wind_at_current_loc(wind)) { + return false; + } + + // TODO calculate gradient of terrain. + Vector2f terrain_grad; + + // TODO find a way to communicate steepness + [[maybe_unused]] auto const terrain_steepness = terrain_grad.length(); + + + // TODO compute lift based on gradient, wind, and altitude above terrain. + lift = 42.0; + + return true; +} diff --git a/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h b/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h new file mode 100644 index 0000000000000..7f83333fd78b6 --- /dev/null +++ b/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h @@ -0,0 +1,27 @@ +#pragma once + +#include "AP_TopoLift.h" + +// Implements a super basic topographic lift model that uses +// the current UAV location, the local terrain gradient, +// the current wind estimate, and current height above +// terrain to calculate the theoretical amount of topographic lift. +// This is not very realistic or accurate, but it is useful as a +// demonstration of the AP_TopoLift library. +// More comprehensive models may be developed, or run offboard +// and supplied through AP_ExternalTopoLift. + +class AP_TopoLiftSinglePointEstimate : public AP_TopoLift { + public: + // Get the terrain height of a point. + virtual bool get_terrain_height(const Location loc, float& height_amsl) override; + + // Calculate the wind rate of topographic influenced lift at the current location. + virtual bool calc_lift(const Location loc, float& lift) override; + + // Get the wind estimate at the current location. + virtual bool get_wind_at_current_loc(Vector3f& wind) override; + + // // Update state + // virtual void update() override; +}; \ No newline at end of file diff --git a/libraries/AP_TopoLift/AP_TopoLift_config.h b/libraries/AP_TopoLift/AP_TopoLift_config.h new file mode 100644 index 0000000000000..7a84e0bcaf644 --- /dev/null +++ b/libraries/AP_TopoLift/AP_TopoLift_config.h @@ -0,0 +1,3 @@ +#ifndef AP_TOPO_LIFT_ENABLED +#define AP_TOPO_LIFT_ENABLED 1 +#endif diff --git a/libraries/AP_TopoLift/README.md b/libraries/AP_TopoLift/README.md new file mode 100644 index 0000000000000..bc0042e46f590 --- /dev/null +++ b/libraries/AP_TopoLift/README.md @@ -0,0 +1,9 @@ +# AP Topographic Lift + +This library is responsible for running model(s) that estimate the topographic lift. AP could support more advanced models, however only a simplistic model is supported today. + +## Interface + +* Current UAV location in global coordinates +* Ability to query terrain elevation at lat-lon coordinates +* Ability to retrieve the current wind estimate \ No newline at end of file diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 02895d1bc2740..d114ec48799ba 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -80,6 +80,11 @@ #include #endif +#include +#if AP_TOPO_LIFT_ENABLED +#include +#endif + #include class AP_DDS_Client; @@ -562,6 +567,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_Filters filters; #endif +#if AP_TOPO_LIFT_ENABLED + AP_TopoLiftSinglePointEstimate topo_lift; +#endif + // Bitmask of modes to disable from gcs AP_Int32 flight_mode_GCS_block; }; diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index c0469abc7cf20..021b69f90eefe 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -853,7 +853,9 @@ void Aircraft::update_wind(const struct sitl_input &input) sinf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)), sinf(radians(input.wind.dir_z))) * input.wind.speed; - wind_ef.z += get_local_updraft(position + home.get_distance_NED_double(origin)); + auto const curr_pos = position + home.get_distance_NED_double(origin); + wind_ef.z += get_local_updraft(curr_pos); + wind_ef.z += get_topo_lift(location); const float wind_turb = input.wind.turbulence * 10.0f; // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98 const float iir_coef = 0.98f; // filtering high frequencies from turbulence @@ -1244,6 +1246,17 @@ float Aircraft::get_local_updraft(const Vector3d ¤tPos) return w; } + +float Aircraft::get_topo_lift(const Location &loc) +{ + float lift; + if (topo_lift_estimate.calc_lift(loc, lift)) { + return lift; + } else { + return 0.0; + } +} + void Aircraft::add_twist_forces(Vector3f &rot_accel) { if (sitl == nullptr) { diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index e84a078ac8faf..b5b9030d7886a 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -38,6 +38,10 @@ #include #include "SIM_JSON_Master.h" #include "ServoModel.h" +#include +#if AP_TOPO_LIFT_ENABLED +#include +#endif #include "SIM_GPIO_LED_1.h" #include "SIM_GPIO_LED_2.h" #include "SIM_GPIO_LED_3.h" @@ -330,6 +334,11 @@ class Aircraft { // get local thermal updraft float get_local_updraft(const Vector3d ¤tPos); +#if AP_TOPO_LIFT_ENABLED + // get topographic lift influence + float get_topo_lift(const Location &loc); +#endif + // update EAS speeds void update_eas_airspeed(); @@ -382,6 +391,9 @@ class Aircraft { DroneCANDevice *dronecan; #endif +#if AP_TOPO_LIFT_ENABLED + AP_TopoLiftSinglePointEstimate topo_lift_estimate; +#endif #if AP_SIM_GPIO_LED_1_ENABLED GPIO_LED_1 sim_led1{8}; // pin to match sitl.h From 5d3c167706a2ada29f7b3ecf6b08f1d89a447783 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Sun, 24 Nov 2024 20:42:26 -0700 Subject: [PATCH 3/3] Add gradient support Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- libraries/AP_Terrain/AP_Terrain.cpp | 45 +++++++++++++++++++ libraries/AP_Terrain/AP_Terrain.h | 6 ++- .../AP_TopoLiftSinglePointEstimate.cpp | 38 +++++++++++++++- .../AP_TopoLiftSinglePointEstimate.h | 5 +++ libraries/SITL/SIM_Aircraft.cpp | 15 ++++++- 5 files changed, 105 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 48966bfbe161c..e7967a81854fd 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -185,6 +185,51 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected) return true; } +bool AP_Terrain::gradient(const Location &loc, Vector2f& gradient) +{ + if (!allocate()) { + return false; + } + + struct grid_info info; + + calculate_grid_info(loc, info); + + // find the grid + const struct grid_block &grid = find_grid_cache(info).grid; + + /* + note that we rely on the one square overlap to ensure these + calculations don't go past the end of the arrays + */ + ASSERT_RANGE(info.idx_x, 0, TERRAIN_GRID_BLOCK_SIZE_X-2); + ASSERT_RANGE(info.idx_y, 0, TERRAIN_GRID_BLOCK_SIZE_Y-2); + + + // check we have all 4 required heights + if (!check_bitmap(grid, info.idx_x, info.idx_y) || + !check_bitmap(grid, info.idx_x, info.idx_y+1) || + !check_bitmap(grid, info.idx_x+1, info.idx_y) || + !check_bitmap(grid, info.idx_x+1, info.idx_y+1)) { + return false; + } + + // hXY are the heights of the 4 surrounding grid points + int16_t h00, h01, h10; + + h00 = grid.height[info.idx_x+0][info.idx_y+0]; + h01 = grid.height[info.idx_x+0][info.idx_y+1]; + h10 = grid.height[info.idx_x+1][info.idx_y+0]; + // h11 = grid.height[info.idx_x+1][info.idx_y+1]; + + // Positive X gradient means terrain faces east. + gradient.x = float(h00 - h10) / grid.spacing; + // Positive Y gradient means terrain faces north. + gradient.y = float(h01 - h01) / grid.spacing; + + return true; +} + /* find difference between home terrain height and the terrain diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index df9eb458e184b..e4afe0575e9c1 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -126,11 +126,12 @@ class AP_Terrain { bool height_amsl(const Location &loc, float &height, bool corrected = true); /* - find the terrain gradient (slope) for a location + find the terrain gradient (slope) for a location. + Gradient is [x, y] in m/m return false if not available */ - bool gradient(const Location &loc); + bool gradient(const Location &loc, Vector2f& gradient); /* find difference between home terrain height and the terrain height at the current location in meters. A positive result @@ -428,6 +429,7 @@ class AP_Terrain { // cache the home altitude, as it is needed so often float home_height; + Vector2f home_gradient; Location home_loc; bool have_home_height; diff --git a/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp b/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp index 9cb3c120ff6e4..1aca4541ff1ca 100644 --- a/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp +++ b/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp @@ -23,12 +23,45 @@ bool AP_TopoLiftSinglePointEstimate::get_terrain_height(const Location loc, floa return true; } +bool AP_TopoLiftSinglePointEstimate::get_terrain_grad(const Location loc, Vector2f& grad) { + + AP_Terrain *terrain = AP::terrain(); + if (terrain == nullptr) { + return false; + } + + // status() handles enable check. + if (terrain->status() != AP_Terrain::TerrainStatusOK) { + return false; + } + + return terrain->gradient(loc, grad); +} + bool AP_TopoLiftSinglePointEstimate::get_wind_at_current_loc(Vector3f& wind) { const AP_AHRS &ahrs = AP::ahrs(); return ahrs.wind_estimate(wind); } +bool AP_TopoLiftSinglePointEstimate::calc_lift(const Location loc, float& lift, const Vector2f& env_wind) { + float terrain_height_amsl; + if (!get_terrain_height(loc, terrain_height_amsl)) { + return false; + } + + [[maybe_unused]] const float veh_height_agl = loc.alt * 1E-2 - terrain_height_amsl; + + Vector2f terrain_grad; + if (!get_terrain_grad(loc, terrain_grad)) { + return false; + } + + lift = env_wind * terrain_grad; + + return true; +} + bool AP_TopoLiftSinglePointEstimate::calc_lift(const Location loc, float& lift) { float height_amsl; if (!get_terrain_height(loc, height_amsl)) { @@ -40,8 +73,11 @@ bool AP_TopoLiftSinglePointEstimate::calc_lift(const Location loc, float& lift) return false; } - // TODO calculate gradient of terrain. + Vector2f terrain_grad; + if (!get_terrain_grad(loc, terrain_grad)) { + return false; + } // TODO find a way to communicate steepness [[maybe_unused]] auto const terrain_steepness = terrain_grad.length(); diff --git a/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h b/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h index 7f83333fd78b6..04c18cfcc07ea 100644 --- a/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h +++ b/libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h @@ -19,9 +19,14 @@ class AP_TopoLiftSinglePointEstimate : public AP_TopoLift { // Calculate the wind rate of topographic influenced lift at the current location. virtual bool calc_lift(const Location loc, float& lift) override; + // Calculate the wind rate of topographic influenced lift given the steady state environmental wind vector. + virtual bool calc_lift(const Location loc, float& lift, const Vector2f& env_wind); + // Get the wind estimate at the current location. virtual bool get_wind_at_current_loc(Vector3f& wind) override; + bool get_terrain_grad(const Location loc, Vector2f& grad); + // // Update state // virtual void update() override; }; \ No newline at end of file diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 021b69f90eefe..6159026abeba8 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -856,6 +856,7 @@ void Aircraft::update_wind(const struct sitl_input &input) auto const curr_pos = position + home.get_distance_NED_double(origin); wind_ef.z += get_local_updraft(curr_pos); wind_ef.z += get_topo_lift(location); + // wind_ef.z += get_topo_lift(location); const float wind_turb = input.wind.turbulence * 10.0f; // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98 const float iir_coef = 0.98f; // filtering high frequencies from turbulence @@ -1249,8 +1250,20 @@ float Aircraft::get_local_updraft(const Vector3d ¤tPos) float Aircraft::get_topo_lift(const Location &loc) { + const float speed = AP::sitl()->wind_speed; + // A SITL wind of 0 is N wind (from N to S). + // A SITL wind of 90 is E wind (from E to W). + + // This dir will be 0 with east wind. + const float dir = radians(AP::sitl()->wind_direction - 90.0); + + // Wind in the topographic frame. X points east, Y points north. + // East wind will be [speed, 0] + // North wind will be [0, speed] + const Vector2f wind_topo(cosf(dir) * speed, sinf(dir) * speed); + // auto const wind_topo = Vector2f() float lift; - if (topo_lift_estimate.calc_lift(loc, lift)) { + if (topo_lift_estimate.calc_lift(loc, lift, wind_topo)) { return lift; } else { return 0.0;