diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index a21f201458..016f827f31 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -63,6 +63,7 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) & oa_checks(display_failure) & gcs_failsafe_check(display_failure) & winch_checks(display_failure) + & alt_checks(display_failure) & AP_Arming::pre_arm_checks(display_failure); } @@ -605,6 +606,18 @@ bool AP_Arming_Copter::winch_checks(bool display_failure) const return true; } +// performs altitude checks. returns true if passed +bool AP_Arming_Copter::alt_checks(bool display_failure) +{ + // always EKF altitude estimate + if (!copter.flightmode->has_manual_throttle() && !copter.ekf_alt_ok()) { + check_failed(display_failure, "Need Alt Estimate"); + return false; + } + + return true; +} + // arm_checks - perform final checks before arming // always called just before arming. Return true if ok to arm // has side-effect that logging is started @@ -729,8 +742,14 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) bool AP_Arming_Copter::mandatory_checks(bool display_failure) { // call mandatory gps checks and update notify status because regular gps checks will not run - const bool result = mandatory_gps_checks(display_failure); + bool result = mandatory_gps_checks(display_failure); AP_Notify::flags.pre_arm_gps_check = result; + + // call mandatory alt check + if (!alt_checks(display_failure)) { + result = false; + } + return result; } diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index 9e7c9e914d..b55a9de865 100644 --- a/ArduCopter/AP_Arming.h +++ b/ArduCopter/AP_Arming.h @@ -51,6 +51,7 @@ protected: bool mandatory_gps_checks(bool display_failure); bool gcs_failsafe_check(bool display_failure); bool winch_checks(bool display_failure) const; + bool alt_checks(bool display_failure); void set_pre_arm_check(bool b); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index a4d1773fef..7e70dfd5de 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -863,6 +863,7 @@ private: bool position_ok() const; bool ekf_position_ok() const; bool optflow_position_ok() const; + bool ekf_alt_ok() const; void update_auto_armed(); bool should_log(uint32_t mask); MAV_TYPE get_frame_mav_type(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 619f1baab1..5a71cf7787 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -250,6 +250,17 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) return false; } + // check for valid altitude if old mode did not require it but new one does + // we only want to stop changing modes if it could make things worse + if (!ignore_checks && + !copter.ekf_alt_ok() && + flightmode->has_manual_throttle() && + !new_flightmode->has_manual_throttle()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + if (!new_flightmode->init(ignore_checks)) { gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name()); AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 6a1ed570b8..174eeb2355 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -385,6 +385,21 @@ bool Copter::optflow_position_ok() const } } +// returns true if the ekf has a good altitude estimate (required for modes which do AltHold) +bool Copter::ekf_alt_ok() const +{ + if (!ahrs.have_inertial_nav()) { + // do not allow alt control with only dcm + return false; + } + + // with EKF use filter status and ekf check + nav_filter_status filt_status = inertial_nav.get_filter_status(); + + // require both vertical velocity and position + return (filt_status.flags.vert_vel && filt_status.flags.vert_pos); +} + // update_auto_armed - update status of auto_armed flag void Copter::update_auto_armed() {