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

AP_AHRS: Enable AP_InertialNav to access all EKF functions via AP_AHRS

Browse Source 
This patch adds additional methods to the the AHRS library so that the AP_InertialNav library dow nto have to access the EKF directly. This enables Copter to fly using the EKF nominated by AHRS_EKF_TYPE.
It will also pave the way to elimination of the AP_InertialNav library.
This commit is contained in:
Paul Riseborough 2015-10-12 20:50:01 +11:00 committed by Andrew Tridgell
parent f062ed4180
commit e65ae51564
2 changed files with 129 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -428,6 +428,41 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
}
}
// Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity)
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE1:
default:
EKF1.getPosDownDerivative(velocity);
return true;
case EKF_TYPE2:
EKF2.getPosDownDerivative(velocity);
return true;
}
}
// get latest height above ground level estimate in metres and a validity flag
bool AP_AHRS_NavEKF::get_hagl(float &height) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE1:
default:
return EKF1.getHAGL(height);
case EKF_TYPE2:
return EKF2.getHAGL(height);
}
return false;
}
// return a relative ground position in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
@ -611,6 +646,21 @@ bool AP_AHRS_NavEKF::initialised(void) const
}
};
// get_filter_status : returns filter status as a series of flags
bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE1:
default:
EKF1.getFilterStatus(status);
case EKF_TYPE2:
EKF2.getFilterStatus(status);
}
return true;
}
// write optical flow data to EKF
void AP_AHRS_NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas)
{
@ -722,5 +772,61 @@ void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan)
}
}
// returns the inertial navigation origin in lat/lon/alt
struct Location AP_AHRS_NavEKF::get_origin() const
{
struct Location ret;
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using inertial nav so no origin
// initialise location to all zeros
memset(&ret, 0, sizeof(ret));
case EKF_TYPE1:
if (!EKF1.getOriginLLH(ret)) {
// initialise location to all zeros if EKF1 origin not yet set
memset(&ret, 0, sizeof(ret));
}
case EKF_TYPE2:
if (!EKF2.getOriginLLH(ret)) {
// initialise location to all zeros if EKF2 origin not yet set
memset(&ret, 0, sizeof(ret));
}
}
return ret;
}
// get_hgt_ctrl_limit - get maximum height to be observed by the control loops in metres and a validity flag
// this is used to limit height during optical flow navigation
// it will return invalid when no limiting is required
bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using an EKF so no limiting applies
return false;
case EKF_TYPE1:
return EKF1.getHeightControlLimit(limit);
case EKF_TYPE2:
return EKF2.getHeightControlLimit(limit);
}
return false;
}
// get_location - updates the provided location with the latest calculated locatoin
// returns true on success (i.e. the EKF knows it's latest position), false on failure
bool AP_AHRS_NavEKF::get_location(struct Location &loc) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using an EKF so no limiting applies
return false;
case EKF_TYPE1:
return EKF1.getLLH(loc);
case EKF_TYPE2:
return EKF2.getLLH(loc);
}
return false;
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

23
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -28,6 +28,7 @@
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
#define AP_AHRS_NAVEKF_AVAILABLE 1
#define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started
@ -64,6 +65,9 @@ public:
// dead-reckoning support
bool get_position(struct Location &loc) const;
// get latest altitude estimate above ground level in metres and validity flag
bool get_hagl(float &hagl) const;
// status reporting of estimated error
float get_error_rp(void) const;
float get_error_yaw(void) const;
@ -113,11 +117,18 @@ public:
// set home location
void set_home(const Location &loc);
// returns the inertial navigation origin in lat/lon/alt
struct Location get_origin() const;
bool have_inertial_nav(void) const;
bool get_velocity_NED(Vector3f &vec) const;
bool get_relative_position_NED(Vector3f &vec) const;
// Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
bool get_vert_pos_rate(float &velocity);
// write optical flow measurements to EKF
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas);
@ -135,6 +146,9 @@ public:
// true if the AHRS has completed initialisation
bool initialised(void) const;
// get_filter_status - returns filter status as a series of flags
bool get_filter_status(nav_filter_status &status) const;
// get compass offset estimates
// true if offsets are valid
bool getMagOffsets(Vector3f &magOffsets);
@ -156,6 +170,15 @@ public:
// send a EKF_STATUS_REPORT for current EKF
void send_ekf_status_report(mavlink_channel_t chan);
// get_hgt_ctrl_limit - get maximum height to be observed by the control loops in metres and a validity flag
// this is used to limit height during optical flow navigation
// it will return invalid when no limiting is required
bool get_hgt_ctrl_limit(float &limit) const;
// get_llh - updates the provided location with the latest calculated location including absolute altitude
// returns true on success (i.e. the EKF knows it's latest position), false on failure
bool get_location(struct Location &loc) const;
private:
enum EKF_TYPE {EKF_TYPE_NONE, EKF_TYPE1, EKF_TYPE2};
EKF_TYPE active_EKF_type(void) const;