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:
parent
5da62aeaa2
commit
1c1c067dee
libraries/AP_Baro
@ -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()
|
||||
|
@ -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 {
|
||||
|
86
libraries/AP_Baro/AP_Baro_Wind.cpp
Normal file
86
libraries/AP_Baro/AP_Baro_Wind.cpp
Normal 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
|
Loading…
Reference in New Issue
Block a user