Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Baro thrust scaling #28982

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+71 −6
Open

Baro thrust scaling #28982

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 49 additions & 0 deletions libraries/AP_Baro/AP_Baro.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,9 @@
#include <AP_Logger/AP_Logger.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#if AP_BARO_THST_COMP_ENABLED
#include <AP_Motors/AP_Motors.h>
#endif

#define INTERNAL_TEMPERATURE_CLAMP 35.0f

Expand Down Expand Up @@ -225,6 +228,32 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
#endif
#endif // HAL_BARO_WIND_COMP_ENABLED

#if AP_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
Expand Down Expand Up @@ -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 AP_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);

Expand Down Expand Up @@ -987,6 +1020,22 @@ void AP_Baro::update_field_elevation(void)
#endif
}

#if AP_BARO_THST_COMP_ENABLED
// scale the baro linearly with thrust
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
Expand Down
11 changes: 10 additions & 1 deletion libraries/AP_Baro/AP_Baro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 AP_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); }
Expand Down Expand Up @@ -298,6 +301,10 @@ class AP_Baro
#if HAL_BARO_WIND_COMP_ENABLED
WindCoeff wind_coeff;
Vector3f dynamic_pressure; // calculated dynamic pressure
#endif
#if AP_BARO_THST_COMP_ENABLED
AP_Float mot_scale; // thrust-based pressure scaling
float corrected_pressure;
#endif
} sensors[BARO_MAX_INSTANCES];

Expand Down Expand Up @@ -340,7 +347,9 @@ class AP_Baro
*/
float wind_pressure_correction(uint8_t instance);
#endif

#if AP_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);
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Baro/AP_Baro_Logging.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 4 additions & 0 deletions libraries/AP_Baro/AP_Baro_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 AP_BARO_THST_COMP_ENABLED
#define AP_BARO_THST_COMP_ENABLED 1
#endif
12 changes: 7 additions & 5 deletions libraries/AP_Baro/LogStructure.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -32,6 +33,7 @@ struct PACKED log_BARO {
float drift_offset;
float ground_temp;
uint8_t healthy;
float corrected_pressure;
};

// @LoggerMessage: BARD
Expand All @@ -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), \
Expand Down
Loading