bool AP_AHRS_DCM::set_home(const Location &loc)
{
// 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;
}
_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;
}
// run this at setup on the ground
// -------------------------------
void Rover::init_home()
{
if (!have_position) {
// we need position information
return;
}
gcs_send_text(MAV_SEVERITY_INFO, "init home");
ahrs.set_home(gps.location());
home_is_set = true;
Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(gps.location());
// Save Home to EEPROM
mission.write_home_to_storage();
// Save prev loc
// -------------
next_WP = prev_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
}
// sets ahrs home to specified location
// returns true if home location set successfully
bool Rover::set_home(const Location& loc, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0 && loc.alt == 0) {
return false;
}
if (!check_latlng(loc)) {
return false;
}
// check if EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
// set ahrs home
ahrs.set_home(loc);
// init compass declination
if (home_is_set == HOME_UNSET) {
// record home is set
home_is_set = HOME_SET_NOT_LOCKED;
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
}
}
}
// lock home position
if (lock) {
home_is_set = HOME_SET_AND_LOCKED;
}
// Save Home to EEPROM
mission.write_home_to_storage();
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// send new home and ekf origin to GCS
gcs().send_home(loc);
gcs().send_ekf_origin(loc);
// send text of home position to ground stations
gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
static_cast<double>(loc.lat * 1.0e-7f),
static_cast<double>(loc.lng * 1.0e-7f),
static_cast<double>(loc.alt * 0.01f));
// return success
return true;
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is
not pushed
*/
void Plane::update_home()
{
if (home_is_set == HOME_SET_NOT_LOCKED) {
ahrs.set_home(gps.location());
Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(gps.location());
}
barometer.update_calibration();
}
void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
init_home();
} else {
ahrs.set_home(cmd.content.location);
home_is_set = HOME_SET_NOT_LOCKED;
Log_Write_Home_And_Origin();
}
}
void Rover::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if(cmd.p1 == 1 && have_position) {
init_home();
} else {
ahrs.set_home(cmd.content.location);
home_is_set = HOME_SET_NOT_LOCKED;
Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(cmd.content.location);
}
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is
not pushed
*/
void Rover::update_home()
{
if (home_is_set == HOME_SET_NOT_LOCKED) {
Location loc;
if (ahrs.get_position(loc)) {
if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) {
ahrs.set_home(loc);
Log_Write_Home_And_Origin();
gcs().send_home(gps.location());
}
}
}
barometer.update_calibration();
}
// run this at setup on the ground
// -------------------------------
void Plane::init_home()
{
gcs().send_text(MAV_SEVERITY_INFO, "Init HOME");
ahrs.set_home(gps.location());
home_is_set = HOME_SET_NOT_LOCKED;
Log_Write_Home_And_Origin();
gcs().send_home(gps.location());
// Save Home to EEPROM
mission.write_home_to_storage();
// Save prev loc
// -------------
next_WP_loc = prev_WP_loc = home;
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is
not pushed
*/
void Plane::update_home()
{
if (home_is_set == HOME_SET_NOT_LOCKED) {
Location loc = gps.location();
Location origin;
// if an EKF origin is available then we leave home equal to
// the height of that origin. This ensures that our relative
// height calculations are using the same origin
if (ahrs.get_origin(origin)) {
loc.alt = origin.alt;
}
ahrs.set_home(loc);
Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(gps.location());
}
barometer.update_calibration();
}
// run this at setup on the ground
// -------------------------------
void Plane::init_home()
{
gcs_send_text(MAV_SEVERITY_INFO, "Init HOME");
ahrs.set_home(gps.location());
home_is_set = HOME_SET_NOT_LOCKED;
Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(gps.location());
gcs_send_text_fmt(MAV_SEVERITY_INFO, "GPS alt: %lu", (unsigned long)home.alt);
// Save Home to EEPROM
mission.write_home_to_storage();
// Save prev loc
// -------------
next_WP_loc = prev_WP_loc = home;
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is
not pushed
*/
void Plane::update_home()
{
if ((g2.home_reset_threshold == -1) ||
((g2.home_reset_threshold > 0) &&
(fabsf(barometer.get_altitude()) > g2.home_reset_threshold))) {
// don't auto-update if we have changed barometer altitude
// significantly. This allows us to cope with slow baro drift
// but not re-do home and the baro if we have changed height
// significantly
return;
}
if (home_is_set == HOME_SET_NOT_LOCKED) {
Location loc;
if(ahrs.get_position(loc)) {
ahrs.set_home(loc);
Log_Write_Home_And_Origin();
gcs().send_home(loc);
}
}
barometer.update_calibration();
}
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Copter::set_home(const Location& loc)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
// set ahrs home (used for RTL)
ahrs.set_home(loc);
// init inav and compass declination
if (ap.home_state == HOME_UNSET) {
// Set compass declination automatically
if (g.compass_enabled) {
compass.set_initial_location(gps.location().lat, gps.location().lng);
}
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
set_home_state(HOME_SET_NOT_LOCKED);
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
}
}
}
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// send new home location to GCS
GCS_MAVLINK::send_home_all(loc);
// return success
return true;
}
// sets ekf_origin if it has not been set.
// should only be used when there is no GPS to provide an absolute position
void Rover::set_ekf_origin(const Location& loc)
{
// check location is valid
if (!check_latlng(loc)) {
return;
}
// check if EKF origin has already been set
Location ekf_origin;
if (ahrs.get_origin(ekf_origin)) {
return;
}
if (!ahrs.set_origin(loc)) {
return;
}
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// send ekf origin to GCS
gcs().send_ekf_origin(loc);
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is
not pushed
*/
void Plane::update_home()
{
if (fabsf(barometer.get_altitude()) > 2) {
// don't auto-update if we have changed barometer altitude
// significantly. This allows us to cope with slow baro drift
// but not re-do home and the baro if we have changed height
// significantly
return;
}
if (home_is_set == HOME_SET_NOT_LOCKED) {
Location loc = gps.location();
Location origin;
// if an EKF origin is available then we leave home equal to
// the height of that origin. This ensures that our relative
// height calculations are using the same origin
if (ahrs.get_origin(origin)) {
loc.alt = origin.alt;
}
ahrs.set_home(loc);
Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(gps.location());
}
barometer.update_calibration();
}
