aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Baro: baro thrust scaling

Browse Source
This commit is contained in:
Andy Piper 2025-01-01 20:27:55 +00:00
parent 5726c03e38
commit a64c7e93c1
5 changed files with 70 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_Baro/AP_Baro.cpp
View File

@ -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 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

11
libraries/AP_Baro/AP_Baro.h
View File

@ -75,6 +75,9 @@ public:
// 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 @@ private:
#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 @@ private:
*/
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);

1
libraries/AP_Baro/AP_Baro_Logging.cpp
View File

@ -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

4
libraries/AP_Baro/AP_Baro_config.h
View File

@ -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

12
libraries/AP_Baro/LogStructure.h
View File

@ -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), \