ardupilot/ArduCopter/commands.cpp

150 lines
4.2 KiB
C++
Raw Normal View History

#include "Copter.h"
// checks if we should update ahrs/RTL home position from the EKF
void Copter::update_home_from_EKF()
{
// exit immediately if home already set
2018-03-15 22:23:28 -03:00
if (ahrs.home_is_set()) {
return;
}
// special logic if home is set in-flight
if (motors->armed()) {
set_home_to_current_location_inflight();
} else {
// move home to current ekf location (this will set home_state to HOME_SET)
set_home_to_current_location(false);
}
}
// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically
void Copter::set_home_to_current_location_inflight() {
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
const struct Location &ekf_origin = inertial_nav.get_origin();
temp_loc.alt = ekf_origin.alt;
if (!set_home(temp_loc, false)) {
return;
}
// we have successfully set AHRS home, set it for SmartRTL
#if MODE_SMARTRTL_ENABLED == ENABLED
g2.smart_rtl.set_home(true);
#endif
}
}
// set_home_to_current_location - set home to current GPS location
bool Copter::set_home_to_current_location(bool lock) {
// get current location from EKF
Location temp_loc;
if (inertial_nav.get_location(temp_loc)) {
if (!set_home(temp_loc, lock)) {
return false;
}
// we have successfully set AHRS home, set it for SmartRTL
#if MODE_SMARTRTL_ENABLED == ENABLED
g2.smart_rtl.set_home(true);
#endif
return true;
}
return false;
}
2014-03-31 04:07:46 -03:00
// 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, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
2014-03-31 04:07:46 -03:00
// check EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
// check home is close to EKF origin
if (far_from_EKF_origin(loc)) {
return false;
}
const bool home_was_set = ahrs.home_is_set();
// set ahrs home (used for RTL)
2014-03-31 04:07:46 -03:00
ahrs.set_home(loc);
2012-08-16 21:50:03 -03:00
// init inav and compass declination
if (!home_was_set) {
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
2018-03-15 22:23:28 -03:00
Log_Write_Event(DATA_SET_HOME);
#if MODE_AUTO_ENABLED == ENABLED
// 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);
}
}
#endif
2014-02-27 22:16:33 -04:00
}
2012-08-16 21:50:03 -03:00
// lock home position
if (lock) {
ahrs.lock_home();
}
2015-07-05 05:25:28 -03:00
// log ahrs home and ekf origin dataflash
ahrs.Log_Write_Home_And_Origin();
2015-07-05 05:25:28 -03:00
// send new home and ekf origin to GCS
gcs().send_home();
gcs().send_ekf_origin();
// return success
return true;
}
// far_from_EKF_origin - checks if a location is too far from the EKF origin
// returns true if too far
bool Copter::far_from_EKF_origin(const Location& loc)
{
// check distance to EKF origin
const struct Location &ekf_origin = inertial_nav.get_origin();
if (get_distance(ekf_origin, loc) > EKF_ORIGIN_MAX_DIST_M) {
return true;
}
// close enough to origin
return false;
}
// checks if we should update ahrs/RTL home position from GPS
void Copter::set_system_time_from_GPS()
{
// exit immediately if system time already set
if (ap.system_time_set) {
return;
}
// if we have a 3d lock and valid location
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// set system clock for log timestamps
uint64_t gps_timestamp = gps.time_epoch_usec();
hal.util->set_system_clock(gps_timestamp);
// update signing timestamp
GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
ap.system_time_set = true;
Log_Write_Event(DATA_SYSTEM_TIME_SET);
}
}