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

AP_ExternalAHRS: added set_origin() call 

and remove code that aligns origin to AHRS, this will be handled by
the AHRS common origin logic
This commit is contained in:
Andrew Tridgell 2024-08-16 07:33:54 +10:00
parent 3444de06b1
commit 91a5cfea6b
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View File

@ -180,6 +180,17 @@ bool AP_ExternalAHRS::get_origin(Location &loc)
return false; return false;
} }
bool AP_ExternalAHRS::set_origin(const Location &loc)
{
WITH_SEMAPHORE(state.sem);
if (state.have_origin) {
return false;
}
state.origin = loc;
state.have_origin = true;
return true;
}
bool AP_ExternalAHRS::get_location(Location &loc) bool AP_ExternalAHRS::get_location(Location &loc)
{ {
if (!state.have_location) { if (!state.have_location) {
@ -312,19 +323,7 @@ void AP_ExternalAHRS::update(void)
backend->update(); backend->update();
} }
/*
if backend has not supplied an origin and AHRS has an origin
then use that origin so we get a common origin for minimum
disturbance when switching backends
*/
WITH_SEMAPHORE(state.sem); WITH_SEMAPHORE(state.sem);
if (!state.have_origin) {
Location origin;
if (AP::ahrs().get_origin(origin)) {
state.origin = origin;
state.have_origin = true;
}
}
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED
const uint32_t now_ms = AP_HAL::millis(); const uint32_t now_ms = AP_HAL::millis();
if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) { if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) {

1
libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View File

@ -109,6 +109,7 @@ public:
bool initialised(void) const; bool initialised(void) const;
bool get_quaternion(Quaternion &quat); bool get_quaternion(Quaternion &quat);
bool get_origin(Location &loc); bool get_origin(Location &loc);
bool set_origin(const Location &loc);
bool get_location(Location &loc); bool get_location(Location &loc);
Vector2f get_groundspeed_vector(); Vector2f get_groundspeed_vector();
bool get_velocity_NED(Vector3f &vel); bool get_velocity_NED(Vector3f &vel);