AP_Baro: baro thrust scaling

ArduPilot · Jan 1, 2025 · 7567331 · 7567331
1 parent 5726c03
commit 7567331
Show file tree

Hide file tree

Showing 5 changed files with 66 additions and 5 deletions.
diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
@@ -56,6 +56,9 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#if HAL_BARO_THST_COMP_ENABLED
+#include <AP_Motors/AP_Motors.h>
+#endif
 
 #define INTERNAL_TEMPERATURE_CLAMP 35.0f
 
@@ -225,6 +228,32 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
 #endif
 #endif  // HAL_BARO_WIND_COMP_ENABLED
 
+#if HAL_BARO_THST_COMP_ENABLED
+    // @Param: 1_THST_SCALE
+    // @DisplayName: Thrust compensation
+    // @Description: User provided thrust scaling. This is used to adjust for local pressure difference induced by the props.
+    // @Range: -300 300
+    // @User: Advanced
+    AP_GROUPINFO("1_THST_SCALE", 25, AP_Baro, sensors[0].mot_scale, 0),
+
+#if BARO_MAX_INSTANCES > 1
+    // @Param: 2_THST_SCALE
+    // @DisplayName: Thrust compensation
+    // @Description: User provided thrust scaling. This is used to adjust for local pressure difference induced by the props.
+    // @Range: -300 300
+    // @User: Advanced
+    AP_GROUPINFO("2_THST_SCALE", 26, AP_Baro, sensors[1].mot_scale, 0),
+#endif
+#if BARO_MAX_INSTANCES > 2
+    // @Param: 3_THST_SCALE
+    // @DisplayName: Thrust compensation
+    // @Description: User provided thrust scaling. This is used to adjust for local pressure difference induced by the props.
+    // @Range: -300 300
+    // @User: Advanced
+    AP_GROUPINFO("3_THST_SCALE", 27, AP_Baro, sensors[2].mot_scale, 0),
+#endif
+#endif  // HAL_BARO_WIND_COMP_ENABLED
+
 #if AP_FIELD_ELEVATION_ENABLED
     // @Param: _FIELD_ELV
     // @DisplayName: field elevation
@@ -871,6 +900,10 @@ void AP_Baro::update(void)
             if (sensors[i].type == BARO_TYPE_AIR) {
 #if HAL_BARO_WIND_COMP_ENABLED
                 corrected_pressure -= wind_pressure_correction(i);
+#endif
+#if HAL_BARO_THST_COMP_ENABLED
+                corrected_pressure -= thrust_pressure_correction(i);
+                sensors[i].corrected_pressure = corrected_pressure;
 #endif
                 altitude = get_altitude_difference(sensors[i].ground_pressure, corrected_pressure);
 
@@ -987,6 +1020,19 @@ void AP_Baro::update_field_elevation(void)
 #endif
 }
 
+#if HAL_BARO_THST_COMP_ENABLED
+float AP_Baro::thrust_pressure_correction(uint8_t instance)
+{
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_COPTER_OR_HELI
+    const AP_Motors* motors = AP::motors();
+    if (motors == nullptr) {
+         return 0.0f;
+    }
+    const float motors_throttle = MAX(0,motors->get_throttle_out());
+    return sensors[instance].mot_scale * motors_throttle;
+#endif
+}
+#endif
 
 /* register a new sensor, claiming a sensor slot. If we are out of
    slots it will panic

diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
@@ -75,6 +75,9 @@ class AP_Baro
     // dynamic pressure in Pascal. Divide by 100 for millibars or hectopascals
     const Vector3f& get_dynamic_pressure(uint8_t instance) const { return sensors[instance].dynamic_pressure; }
 #endif
+#if HAL_BARO_THST_COMP_ENABLED
+    float get_corrected_pressure(uint8_t instance) const { return sensors[instance].corrected_pressure; }
+#endif
 
     // temperature in degrees C
     float get_temperature(void) const { return get_temperature(_primary); }
@@ -298,6 +301,10 @@ class AP_Baro
 #if HAL_BARO_WIND_COMP_ENABLED
         WindCoeff wind_coeff;
         Vector3f dynamic_pressure;      // calculated dynamic pressure
+#endif
+#if HAL_BARO_THST_COMP_ENABLED
+        AP_Float mot_scale;             // thrust-based pressure scaling
+        float corrected_pressure;
 #endif
     } sensors[BARO_MAX_INSTANCES];
 
@@ -340,7 +347,9 @@ class AP_Baro
     */
     float wind_pressure_correction(uint8_t instance);
 #endif
-
+#if HAL_BARO_THST_COMP_ENABLED
+    float thrust_pressure_correction(uint8_t instance);
+#endif
     // Logging function
     void Write_Baro(void);
     void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance);

diff --git a/libraries/AP_Baro/AP_Baro_Logging.cpp b/libraries/AP_Baro/AP_Baro_Logging.cpp
@@ -21,6 +21,7 @@ void AP_Baro::Write_Baro_instance(uint64_t time_us, uint8_t baro_instance)
         drift_offset  : get_baro_drift_offset(),
         ground_temp   : get_ground_temperature(),
         healthy       : (uint8_t)healthy(baro_instance),
+        corrected_pressure : get_corrected_pressure(baro_instance),
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 #if HAL_BARO_WIND_COMP_ENABLED

diff --git a/libraries/AP_Baro/AP_Baro_config.h b/libraries/AP_Baro/AP_Baro_config.h
@@ -103,3 +103,7 @@
 // this allows for using the simple model with the --ekf-single configure option
 #define AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED HAL_WITH_EKF_DOUBLE || AP_SIM_ENABLED
 #endif
+
+#ifndef HAL_BARO_THST_COMP_ENABLED
+#define HAL_BARO_THST_COMP_ENABLED 1
+#endif
diff --git a/libraries/AP_Baro/LogStructure.h b/libraries/AP_Baro/LogStructure.h
@@ -32,6 +32,7 @@ struct PACKED log_BARO {
     float   drift_offset;
     float   ground_temp;
     uint8_t healthy;
+    float   corrected_pressure;
 };
 
 // @LoggerMessage: BARD
@@ -53,10 +54,10 @@ struct PACKED log_BARD {
 #define LOG_STRUCTURE_FROM_BARO                                         \
     { LOG_BARO_MSG, sizeof(log_BARO),                                   \
             "BARO",                                                     \
-            "Q"       "B"  "f"    "f"        "f"      "c"     "f"    "I"    "f"       "f"        "B", \
-            "TimeUS," "I," "Alt," "AltAMSL," "Press," "Temp," "CRt," "SMS," "Offset," "GndTemp," "Health", \
-            "s"       "#"  "m"    "m"        "P"      "O"     "n"    "s"    "m"       "O"        "-", \
-            "F"       "-"  "0"    "0"        "0"      "B"     "0"    "C"    "?"       "0"        "-", \
+            "Q"       "B"  "f"    "f"        "f"      "c"     "f"    "I"    "f"       "f"        "B"      "f", \
+            "TimeUS," "I," "Alt," "AltAMSL," "Press," "Temp," "CRt," "SMS," "Offset," "GndTemp," "Hlth,"  "CPress", \
+            "s"       "#"  "m"    "m"        "P"      "O"     "n"    "s"    "m"       "O"        "-"      "P", \
+            "F"       "-"  "0"    "0"        "0"      "B"     "0"    "C"    "?"       "0"        "-"      "0", \
             true                                                        \
             },                                                          \
     { LOG_BARD_MSG, sizeof(log_BARD),                                   \