Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: add static pressure position error correction in the Z-axis for fast copters at high lean

This commit is contained in:
Andy Piper 2022-11-22 20:57:25 +00:00 committed by Andrew Tridgell
parent 343b72dc32
commit 5727cfacb9
3 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Baro/AP_Baro.h
View File

@ -250,6 +250,8 @@ private:
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
AP_Float zp; // ratio of static pressure rise to dynamic pressure when flying up
AP_Float zn; // ratio of static pressure rise to dynamic pressure when flying down
};
#endif

6
libraries/AP_Baro/AP_Baro_SITL.cpp
View File

@ -167,6 +167,7 @@ float AP_Baro_SITL::wind_pressure_correction(void)
float error = 0.0;
const float sqx = sq(airspeed_vec_bf.x);
const float sqy = sq(airspeed_vec_bf.y);
const float sqz = sq(airspeed_vec_bf.z);
if (is_positive(airspeed_vec_bf.x)) {
error += bp.wcof_xp * sqx;
@ -178,6 +179,11 @@ float AP_Baro_SITL::wind_pressure_correction(void)
} else {
error += bp.wcof_yn * sqy;
}
if (is_positive(airspeed_vec_bf.z)) {
error += bp.wcof_zp * sqz;
} else {
error += bp.wcof_zn * sqz;
}
return error * 0.5 * SSL_AIR_DENSITY * AP::baro().get_air_density_ratio();
}

22
libraries/AP_Baro/AP_Baro_Wind.cpp
View File

@ -45,6 +45,22 @@ const AP_Param::GroupInfo AP_Baro::WindCoeff::var_info[] = {
// @User: Advanced
AP_GROUPINFO("LFT", 5, WindCoeff, yn, 0.0),
// @Param: UP
// @DisplayName: Pressure error coefficient in positive Z direction (up)
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Z body axis. If the baro height estimate rises above truth height during climbing flight (or forward flight with a high forwards lean angle), then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("UP", 6, WindCoeff, zp, 0.0),
// @Param: DN
// @DisplayName: Pressure error coefficient in negative Z direction (down)
// @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Z body axis. If the baro height estimate rises above truth height during descending flight (or forward flight with a high backwards lean angle, eg braking manoeuvre), then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
// @Range: -1.0 1.0
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("DN", 7, WindCoeff, zn, 0.0),
AP_GROUPEND
};
@ -69,6 +85,7 @@ float AP_Baro::wind_pressure_correction(uint8_t instance)
float error = 0.0;
const float sqx = sq(airspeed_vec_bf.x);
const float sqy = sq(airspeed_vec_bf.y);
const float sqz = sq(airspeed_vec_bf.z);
if (is_positive(airspeed_vec_bf.x)) {
error += wcoef.xp * sqx;
@ -80,6 +97,11 @@ float AP_Baro::wind_pressure_correction(uint8_t instance)
} else {
error += wcoef.yn * sqy;
}
if (is_positive(airspeed_vec_bf.z)) {
error += wcoef.zp * sqz;
} else {
error += wcoef.zn * sqz;
}
return error * 0.5 * SSL_AIR_DENSITY * get_air_density_ratio();
}