mirror of https://github.com/ArduPilot/ardupilot
AP_AHRS: move home and origin methods to frontend
This commit is contained in:
parent
02ca6b67a2
commit
7f4a4a99d6
|
@ -271,6 +271,12 @@ void AP_AHRS::update(bool skip_ins_update)
|
|||
// support locked access functions to AHRS data
|
||||
WITH_SEMAPHORE(_rsem);
|
||||
|
||||
// see if we have to restore home after a watchdog reset:
|
||||
if (!_checked_watchdog_home) {
|
||||
load_watchdog_home();
|
||||
_checked_watchdog_home = true;
|
||||
}
|
||||
|
||||
// drop back to normal priority if we were boosted by the INS
|
||||
// calling delay_microseconds_boost()
|
||||
hal.scheduler->boost_end();
|
||||
|
@ -657,6 +663,7 @@ void AP_AHRS::reset(bool recover_eulers)
|
|||
|
||||
AP_AHRS_DCM::reset(recover_eulers);
|
||||
_dcm_attitude = {roll, pitch, yaw};
|
||||
|
||||
#if HAL_NAVEKF2_AVAILABLE
|
||||
if (_ekf2_started) {
|
||||
_ekf2_started = EKF2.InitialiseFilter();
|
||||
|
|
|
@ -144,10 +144,10 @@ public:
|
|||
// set the EKF's origin location in 10e7 degrees. This should only
|
||||
// be called when the EKF has no absolute position reference (i.e. GPS)
|
||||
// from which to decide the origin on its own
|
||||
bool set_origin(const Location &loc) override WARN_IF_UNUSED;
|
||||
bool set_origin(const Location &loc) WARN_IF_UNUSED;
|
||||
|
||||
// returns the inertial navigation origin in lat/lon/alt
|
||||
bool get_origin(Location &ret) const override WARN_IF_UNUSED;
|
||||
bool get_origin(Location &ret) const WARN_IF_UNUSED;
|
||||
|
||||
bool have_inertial_nav() const override;
|
||||
|
||||
|
@ -337,6 +337,37 @@ public:
|
|||
// return SSA
|
||||
float getSSA(void) const { return _SSA; }
|
||||
|
||||
/*
|
||||
* home-related functionality
|
||||
*/
|
||||
|
||||
// get the home location. This is const to prevent any changes to
|
||||
// home without telling AHRS about the change
|
||||
const struct Location &get_home(void) const {
|
||||
return _home;
|
||||
}
|
||||
|
||||
// functions to handle locking of home. Some vehicles use this to
|
||||
// allow GCS to lock in a home location.
|
||||
void lock_home() {
|
||||
_home_locked = true;
|
||||
}
|
||||
bool home_is_locked() const {
|
||||
return _home_locked;
|
||||
}
|
||||
|
||||
// returns true if home is set
|
||||
bool home_is_set(void) const {
|
||||
return _home_is_set;
|
||||
}
|
||||
|
||||
// set the home location in 10e7 degrees. This should be called
|
||||
// when the vehicle is at this position. It is assumed that the
|
||||
// current barometer and GPS altitudes correspond to this altitude
|
||||
bool set_home(const Location &loc) WARN_IF_UNUSED;
|
||||
|
||||
void Log_Write_Home_And_Origin();
|
||||
|
||||
protected:
|
||||
// optional view class
|
||||
AP_AHRS_View *_view;
|
||||
|
@ -396,6 +427,15 @@ private:
|
|||
// get the index of the current primary IMU
|
||||
uint8_t get_primary_IMU_index(void) const;
|
||||
|
||||
/*
|
||||
* home-related state
|
||||
*/
|
||||
void load_watchdog_home();
|
||||
bool _checked_watchdog_home;
|
||||
struct Location _home;
|
||||
bool _home_is_set :1;
|
||||
bool _home_locked :1;
|
||||
|
||||
// avoid setting current state repeatedly across all cores on all EKFs:
|
||||
enum class TriState {
|
||||
False = 0,
|
||||
|
|
|
@ -304,7 +304,7 @@ Vector2f AP_AHRS_Backend::body_to_earth2D(const Vector2f &bf) const
|
|||
}
|
||||
|
||||
// log ahrs home and EKF origin
|
||||
void AP_AHRS_Backend::Log_Write_Home_And_Origin()
|
||||
void AP_AHRS::Log_Write_Home_And_Origin()
|
||||
{
|
||||
AP_Logger *logger = AP_Logger::get_singleton();
|
||||
if (logger == nullptr) {
|
||||
|
|
|
@ -386,41 +386,6 @@ public:
|
|||
return false;
|
||||
}
|
||||
|
||||
// get the home location. This is const to prevent any changes to
|
||||
// home without telling AHRS about the change
|
||||
const struct Location &get_home(void) const {
|
||||
return _home;
|
||||
}
|
||||
|
||||
// functions to handle locking of home. Some vehicles use this to
|
||||
// allow GCS to lock in a home location.
|
||||
void lock_home() {
|
||||
_home_locked = true;
|
||||
}
|
||||
bool home_is_locked() const {
|
||||
return _home_locked;
|
||||
}
|
||||
|
||||
// returns true if home is set
|
||||
bool home_is_set(void) const {
|
||||
return _home_is_set;
|
||||
}
|
||||
|
||||
// set the home location in 10e7 degrees. This should be called
|
||||
// when the vehicle is at this position. It is assumed that the
|
||||
// current barometer and GPS altitudes correspond to this altitude
|
||||
virtual bool set_home(const Location &loc) WARN_IF_UNUSED = 0;
|
||||
|
||||
// set the EKF's origin location in 10e7 degrees. This should only
|
||||
// be called when the EKF has no absolute position reference (i.e. GPS)
|
||||
// from which to decide the origin on its own
|
||||
virtual bool set_origin(const Location &loc) WARN_IF_UNUSED { return false; }
|
||||
|
||||
// returns the inertial navigation origin in lat/lon/alt
|
||||
virtual bool get_origin(Location &ret) const WARN_IF_UNUSED { return false; }
|
||||
|
||||
void Log_Write_Home_And_Origin();
|
||||
|
||||
// return true if the AHRS object supports inertial navigation,
|
||||
// with very accurate position and velocity
|
||||
virtual bool have_inertial_nav(void) const {
|
||||
|
@ -629,11 +594,6 @@ protected:
|
|||
Vector2f _hp; // ground vector high-pass filter
|
||||
Vector2f _lastGndVelADS; // previous HPF input
|
||||
|
||||
// reference position for NED positions
|
||||
struct Location _home;
|
||||
bool _home_is_set :1;
|
||||
bool _home_locked :1;
|
||||
|
||||
// helper trig variables
|
||||
float _cos_roll{1.0f};
|
||||
float _cos_pitch{1.0f};
|
||||
|
|
|
@ -50,7 +50,7 @@ AP_AHRS_DCM::reset_gyro_drift(void)
|
|||
|
||||
|
||||
/* if this was a watchdog reset then get home from backup registers */
|
||||
void AP_AHRS_DCM::load_watchdog_home()
|
||||
void AP_AHRS::load_watchdog_home()
|
||||
{
|
||||
const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
|
||||
if (hal.util->was_watchdog_reset() && (pd.home_lat != 0 || pd.home_lon != 0)) {
|
||||
|
@ -70,11 +70,6 @@ AP_AHRS_DCM::update(bool skip_ins_update)
|
|||
// support locked access functions to AHRS data
|
||||
WITH_SEMAPHORE(_rsem);
|
||||
|
||||
if (_last_startup_ms == 0) {
|
||||
_last_startup_ms = AP_HAL::millis();
|
||||
load_watchdog_home();
|
||||
}
|
||||
|
||||
AP_InertialSensor &_ins = AP::ins();
|
||||
|
||||
if (!skip_ins_update) {
|
||||
|
@ -230,9 +225,6 @@ AP_AHRS_DCM::reset(bool recover_eulers)
|
|||
|
||||
}
|
||||
|
||||
if (_last_startup_ms == 0) {
|
||||
load_watchdog_home();
|
||||
}
|
||||
_last_startup_ms = AP_HAL::millis();
|
||||
}
|
||||
|
||||
|
@ -1038,7 +1030,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
|
|||
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
loc.alt = gps.location().alt;
|
||||
} else {
|
||||
loc.alt = baro.get_altitude() * 100 + _home.alt;
|
||||
loc.alt = baro.get_altitude() * 100 + AP::ahrs().get_home().alt;
|
||||
}
|
||||
loc.relative_alt = 0;
|
||||
loc.terrain_alt = 0;
|
||||
|
@ -1094,7 +1086,7 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool AP_AHRS_DCM::set_home(const Location &loc)
|
||||
bool AP_AHRS::set_home(const Location &loc)
|
||||
{
|
||||
WITH_SEMAPHORE(_rsem);
|
||||
// check location is valid
|
||||
|
@ -1135,7 +1127,7 @@ 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 = (_home.alt - gps.location().alt) * 0.01;
|
||||
posD = (AP::ahrs().get_home().alt - gps.location().alt) * 0.01;
|
||||
} else {
|
||||
posD = -AP::baro().get_altitude();
|
||||
}
|
||||
|
|
|
@ -102,7 +102,6 @@ public:
|
|||
// return the quaternion defining the rotation from NED to XYZ (body) axes
|
||||
bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
|
||||
|
||||
bool set_home(const Location &loc) override WARN_IF_UNUSED;
|
||||
void estimate_wind(void);
|
||||
|
||||
// is the AHRS subsystem healthy?
|
||||
|
@ -132,7 +131,6 @@ private:
|
|||
void euler_angles(void);
|
||||
bool have_gps(void) const;
|
||||
bool use_fast_gains(void) const;
|
||||
void load_watchdog_home();
|
||||
void backup_attitude(void);
|
||||
|
||||
// primary representation of attitude of board used for all inertial calculations
|
||||
|
|
Loading…
Reference in New Issue