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

AP_AHRS: move set_home method into correct .cpp file 

NFC, just moving the code in prepration for allowing DCM to be compiled out
This commit is contained in:
Peter Barker 2023-09-18 21:28:04 +10:00 committed by Andrew Tridgell
parent f8a4dd02d9
commit 622b83da9e
2 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -2640,6 +2640,48 @@ bool AP_AHRS::_get_origin(Location &ret) const
return false;
}
bool AP_AHRS::set_home(const Location &loc)
{
WITH_SEMAPHORE(_rsem);
// check location is valid
if (loc.lat == 0 && loc.lng == 0 && loc.alt == 0) {
return false;
}
if (!loc.check_latlng()) {
return false;
}
// home must always be global frame at the moment as .alt is
// accessed directly by the vehicles and they may not be rigorous
// in checking the frame type.
Location tmp = loc;
if (!tmp.change_alt_frame(Location::AltFrame::ABSOLUTE)) {
return false;
}
#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
if (!_home_is_set) {
// record home is set
AP::logger().Write_Event(LogEvent::SET_HOME);
}
#endif
_home = tmp;
_home_is_set = true;
Log_Write_Home_And_Origin();
// send new home and ekf origin to GCS
GCS_SEND_MESSAGE(MSG_HOME);
GCS_SEND_MESSAGE(MSG_ORIGIN);
AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
pd.home_lat = loc.lat;
pd.home_lon = loc.lng;
pd.home_alt_cm = loc.alt;
return true;
}
// 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 false when no limiting is required

42
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1121,48 +1121,6 @@ bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, fl
return false;
}
bool AP_AHRS::set_home(const Location &loc)
{
WITH_SEMAPHORE(_rsem);
// check location is valid
if (loc.lat == 0 && loc.lng == 0 && loc.alt == 0) {
return false;
}
if (!loc.check_latlng()) {
return false;
}
// home must always be global frame at the moment as .alt is
// accessed directly by the vehicles and they may not be rigorous
// in checking the frame type.
Location tmp = loc;
if (!tmp.change_alt_frame(Location::AltFrame::ABSOLUTE)) {
return false;
}
#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
if (!_home_is_set) {
// record home is set
AP::logger().Write_Event(LogEvent::SET_HOME);
}
#endif
_home = tmp;
_home_is_set = true;
Log_Write_Home_And_Origin();
// send new home and ekf origin to GCS
GCS_SEND_MESSAGE(MSG_HOME);
GCS_SEND_MESSAGE(MSG_ORIGIN);
AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
pd.home_lat = loc.lat;
pd.home_lon = loc.lng;
pd.home_alt_cm = loc.alt;
return true;
}
/*
check if the AHRS subsystem is healthy
*/