// 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; } // 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) ahrs.set_home(loc); // 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 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 } // lock home position if (lock) { ahrs.lock_home(); } // log ahrs home and ekf origin dataflash ahrs.Log_Write_Home_And_Origin(); // send new home and ekf origin to GCS gcs().send_home(); gcs().send_ekf_origin(); // return success return true; }
void Copter::do_set_home(const AP_Mission::Mission_Command& cmd) { if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) { set_home_to_current_location(); } else { if (!far_from_EKF_origin(cmd.content.location)) { set_home(cmd.content.location); } } }
void Sub::do_set_home(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) { if (!set_home_to_current_location(false)) { // silently ignore this failure } } else { if (!far_from_EKF_origin(cmd.content.location)) { if (!set_home(cmd.content.location, false)) { // silently ignore this failure } } } }
// performs pre_arm gps related checks and returns true if passed bool Copter::pre_arm_gps_checks(bool display_failure) { // always check if inertial nav has started and is ready if (!ahrs.healthy()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks"); } return false; } // check if flight mode requires GPS bool gps_required = mode_requires_GPS(control_mode); #if AC_FENCE == ENABLED // if circular fence is enabled we need GPS if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { gps_required = true; } #endif // return true if GPS is not required if (!gps_required) { AP_Notify::flags.pre_arm_gps_check = true; return true; } // ensure GPS is ok if (!position_ok()) { if (display_failure) { const char *reason = ahrs.prearm_failure_reason(); if (reason) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason); } else { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix"); } } AP_Notify::flags.pre_arm_gps_check = false; return false; } // check EKF compass variance is below failsafe threshold float vel_variance, pos_variance, hgt_variance, tas_variance; Vector3f mag_variance; Vector2f offset; ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset); if (mag_variance.length() >= g.fs_ekf_thresh) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance"); } return false; } // check home and EKF origin are not too far if (far_from_EKF_origin(ahrs.get_home())) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance"); } AP_Notify::flags.pre_arm_gps_check = false; return false; } // return true immediately if gps check is disabled if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) { AP_Notify::flags.pre_arm_gps_check = true; return true; } // warn about hdop separately - to prevent user confusion with no gps lock if (gps.get_hdop() > g.gps_hdop_good) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP"); } AP_Notify::flags.pre_arm_gps_check = false; return false; } // if we got here all must be ok AP_Notify::flags.pre_arm_gps_check = true; return true; }
// performs pre_arm gps related checks and returns true if passed bool Copter::pre_arm_gps_checks(bool display_failure) { // always check if inertial nav has started and is ready if(!ahrs.get_NavEKF().healthy()) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Waiting for Nav Checks")); } return false; } // return true immediately if gps check is disabled if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) { AP_Notify::flags.pre_arm_gps_check = true; return true; } // check if flight mode requires GPS bool gps_required = mode_requires_GPS(control_mode); #if AC_FENCE == ENABLED // if circular fence is enabled we need GPS if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { gps_required = true; } #endif // return true if GPS is not required if (!gps_required) { AP_Notify::flags.pre_arm_gps_check = true; return true; } // ensure GPS is ok if (!position_ok()) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix")); } AP_Notify::flags.pre_arm_gps_check = false; return false; } // check home and EKF origin are not too far if (far_from_EKF_origin(ahrs.get_home())) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: EKF-home variance")); } AP_Notify::flags.pre_arm_gps_check = false; return false; } // warn about hdop separately - to prevent user confusion with no gps lock if (gps.get_hdop() > g.gps_hdop_good) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP")); } AP_Notify::flags.pre_arm_gps_check = false; return false; } // if we got here all must be ok AP_Notify::flags.pre_arm_gps_check = true; return true; }