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

AP_AHRS: make home-relative functions AHRS-frontend only

This commit is contained in:
Peter Barker 2021-08-24 18:05:05 +10:00 committed by Andrew Tridgell
parent e8066aa993
commit 9114d4fc86
6 changed files with 21 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -51,8 +51,8 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0f),
// @Param: GPS_USE
// @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. A value of 2 means to use GPS for height as well as position in DCM.
// @DisplayName: AHRS use GPS for DCM navigation and position-down
// @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 according to its own parameters. A value of 2 means to use GPS for height as well as position - both in DCM estimation and when determining altitude-above-home.
// @Values: 0:Disabled,1:Use GPS for DCM position,2:Use GPS for DCM position and height
// @User: Advanced
AP_GROUPINFO("GPS_USE", 3, AP_AHRS, _gps_use, float(GPSUse::Enable)),
@ -1639,7 +1639,13 @@ void AP_AHRS::get_relative_position_D_home(float &posD) const
float originD;
if (!get_relative_position_D_origin(originD) ||
!get_origin(originLLH)) {
AP_AHRS_DCM::get_relative_position_D_home(posD);
const auto &gps = AP::gps();
if (_gps_use == GPSUse::EnableWithHeight &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
posD = (get_home().alt - gps.location().alt) * 0.01;
} else {
posD = -AP::baro().get_altitude();
}
return;
}

6
libraries/AP_AHRS/AP_AHRS.h
View File

@ -174,17 +174,17 @@ public:
// return the relative position NED to either home or origin
// return true if the estimate is valid
bool get_relative_position_NED_home(Vector3f &vec) const override;
bool get_relative_position_NED_home(Vector3f &vec) const;
bool get_relative_position_NED_origin(Vector3f &vec) const override;
// return the relative position NE to either home or origin
// return true if the estimate is valid
bool get_relative_position_NE_home(Vector2f &posNE) const override;
bool get_relative_position_NE_home(Vector2f &posNE) const;
bool get_relative_position_NE_origin(Vector2f &posNE) const override;
// return the relative position down to either home or origin
// baro will be used for the _home relative one if the EKF isn't
void get_relative_position_D_home(float &posD) const override;
void get_relative_position_D_home(float &posD) const;
bool get_relative_position_D_origin(float &posD) const override;
// Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.

24
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -221,24 +221,12 @@ public:
return false;
}
// return a position relative to home in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true
virtual bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED {
return false;
}
// return a position relative to origin in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true
virtual bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED {
return false;
}
// return a position relative to home in meters, North/East
// order. Return true if estimate is valid
virtual bool get_relative_position_NE_home(Vector2f &vecNE) const WARN_IF_UNUSED {
return false;
}
// return a position relative to origin in meters, North/East
// order. Return true if estimate is valid
@ -246,10 +234,6 @@ public:
return false;
}
// return a Down position relative to home in meters
// if EKF is unavailable will return the baro altitude
virtual void get_relative_position_D_home(float &posD) const = 0;
// return a Down position relative to origin in meters
// Return true if estimate is valid
virtual bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED {
@ -427,6 +411,14 @@ public:
protected:
enum class GPSUse : uint8_t {
Disable = 0,
Enable = 1,
EnableWithHeight = 2,
};
AP_Enum<GPSUse> _gps_use;
// multi-thread access support
HAL_Semaphore _rsem;

12
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1141,18 +1141,6 @@ bool AP_AHRS::set_home(const Location &loc)
return true;
}
// a relative ground position to home in meters, Down
void AP_AHRS_DCM::get_relative_position_D_home(float &posD) const
{
const auto &gps = AP::gps();
if (_gps_use == GPSUse::EnableWithHeight &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
posD = (AP::ahrs().get_home().alt - gps.location().alt) * 0.01;
} else {
posD = -AP::baro().get_altitude();
}
}
/*
check if the AHRS subsystem is healthy
*/

9
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -82,8 +82,6 @@ public:
return _wind;
}
void get_relative_position_D_home(float &posD) const override;
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const override;
@ -135,13 +133,6 @@ protected:
AP_Float beta;
enum class GPSUse : uint8_t {
Disable = 0,
Enable = 1,
EnableWithHeight = 2,
};
AP_Enum<GPSUse> _gps_use;
AP_Int8 _gps_minsats;
private:

2
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -84,7 +84,7 @@ void AP_AHRS_Backend::Write_POS() const
return;
}
float home, origin;
get_relative_position_D_home(home);
AP::ahrs().get_relative_position_D_home(home);
const struct log_POS pkt{
LOG_PACKET_HEADER_INIT(LOG_POS_MSG),
time_us : AP_HAL::micros64(),