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

AP_AHRS: added common origin logic 

this aligns the origin between EKF2, EKF3 and ExternalAHRS, making for
smooth transitions between AHRS backends in flight
This commit is contained in:
Andrew Tridgell 2024-08-16 07:35:15 +10:00
parent 91a5cfea6b
commit be5ecab686
2 changed files with 54 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -617,6 +617,22 @@ void AP_AHRS::update_EKF2(void)
EKF2.getFilterStatus(filt_state); EKF2.getFilterStatus(filt_state);
update_notify_from_filter_status(filt_state); update_notify_from_filter_status(filt_state);
} }
/*
if we now have an origin then set in all backends
*/
if (!done_common_origin) {
Location new_origin;
if (EKF2.getOriginLLH(new_origin)) {
done_common_origin = true;
#if HAL_NAVEKF3_AVAILABLE
EKF3.setOriginLLH(new_origin);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
external.set_origin(new_origin);
#endif
}
}
} }
} }
#endif #endif
@ -686,6 +702,21 @@ void AP_AHRS::update_EKF3(void)
EKF3.getFilterStatus(filt_state); EKF3.getFilterStatus(filt_state);
update_notify_from_filter_status(filt_state); update_notify_from_filter_status(filt_state);
} }
/*
if we now have an origin then set in all backends
*/
if (!done_common_origin) {
Location new_origin;
if (EKF3.getOriginLLH(new_origin)) {
done_common_origin = true;
#if HAL_NAVEKF2_AVAILABLE
EKF2.setOriginLLH(new_origin);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
external.set_origin(new_origin);
#endif
}
}
} }
} }
#endif #endif
@ -699,6 +730,22 @@ void AP_AHRS::update_external(void)
if (_active_EKF_type() == EKFType::EXTERNAL) { if (_active_EKF_type() == EKFType::EXTERNAL) {
copy_estimates_from_backend_estimates(external_estimates); copy_estimates_from_backend_estimates(external_estimates);
} }
/*
if we now have an origin then set in all backends
*/
if (!done_common_origin) {
Location new_origin;
if (external.get_origin(new_origin)) {
done_common_origin = true;
#if HAL_NAVEKF2_AVAILABLE
EKF2.setOriginLLH(new_origin);
#endif
#if HAL_NAVEKF3_AVAILABLE
EKF3.setOriginLLH(new_origin);
#endif
}
}
} }
#endif // AP_AHRS_EXTERNAL_ENABLED #endif // AP_AHRS_EXTERNAL_ENABLED
@ -1412,6 +1459,9 @@ bool AP_AHRS::set_origin(const Location &loc)
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
const bool ret3 = EKF3.setOriginLLH(loc); const bool ret3 = EKF3.setOriginLLH(loc);
#endif #endif
#if AP_AHRS_EXTERNAL_ENABLED
const bool ret_ext = external.set_origin(loc);
#endif
// return success if active EKF's origin was set // return success if active EKF's origin was set
bool success = false; bool success = false;
@ -1441,7 +1491,7 @@ bool AP_AHRS::set_origin(const Location &loc)
#endif #endif
#if AP_AHRS_EXTERNAL_ENABLED #if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL: case EKFType::EXTERNAL:
// don't allow origin set with external AHRS success = ret_ext;
break; break;
#endif #endif
} }

3
libraries/AP_AHRS/AP_AHRS.h
View File

@ -1024,6 +1024,9 @@ private:
bool option_set(Options option) const { bool option_set(Options option) const {
return (_options & uint16_t(option)) != 0; return (_options & uint16_t(option)) != 0;
} }
// true when we have completed the common origin setup
bool done_common_origin;
}; };
namespace AP { namespace AP {