diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 97e863fa66..7d9e6ffee0 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -900,6 +900,7 @@ private: void pre_arm_rc_checks(); bool pre_arm_gps_checks(bool display_failure); bool pre_arm_ekf_attitude_check(); + bool pre_arm_terrain_check(); bool arm_checks(bool display_failure, bool arming_from_gcs); void init_disarm_motors(); void motors_output(); diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index e623abc8b7..137a184c2d 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -332,6 +332,12 @@ bool Copter::pre_arm_checks(bool display_failure) return false; } #endif // HELI_FRAME + + // check for missing terrain data + if (!pre_arm_terrain_check()) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data"); + return false; + } } // check throttle is above failsafe throttle @@ -486,6 +492,20 @@ bool Copter::pre_arm_ekf_attitude_check() return filt_status.flags.attitude; } +// check we have required terrain data +bool Copter::pre_arm_terrain_check() +{ + // succeed if not using terrain data + if (!terrain_use()) { + return true; + } + + // show terrain statistics + uint16_t terr_pending, terr_loaded; + terrain.get_statistics(terr_pending, terr_loaded); + return (terr_pending <= 0); +} + // 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 @@ -636,6 +656,14 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) } } + // check for missing terrain data + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { + if (!pre_arm_terrain_check()) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Terrain data"); + return false; + } + } + // check throttle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { // check throttle is not too low - must be above failsafe throttle