5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

AP_Baro: rework to separate out wind coefficient params

- disable on low flash boards
 - add an enable parameter per baro for wind coeffients
This commit is contained in:
Andrew Tridgell 2020-11-23 18:58:27 +11:00
parent 5da62aeaa2
commit 1c1c067dee
3 changed files with 133 additions and 114 deletions

View File

@ -198,101 +198,25 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
AP_GROUPINFO_FLAGS("3_DEVID", 17, AP_Baro, sensors[2].bus_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
#endif
// @Param: COEF_XP
// @DisplayName: Pressure error coefficient in positive X direction
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. If the baro height estimate rises during forwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF_XP", 18, AP_Baro, sensors[0].pcoef_xp, 0.0),
#if HAL_BARO_WIND_COMP_ENABLED
// @Group: WCOF1_
// @Path: AP_Baro_Wind.cpp
AP_SUBGROUPINFO(sensors[0].wind_coeff, "WCOF1_", 18, AP_Baro, WindCoeff),
// @Param: COEF_XN
// @DisplayName: Pressure error coefficient in negative X direction
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. If the baro height estimate rises during backwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF_XN", 19, AP_Baro, sensors[0].pcoef_xn, 0.0),
#if BARO_MAX_INSTANCES > 1
// @Group: WCOF2_
// @Path: AP_Baro_Wind.cpp
AP_SUBGROUPINFO(sensors[1].wind_coeff, "WCOF2_", 19, AP_Baro, WindCoeff),
#endif
// @Param: COEF_YP
// @DisplayName: Pressure error coefficient in positive Y direction
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the right, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF_YP", 20, AP_Baro, sensors[0].pcoef_yp, 0.0),
#if BARO_MAX_INSTANCES > 2
// @Group: WCOF3_
// @Path: AP_Baro_Wind.cpp
AP_SUBGROUPINFO(sensors[2].wind_coeff, "WCOF3_", 20, AP_Baro, WindCoeff),
#endif
#endif
// @Param: COEF_YN
// @DisplayName: Pressure error coefficient in negative Y direction
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the left, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF_YN", 21, AP_Baro, sensors[0].pcoef_yn, 0.0),
// @Param: COEF2_XP
// @DisplayName: Pressure error coefficient to the front
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. If the baro height estimate rises during forwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF2_XP", 22, AP_Baro, sensors[1].pcoef_xp, 0.0),
// @Param: COEF2_XN
// @DisplayName: Pressure error coefficient to the back
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. If the baro height estimate rises during backwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF2_XN", 23, AP_Baro, sensors[1].pcoef_xn, 0.0),
// @Param: COEF2_YP
// @DisplayName: Pressure error coefficient to the right
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the right, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF2_YP", 24, AP_Baro, sensors[1].pcoef_yp, 0.0),
// @Param: COEF2_YN
// @DisplayName: Pressure error coefficient to the left
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the left, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF2_YN", 25, AP_Baro, sensors[1].pcoef_yn, 0.0),
// @Param: COEF3_XP
// @DisplayName: Pressure error coefficient to the front
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. If the baro height estimate rises during forwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF3_XP", 26, AP_Baro, sensors[2].pcoef_xp, 0.0),
// @Param: COEF3_XN
// @DisplayName: Pressure error coefficient to the back
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. If the baro height estimate rises during backwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF3_XN", 27, AP_Baro, sensors[2].pcoef_xn, 0.0),
// @Param: COEF3_YP
// @DisplayName: Pressure error coefficient to the right
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the right, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF3_YP", 28, AP_Baro, sensors[2].pcoef_yp, 0.0),
// @Param: COEF3_YN
// @DisplayName: Pressure error coefficient to the left
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the left, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("COEF3_YN", 29, AP_Baro, sensors[2].pcoef_yn, 0.0), AP_GROUPEND
AP_GROUPEND
};
// singleton instance
@ -919,24 +843,10 @@ void AP_Baro::update(void)
}
float altitude = sensors[i].altitude;
float corrected_pressure = sensors[i].pressure + sensors[i].p_correction;
// correct for ststic pressure position errors
Vector3f airspeed_vec_bf;
if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
float pressure_error = 0.0f;
const float density = 1.225f * get_air_density_ratio();
if (!is_zero(sensors[i].pcoef_xp) && is_positive(airspeed_vec_bf.x)) {
pressure_error += sensors[i].pcoef_xp * 0.5f * density * sq(airspeed_vec_bf.x);
} else if (!is_zero(sensors[i].pcoef_xn) && is_negative(airspeed_vec_bf.x)) {
pressure_error += sensors[i].pcoef_xn * 0.5f * density * sq(airspeed_vec_bf.x);
}
if (!is_zero(sensors[i].pcoef_yp) && is_positive(airspeed_vec_bf.y)) {
pressure_error += sensors[i].pcoef_yp * 0.5f * density * sq(airspeed_vec_bf.y);
} else if (!is_zero(sensors[i].pcoef_yn) && is_negative(airspeed_vec_bf.y)) {
pressure_error += sensors[i].pcoef_yn * 0.5f * density * sq(airspeed_vec_bf.y);
}
corrected_pressure -= pressure_error;
}
if (sensors[i].type == BARO_TYPE_AIR) {
#if HAL_BARO_WIND_COMP_ENABLED
corrected_pressure -= wind_pressure_correction(i);
#endif
altitude = get_altitude_difference(sensors[i].ground_pressure, corrected_pressure);
} else if (sensors[i].type == BARO_TYPE_WATER) {
//101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
@ -1046,7 +956,6 @@ void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt)
}
#endif
namespace AP {
AP_Baro &baro()

View File

@ -21,6 +21,10 @@
#define BARO_TIMEOUT_MS 500 // timeout in ms since last successful read
#define BARO_DATA_CHANGE_TIMEOUT_MS 2000 // timeout in ms since last successful read that involved temperature of pressure changing
#ifndef HAL_BARO_WIND_COMP_ENABLED
#define HAL_BARO_WIND_COMP_ENABLED !HAL_MINIMIZE_FEATURES && !defined(HAL_BUILD_AP_PERIPH)
#endif
class AP_Baro_Backend;
class AP_Baro
@ -240,6 +244,19 @@ private:
PROBE_MSP =(1<<12),
};
#if HAL_BARO_WIND_COMP_ENABLED
class WindCoeff {
public:
static const struct AP_Param::GroupInfo var_info[];
AP_Int8 enable; // enable compensation for this barometer
AP_Float xp; // ratio of static pressure rise to dynamic pressure when flying forwards
AP_Float xn; // ratio of static pressure rise to dynamic pressure when flying backwards
AP_Float yp; // ratio of static pressure rise to dynamic pressure when flying to the right
AP_Float yn; // ratio of static pressure rise to dynamic pressure when flying to the left
};
#endif
struct sensor {
uint32_t last_update_ms; // last update time in ms
uint32_t last_change_ms; // last update time in ms that included a change in reading from previous readings
@ -253,10 +270,9 @@ private:
bool alt_ok; // true if calculated altitude is ok
bool calibrated; // true if calculated calibrated successfully
AP_Int32 bus_id;
AP_Float pcoef_xp; // ratio of static pressure rise to dynamic pressure when flying forwards
AP_Float pcoef_xn; // ratio of static pressure rise to dynamic pressure when flying backwards
AP_Float pcoef_yp; // ratio of static pressure rise to dynamic pressure when flying to the right
AP_Float pcoef_yn; // ratio of static pressure rise to dynamic pressure when flying to the left
#if HAL_BARO_WIND_COMP_ENABLED
WindCoeff wind_coeff;
#endif
} sensors[BARO_MAX_INSTANCES];
AP_Float _alt_offset;
@ -283,6 +299,14 @@ private:
// semaphore for API access from threads
HAL_Semaphore _rsem;
#if HAL_BARO_WIND_COMP_ENABLED
/*
return pressure correction for wind based on GND_WCOEF parameters
*/
float wind_pressure_correction(uint8_t instance);
#endif
};
namespace AP {

View File

@ -0,0 +1,86 @@
#include "AP_Baro.h"
#include <AP_AHRS/AP_AHRS.h>
#if HAL_BARO_WIND_COMP_ENABLED
// table of compensation coefficient parameters for one baro
const AP_Param::GroupInfo AP_Baro::WindCoeff::var_info[] = {
// @Param: ENABLE
// @DisplayName: Wind coefficient enable
// @Description: This enables the use of wind coefficients for barometer compensation
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, WindCoeff, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: XP
// @DisplayName: Pressure error coefficient in positive X direction
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. If the baro height estimate rises during forwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("XP", 2, WindCoeff, xp, 0.0),
// @Param: XN
// @DisplayName: Pressure error coefficient in negative X direction
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. If the baro height estimate rises during backwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("XN", 3, WindCoeff, xn, 0.0),
// @Param: YP
// @DisplayName: Pressure error coefficient in positive Y direction
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the right, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("YP", 4, WindCoeff, yp, 0.0),
// @Param: YN
// @DisplayName: Pressure error coefficient in negative Y direction
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the left, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("YN", 5, WindCoeff, yn, 0.0),
AP_GROUPEND
};
/*
return pressure correction for wind based on GND_WCOEF parameters
*/
float AP_Baro::wind_pressure_correction(uint8_t instance)
{
const WindCoeff &wcoef = sensors[instance].wind_coeff;
if (!wcoef.enable) {
return 0;
}
// correct for static pressure position errors
Vector3f airspeed_vec_bf;
if (!AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
return 0;
}
float error = 0.0;
const float sqx = sq(airspeed_vec_bf.x);
const float sqy = sq(airspeed_vec_bf.y);
if (is_positive(airspeed_vec_bf.x)) {
error += wcoef.xp * sqx;
} else {
error += wcoef.xn * sqx;
}
if (is_positive(airspeed_vec_bf.y)) {
error += wcoef.yp * sqy;
} else {
error += wcoef.yn * sqy;
}
return error * 0.5 * SSL_AIR_DENSITY * get_air_density_ratio();
}
#endif // HAL_BARO_WIND_COMP_ENABLED