diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 1f4c397718ef13..ff7b10848e2563 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -56,6 +56,9 @@ #include #include #include +#if HAL_BARO_THST_COMP_ENABLED +#include +#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 in Pascals. This is used to adjust linearly based on the thrust output 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 in Pascals. This is used to adjust linearly based on the thrust output 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 in Pascals. This is used to adjust linearly based on the thrust output 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,21 @@ 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; +#else + return 0.0f; +#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 index 6fdbb02880e629..c1718317511b07 100644 --- 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 index e0e38f9becf896..8f51150990f6b4 100644 --- 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 index 643ec9202385be..99cc70327ecf98 100644 --- 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 index d67b6be3c4b78f..a80d2eafaaff7f 100644 --- a/libraries/AP_Baro/LogStructure.h +++ b/libraries/AP_Baro/LogStructure.h @@ -18,7 +18,8 @@ // @Field: SMS: time last sample was taken // @Field: Offset: raw adjustment of barometer altitude, zeroed on calibration, possibly set by GCS // @Field: GndTemp: temperature on ground, specified by parameter or measured while on ground -// @Field: Health: true if barometer is considered healthy +// @Field: H: true if barometer is considered healthy +// @Field: CPress: compensated atmospheric pressure struct PACKED log_BARO { LOG_PACKET_HEADER; uint64_t time_us; @@ -32,6 +33,7 @@ struct PACKED log_BARO { float drift_offset; float ground_temp; uint8_t healthy; + float corrected_pressure; }; // @LoggerMessage: BARD @@ -53,10 +55,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," "H," "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), \