zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Baro: rework to separate out wind coefficient params 

- disable on low flash boards
 - add an enable parameter per baro for wind coeffients
c415-sdk
Andrew Tridgell 4 years ago
parent
5da62aeaa2
commit
1c1c067dee
3 changed files with 133 additions and 114 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 129
      libraries/AP_Baro/AP_Baro.cpp
  2. 32
      libraries/AP_Baro/AP_Baro.h
  3. 86
      libraries/AP_Baro/AP_Baro_Wind.cpp

129
libraries/AP_Baro/AP_Baro.cpp
Unescape View File

@ -198,101 +198,25 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = { @@ -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),
// @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),
// @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),
// @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),
#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: 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),
#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: 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),
#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: 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) @@ -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) @@ -1046,7 +956,6 @@ void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt)
}
#endif
namespace AP {
AP_Baro &baro()

32
libraries/AP_Baro/AP_Baro.h
Unescape View File

@ -21,6 +21,10 @@ @@ -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: @@ -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: @@ -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: @@ -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
Unescape View File

@ -0,0 +1,86 @@ @@ -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
Write Preview
Loading…
Cancel
Save