Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: added AHRS_GPS_USE=2 for no baro

Browse Source 
this allows DCM to use the GPS instead of the baro for height
This commit is contained in:
Andrew Tridgell 2021-01-18 16:32:37 +11:00
parent 8e4d67a7c1
commit cd3ac639fe
4 changed files with 31 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -41,11 +41,11 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0f), AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0f),
// @Param: GPS_USE // @Param: GPS_USE
// @DisplayName: AHRS use GPS for navigation // @DisplayName: AHRS DCM use GPS for navigation
// @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS whenever it is available. // @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS whenever it is available. A value of 2 means to use GPS for height as well as position in DCM.
// @Values: 0:Disabled,1:Enabled // @Values: 0:Disabled,1:Use GPS for DCM position,2:Use GPS for DCM position and height
// @User: Advanced // @User: Advanced
AP_GROUPINFO("GPS_USE", 3, AP_AHRS, _gps_use, 1), AP_GROUPINFO("GPS_USE", 3, AP_AHRS, _gps_use, float(GPSUse::Enable)),
// @Param: YAW_P // @Param: YAW_P
// @DisplayName: Yaw P // @DisplayName: Yaw P

9
libraries/AP_AHRS/AP_AHRS.h
View File

@ -631,7 +631,14 @@ protected:
AP_Float gps_gain; AP_Float gps_gain;
AP_Float beta; AP_Float beta;
AP_Int8 _gps_use;
enum class GPSUse : uint8_t {
Disable = 0,
Enable = 1,
EnableWithHeight = 2,
};
AP_Enum<GPSUse> _gps_use;
AP_Int8 _wind_max; AP_Int8 _wind_max;
AP_Int8 _board_orientation; AP_Int8 _board_orientation;
AP_Int8 _gps_minsats; AP_Int8 _gps_minsats;

24
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -414,7 +414,7 @@ AP_AHRS_DCM::_yaw_gain(void) const
// return true if we have and should use GPS // return true if we have and should use GPS
bool AP_AHRS_DCM::have_gps(void) const bool AP_AHRS_DCM::have_gps(void) const
{ {
if (AP::gps().status() <= AP_GPS::NO_FIX || !_gps_use) { if (AP::gps().status() <= AP_GPS::NO_FIX || _gps_use == GPSUse::Disable) {
return false; return false;
} }
return true; return true;
@ -1031,15 +1031,21 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
{ {
loc.lat = _last_lat; loc.lat = _last_lat;
loc.lng = _last_lng; loc.lng = _last_lng;
loc.alt = AP::baro().get_altitude() * 100 + _home.alt; const auto &baro = AP::baro();
const auto &gps = AP::gps();
if (_gps_use == GPSUse::EnableWithHeight &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
loc.alt = gps.location().alt;
} else {
loc.alt = baro.get_altitude() * 100 + _home.alt;
}
loc.relative_alt = 0; loc.relative_alt = 0;
loc.terrain_alt = 0; loc.terrain_alt = 0;
loc.offset(_position_offset_north, _position_offset_east); loc.offset(_position_offset_north, _position_offset_east);
const AP_GPS &_gps = AP::gps();
if (_flags.fly_forward && _have_position) { if (_flags.fly_forward && _have_position) {
float gps_delay_sec = 0; float gps_delay_sec = 0;
_gps.get_lag(gps_delay_sec); gps.get_lag(gps_delay_sec);
loc.offset_bearing(_gps.ground_course(), _gps.ground_speed() * gps_delay_sec); loc.offset_bearing(gps.ground_course(), gps.ground_speed() * gps_delay_sec);
} }
return _have_position; return _have_position;
} }
@ -1125,7 +1131,13 @@ bool AP_AHRS_DCM::set_home(const Location &loc)
// a relative ground position to home in meters, Down // a relative ground position to home in meters, Down
void AP_AHRS_DCM::get_relative_position_D_home(float &posD) const void AP_AHRS_DCM::get_relative_position_D_home(float &posD) const
{ {
posD = -AP::baro().get_altitude(); const auto &gps = AP::gps();
if (_gps_use == GPSUse::EnableWithHeight &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
posD = (_home.alt - gps.location().alt) * 0.01;
} else {
posD = -AP::baro().get_altitude();
}
} }
/* /*

2
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1416,7 +1416,7 @@ void AP_AHRS_NavEKF::get_relative_position_D_home(float &posD) const
float originD; float originD;
if (!get_relative_position_D_origin(originD) || if (!get_relative_position_D_origin(originD) ||
!get_origin(originLLH)) { !get_origin(originLLH)) {
posD = -AP::baro().get_altitude(); AP_AHRS_DCM::get_relative_position_D_home(posD);
return; return;
} }