mirror of https://github.com/ArduPilot/ardupilot
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:
parent
f8a4dd02d9
commit
622b83da9e
|
@ -2640,6 +2640,48 @@ bool AP_AHRS::_get_origin(Location &ret) const
|
||||||
return false;
|
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
|
// 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
|
// this is used to limit height during optical flow navigation
|
||||||
// it will return false when no limiting is required
|
// it will return false when no limiting is required
|
||||||
|
|
|
@ -1121,48 +1121,6 @@ bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, fl
|
||||||
return false;
|
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
|
check if the AHRS subsystem is healthy
|
||||||
*/
|
*/
|
||||||
|
|
Loading…
Reference in New Issue