Add topographic lift library and initial example implementation to SITL

Tools/ardupilotwaf/ardupilotwaf.py

@@ -118,6 +118,7 @@
     'AP_VideoTX',
     'AP_FETtecOneWire',
     'AP_TemperatureSensor',
+    'AP_TopoLift',
     'AP_Torqeedo',
     'AP_CustomRotations',
     'AP_AIS',

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 {

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

libraries/AP_Terrain/AP_Terrain.h

@@ -125,6 +125,13 @@ class AP_Terrain {
      */
     bool height_amsl(const Location &loc, float &height, bool corrected = true);
 
+    /*
+      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, Vector2f& gradient);
     /* 
        find difference between home terrain height and the terrain
        height at the current location in meters. A positive result
@@ -422,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;
 

libraries/AP_TopoLift/AP_TopoLift.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <AP_Common/Location.h>
+#include <AP_Math/vector3.h>
+
+
+// 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;
+};

libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.cpp

@@ -0,0 +1,90 @@
+#include "AP_TopoLiftSinglePointEstimate.h"
+#include <AP_Terrain/AP_Terrain.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_Math/vector2.h>
+
+
+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_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)) {
+        return false;
+    }
+
+    Vector3f wind;
+    if (!get_wind_at_current_loc(wind)) {
+        return false;
+    }
+
+
+    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();
+
+
+    // TODO compute lift based on gradient, wind, and altitude above terrain.
+    lift = 42.0;
+
+    return true;
+}

libraries/AP_TopoLift/AP_TopoLiftSinglePointEstimate.h

@@ -0,0 +1,32 @@
+#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;
+
+    // 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;
+};

libraries/AP_TopoLift/AP_TopoLift_config.h

@@ -0,0 +1,3 @@
+#ifndef AP_TOPO_LIFT_ENABLED
+#define AP_TOPO_LIFT_ENABLED 1
+#endif

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

libraries/AP_Vehicle/AP_Vehicle.h

@@ -80,6 +80,11 @@
 #include <AP_Gripper/AP_Gripper.h>
 #endif
 
+#include <AP_TopoLift/AP_TopoLift_config.h>
+#if AP_TOPO_LIFT_ENABLED
+#include <AP_TopoLift/AP_TopoLiftSinglePointEstimate.h>
+#endif
+
 #include <AP_IBus_Telem/AP_IBus_Telem.h>
 
 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;
 };

libraries/SITL/SIM_Aircraft.cpp

@@ -853,7 +853,10 @@ 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);
+    // 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 +1247,29 @@ float Aircraft::get_local_updraft(const Vector3d &currentPos)
     return w;
 }
 
+
+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, wind_topo)) {
+        return lift;
+    } else {
+        return 0.0;
+    }   
+}
+
 void Aircraft::add_twist_forces(Vector3f &rot_accel)
 {
     if (sitl == nullptr) {

libraries/SITL/SIM_Aircraft.h

@@ -38,6 +38,10 @@
 #include <Filter/Filter.h>
 #include "SIM_JSON_Master.h"
 #include "ServoModel.h"
+#include <AP_TopoLift/AP_TopoLift_config.h>
+#if AP_TOPO_LIFT_ENABLED
+#include <AP_TopoLift/AP_TopoLiftSinglePointEstimate.h>
+#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 &currentPos);
 
+#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