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(); }