diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index ac3750d3a6..e28752cf65 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -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() diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 2906644cd3..341fa80e56 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -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 { diff --git a/libraries/AP_Baro/AP_Baro_Wind.cpp b/libraries/AP_Baro/AP_Baro_Wind.cpp new file mode 100644 index 0000000000..4476732af9 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_Wind.cpp @@ -0,0 +1,86 @@ +#include "AP_Baro.h" +#include + +#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