diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index d0c38b6987..26a8c157c6 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -101,18 +101,18 @@ void Copter::set_failsafe_gcs(bool b) // --------------------------------------------- -void Copter::set_pre_arm_check(bool b) +void AP_Arming_Copter::set_pre_arm_check(bool b) { - if(ap.pre_arm_check != b) { - ap.pre_arm_check = b; + if(copter.ap.pre_arm_check != b) { + copter.ap.pre_arm_check = b; AP_Notify::flags.pre_arm_check = b; } } -void Copter::set_pre_arm_rc_check(bool b) +void AP_Arming_Copter::set_pre_arm_rc_check(bool b) { - if(ap.pre_arm_rc_check != b) { - ap.pre_arm_rc_check = b; + if(copter.ap.pre_arm_rc_check != b) { + copter.ap.pre_arm_rc_check = b; } } diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 2d472b0181..a84c70fb1a 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -482,7 +482,7 @@ void Copter::one_hz_loop() Log_Write_Data(DATA_AP_STATE, ap.value); } - update_arming_checks(); + arming.update(); if (!motors->armed()) { // make it possible to change ahrs orientation at runtime during initial config diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5a384ead58..df6da75a64 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -90,6 +90,7 @@ #include // Pilot input handling library #include // Heli specific pilot input handling library #include +#include // Configuration #include "defines.h" @@ -97,6 +98,7 @@ #include "GCS_Mavlink.h" #include "AP_Rally.h" // Rally point library +#include "arming_checks.h" // libraries which are dependent on #defines in defines.h and/or config.h #if SPRAYER == ENABLED @@ -139,6 +141,7 @@ public: #if ADVANCED_FAILSAFE == ENABLED friend class AP_AdvancedFailsafe_Copter; #endif + friend class AP_Arming_Copter; Copter(void); @@ -213,6 +216,9 @@ private: // Mission library AP_Mission mission; + // Arming/Disarming mangement class + AP_Arming_Copter arming {ahrs, barometer, compass, battery, inertial_nav, ins}; + // Optical flow sensor #if OPTFLOW == ENABLED OpticalFlow optflow{ahrs}; @@ -644,18 +650,6 @@ private: void set_failsafe_gcs(bool b); void set_land_complete(bool b); void set_land_complete_maybe(bool b); - void set_pre_arm_check(bool b); - void set_pre_arm_rc_check(bool b); - bool rc_calibration_checks(bool display_failure); - bool gps_checks(bool display_failure); - bool fence_checks(bool display_failure); - bool compass_checks(bool display_failure); - bool ins_checks(bool display_failure); - bool board_voltage_checks(bool display_failure); - bool parameter_checks(bool display_failure); - bool motor_checks(bool display_failure); - bool pilot_throttle_checks(bool display_failure); - bool barometer_checks(bool display_failure); void update_using_interlock(); void set_motor_emergency_stop(bool b); float get_smoothing_gain(); @@ -983,15 +977,6 @@ private: void arm_motors_check(); void auto_disarm_check(); bool init_arm_motors(bool arming_from_gcs); - void update_arming_checks(void); - bool all_arming_checks_passing(bool arming_from_gcs); - bool pre_arm_checks(bool display_failure); - 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 display_failure); - bool pre_arm_proximity_check(bool display_failure); - bool arm_checks(bool display_failure, bool arming_from_gcs); void init_disarm_motors(); void motors_output(); void lost_vehicle_check(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 1f2d97a5d7..f141e2107a 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -435,13 +435,9 @@ const AP_Param::Info Copter::var_info[] = { // @User: Standard GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), - // @Param: ARMING_CHECK - // @DisplayName: Arming check - // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS - // @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Params/Rangefinder, -65:Skip RC, 127:Skip Voltage - // @Bitmask: 0:All,1:Baro,2:Compass,3:GPS,4:INS,5:Parameters+Rangefinder,6:RC,7:Voltage - // @User: Standard - GSCALAR(arming_check, "ARMING_CHECK", ARMING_CHECK_ALL), + // @Group: ARMING_ + // @Path: ../libraries/AP_Arming/AP_Arming.cpp + GOBJECT(arming, "ARMING_", AP_Arming_Copter), // @Param: DISARM_DELAY // @DisplayName: Disarm delay diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 101c2ab5dd..89a7c53e27 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -102,7 +102,7 @@ public: k_param_circle_rate, // deprecated - remove k_param_rangefinder_gain, k_param_ch8_option, - k_param_arming_check, + k_param_arming_check_old, // deprecated - remove k_param_sprayer, k_param_angle_max, k_param_gps_hdop_good, @@ -362,6 +362,7 @@ public: k_param_rtl_climb_min, k_param_rpm_sensor, k_param_autotune_min_d, // 251 + k_param_arming, // 252 - AP_Arming k_param_DataFlash = 253, // 253 - Logging Group // 254,255: reserved @@ -448,7 +449,6 @@ public: AP_Int8 ch10_option; AP_Int8 ch11_option; AP_Int8 ch12_option; - AP_Int8 arming_check; AP_Int8 disarm_delay; AP_Int8 land_repositioning; diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 04931503d8..27cf179c51 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -1,7 +1,7 @@ #include "Copter.h" // performs pre-arm checks. expects to be called at 1hz. -void Copter::update_arming_checks(void) +void AP_Arming_Copter::update(void) { // perform pre-arm checks & display failures every 30 seconds static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2; @@ -18,7 +18,7 @@ void Copter::update_arming_checks(void) } // performs pre-arm checks and arming checks -bool Copter::all_arming_checks_passing(bool arming_from_gcs) +bool AP_Arming_Copter::all_checks_passing(bool arming_from_gcs) { if (pre_arm_checks(true)) { set_pre_arm_check(true); @@ -26,21 +26,22 @@ bool Copter::all_arming_checks_passing(bool arming_from_gcs) return false; } - return ap.pre_arm_check && arm_checks(true, arming_from_gcs); + return copter.ap.pre_arm_check && arm_checks(true, arming_from_gcs); } // perform pre-arm checks and set ap.pre_arm_check flag // return true if the checks pass successfully -bool Copter::pre_arm_checks(bool display_failure) +// NOTE: this does *NOT* call AP_Arming::pre_arm_checks() yet! +bool AP_Arming_Copter::pre_arm_checks(bool display_failure) { // exit immediately if already armed - if (motors->armed()) { + if (copter.motors->armed()) { return true; } // check if motor interlock and Emergency Stop aux switches are used // at the same time. This cannot be allowed. - if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ + if (copter.check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict"); } @@ -51,7 +52,7 @@ bool Copter::pre_arm_checks(bool display_failure) // if it is, switch needs to be in disabled position to arm // otherwise exit immediately. This check to be repeated, // as state can change at any time. - if (ap.using_interlock && ap.motor_interlock_switch) { + if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); } @@ -59,7 +60,7 @@ bool Copter::pre_arm_checks(bool display_failure) } // exit immediately if we've already successfully performed the pre-arm check - if (ap.pre_arm_check) { + if (copter.ap.pre_arm_check) { // run gps checks because results may change and affect LED colour // no need to display failures because arm_checks will do that if the pilot tries to arm pre_arm_gps_checks(false); @@ -67,7 +68,7 @@ bool Copter::pre_arm_checks(bool display_failure) } // succeed if pre arm checks are disabled - if (g.arming_check == ARMING_CHECK_NONE) { + if (checks_to_perform == ARMING_CHECK_NONE) { set_pre_arm_check(true); set_pre_arm_rc_check(true); return true; @@ -85,11 +86,11 @@ bool Copter::pre_arm_checks(bool display_failure) & pilot_throttle_checks(display_failure); } -bool Copter::rc_calibration_checks(bool display_failure) +bool AP_Arming_Copter::rc_calibration_checks(bool display_failure) { // pre-arm rc checks a prerequisite pre_arm_rc_checks(); - if (!ap.pre_arm_rc_check) { + if (!copter.ap.pre_arm_rc_check) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC not calibrated"); } @@ -98,10 +99,10 @@ bool Copter::rc_calibration_checks(bool display_failure) return true; } -bool Copter::barometer_checks(bool display_failure) +bool AP_Arming_Copter::barometer_checks(bool display_failure) { // check Baro - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_BARO)) { // barometer health check if (!barometer.all_healthy()) { if (display_failure) { @@ -115,7 +116,7 @@ bool Copter::barometer_checks(bool display_failure) nav_filter_status filt_status = inertial_nav.get_filter_status(); bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); if (using_baro_ref) { - if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { + if (fabsf(inertial_nav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity"); } @@ -126,10 +127,11 @@ bool Copter::barometer_checks(bool display_failure) return true; } -bool Copter::compass_checks(bool display_failure) +bool AP_Arming_Copter::compass_checks(bool display_failure) { // check Compass - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) { + Compass &compass = _compass; // avoid code churn + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_COMPASS)) { //check if compass has calibrated and requires reboot if (compass.compass_cal_requires_reboot()) { if (display_failure) { @@ -185,7 +187,7 @@ bool Copter::compass_checks(bool display_failure) return true; } -bool Copter::gps_checks(bool display_failure) +bool AP_Arming_Copter::gps_checks(bool display_failure) { // check GPS if (!pre_arm_gps_checks(display_failure)) { @@ -194,11 +196,11 @@ bool Copter::gps_checks(bool display_failure) return true; } -bool Copter::fence_checks(bool display_failure) +bool AP_Arming_Copter::fence_checks(bool display_failure) { #if AC_FENCE == ENABLED // check fence is initialised - if (!fence.pre_arm_check()) { + if (!copter.fence.pre_arm_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check fence"); } @@ -208,10 +210,11 @@ bool Copter::fence_checks(bool display_failure) return true; } -bool Copter::ins_checks(bool display_failure) +bool AP_Arming_Copter::ins_checks(bool display_failure) { // check INS - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { + const AP_InertialSensor &ins = _ins; // avoid code churn + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { // check accelerometers have been calibrated if (!ins.accel_calibrated_ok_all()) { if (display_failure) { @@ -298,12 +301,12 @@ bool Copter::ins_checks(bool display_failure) return true; } -bool Copter::board_voltage_checks(bool display_failure) +bool AP_Arming_Copter::board_voltage_checks(bool display_failure) { #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 // check board voltage - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { if (hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage"); @@ -314,9 +317,11 @@ bool Copter::board_voltage_checks(bool display_failure) #endif #endif + Parameters &g = copter.g; + // check battery voltage - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { - if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { + if (copter.failsafe.battery || (!copter.ap.usb_connected && copter.battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery"); } @@ -327,13 +332,13 @@ bool Copter::board_voltage_checks(bool display_failure) return true; } -bool Copter::parameter_checks(bool display_failure) +bool AP_Arming_Copter::parameter_checks(bool display_failure) { // check various parameter values - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { // ensure ch7 and ch8 have different functions - if (check_duplicate_auxsw()) { + if (copter.check_duplicate_auxsw()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options"); } @@ -341,9 +346,9 @@ bool Copter::parameter_checks(bool display_failure) } // failsafe parameter checks - if (g.failsafe_throttle) { + if (copter.g.failsafe_throttle) { // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 - if (channel_throttle->get_radio_min() <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { + if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE"); } @@ -352,7 +357,7 @@ bool Copter::parameter_checks(bool display_failure) } // lean angle parameter check - if (aparm.angle_max < 1000 || aparm.angle_max > 8000) { + if (copter.aparm.angle_max < 1000 || copter.aparm.angle_max > 8000) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX"); } @@ -360,7 +365,7 @@ bool Copter::parameter_checks(bool display_failure) } // acro balance parameter check - if ((g.acro_balance_roll > attitude_control->get_angle_roll_p().kP()) || (g.acro_balance_pitch > attitude_control->get_angle_pitch_p().kP())) { + if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH"); } @@ -369,7 +374,7 @@ bool Copter::parameter_checks(bool display_failure) #if RANGEFINDER_ENABLED == ENABLED && OPTFLOW == ENABLED // check range finder if optflow enabled - if (optflow.enabled() && !rangefinder.pre_arm_check()) { + if (copter.optflow.enabled() && !copter.rangefinder.pre_arm_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder"); } @@ -379,7 +384,7 @@ bool Copter::parameter_checks(bool display_failure) #if FRAME_CONFIG == HELI_FRAME // check helicopter parameters - if (!motors->parameter_check(display_failure)) { + if (!copter.motors->parameter_check(display_failure)) { return false; } #endif // HELI_FRAME @@ -390,7 +395,7 @@ bool Copter::parameter_checks(bool display_failure) } // check adsb avoidance failsafe - if (failsafe.adsb) { + if (copter.failsafe.adsb) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected"); } @@ -419,12 +424,12 @@ bool Copter::motor_checks(bool display_failure) return true; } -bool Copter::pilot_throttle_checks(bool display_failure) +bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure) { // check throttle is above failsafe throttle // this is near the bottom to allow other failures to be displayed before checking pilot throttle - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { - if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->get_radio_in() < g.failsafe_throttle_value) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { + if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe"); @@ -440,19 +445,24 @@ bool Copter::pilot_throttle_checks(bool display_failure) } // perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag -void Copter::pre_arm_rc_checks() +void AP_Arming_Copter::pre_arm_rc_checks() { // exit immediately if we've already successfully performed the pre-arm rc check - if (ap.pre_arm_rc_check) { + if (copter.ap.pre_arm_rc_check) { return; } // set rc-checks to success if RC checks are disabled - if ((g.arming_check != ARMING_CHECK_ALL) && !(g.arming_check & ARMING_CHECK_RC)) { + if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_RC)) { set_pre_arm_rc_check(true); return; } + RC_Channel *&channel_roll = copter.channel_roll; + RC_Channel *&channel_pitch = copter.channel_pitch; + RC_Channel *&channel_throttle = copter.channel_throttle; + RC_Channel *&channel_yaw = copter.channel_yaw; + // check if radio has been calibrated if (!channel_throttle->min_max_configured()) { return; @@ -483,7 +493,7 @@ void Copter::pre_arm_rc_checks() } // performs pre_arm gps related checks and returns true if passed -bool Copter::pre_arm_gps_checks(bool display_failure) +bool AP_Arming_Copter::pre_arm_gps_checks(bool display_failure) { // always check if inertial nav has started and is ready if (!ahrs.healthy()) { @@ -494,11 +504,11 @@ bool Copter::pre_arm_gps_checks(bool display_failure) } // check if flight mode requires GPS - bool gps_required = mode_requires_GPS(control_mode); + bool gps_required = copter.mode_requires_GPS(copter.control_mode); #if AC_FENCE == ENABLED // if circular fence is enabled we need GPS - if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { + if ((copter.fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { gps_required = true; } #endif @@ -510,7 +520,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure) } // ensure GPS is ok - if (!position_ok()) { + if (!copter.position_ok()) { if (display_failure) { const char *reason = ahrs.prearm_failure_reason(); if (reason) { @@ -527,8 +537,8 @@ bool Copter::pre_arm_gps_checks(bool display_failure) 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) { + ahrs_navekf.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset); + if (mag_variance.length() >= copter.g.fs_ekf_thresh) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance"); } @@ -536,7 +546,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure) } // check home and EKF origin are not too far - if (far_from_EKF_origin(ahrs.get_home())) { + if (copter.far_from_EKF_origin(ahrs.get_home())) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance"); } @@ -545,27 +555,27 @@ bool Copter::pre_arm_gps_checks(bool display_failure) } // return true immediately if gps check is disabled - if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) { + if (!(checks_to_perform == ARMING_CHECK_ALL || checks_to_perform & ARMING_CHECK_GPS)) { AP_Notify::flags.pre_arm_gps_check = true; return true; } #if CONFIG_HAL_BOARD != HAL_BOARD_SITL // check GPS configuration has completed - uint8_t first_unconfigured = gps.first_unconfigured_gps(); + uint8_t first_unconfigured = copter.gps.first_unconfigured_gps(); if (first_unconfigured != AP_GPS::GPS_ALL_CONFIGURED) { if (display_failure) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: GPS %d failing configuration checks", first_unconfigured + 1); - gps.broadcast_first_configuration_failure_reason(); + copter.gps.broadcast_first_configuration_failure_reason(); } return false; } #endif // warn about hdop separately - to prevent user confusion with no gps lock - if (gps.get_hdop() > g.gps_hdop_good) { + if (copter.gps.get_hdop() > copter.g.gps_hdop_good) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP"); } @@ -579,7 +589,7 @@ bool Copter::pre_arm_gps_checks(bool display_failure) } // check ekf attitude is acceptable -bool Copter::pre_arm_ekf_attitude_check() +bool AP_Arming_Copter::pre_arm_ekf_attitude_check() { // get ekf filter status nav_filter_status filt_status = inertial_nav.get_filter_status(); @@ -588,24 +598,24 @@ bool Copter::pre_arm_ekf_attitude_check() } // check we have required terrain data -bool Copter::pre_arm_terrain_check(bool display_failure) +bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure) { #if AP_TERRAIN_AVAILABLE && AC_TERRAIN // succeed if not using terrain data - if (!terrain_use()) { + if (!copter.terrain_use()) { return true; } // check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range // To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check - if ((rangefinder.num_sensors() > 0) && (g.rtl_altitude > rangefinder.max_distance_cm())) { + if ((copter.rangefinder.num_sensors() > 0) && (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm())) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range"); return false; } // show terrain statistics uint16_t terr_pending, terr_loaded; - terrain.get_statistics(terr_pending, terr_loaded); + copter.terrain.get_statistics(terr_pending, terr_loaded); bool have_all_data = (terr_pending <= 0); if (!have_all_data && display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data"); @@ -657,15 +667,17 @@ bool Copter::pre_arm_proximity_check(bool display_failure) // 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 -bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) +bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) { #if LOGGING_ENABLED == ENABLED // start dataflash - start_logging(); + copter.start_logging(); #endif + const AP_InertialSensor &ins = _ins; + // check accels and gyro are healthy - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { //check if accelerometers have calibrated and require reboot if (ins.accel_cal_requires_reboot()) { if (display_failure) { @@ -704,14 +716,14 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) } // check compass health - if (!compass.healthy()) { + if (!_compass.healthy()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass not healthy"); } return false; } - if (compass.is_calibrating()) { + if (_compass.is_calibrating()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running"); } @@ -719,15 +731,17 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) } //check if compass has calibrated and requires reboot - if (compass.compass_cal_requires_reboot()) { + if (_compass.compass_cal_requires_reboot()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot"); } return false; } + control_mode_t control_mode = copter.control_mode; + // always check if the current mode allows arming - if (!mode_allows_arming(control_mode, arming_from_gcs)) { + if (!copter.mode_allows_arming(control_mode, arming_from_gcs)) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable"); } @@ -746,28 +760,28 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // if we are using motor interlock switch and it's enabled, fail to arm // skip check in Throw mode which takes control of the motor interlock - if (ap.using_interlock && motors->get_interlock()) { + if (copter.ap.using_interlock && copter.motors->get_interlock()) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled"); return false; } // if we are not using Emergency Stop switch option, force Estop false to ensure motors // can run normally - if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ - set_motor_emergency_stop(false); + if (!copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ + copter.set_motor_emergency_stop(false); // if we are using motor Estop switch, it must not be in Estop position - } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ + } else if (copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && copter.ap.motor_emergency_stop){ gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); return false; } // succeed if arming checks are disabled - if (g.arming_check == ARMING_CHECK_NONE) { + if (checks_to_perform == ARMING_CHECK_NONE) { return true; } // baro checks - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_BARO)) { // baro health check if (!barometer.all_healthy()) { if (display_failure) { @@ -780,7 +794,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // that may differ from the baro height due to baro drift. nav_filter_status filt_status = inertial_nav.get_filter_status(); bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); - if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) { + if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity"); } @@ -790,7 +804,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) #if AC_FENCE == ENABLED // check vehicle is within fence - if (!fence.pre_arm_check()) { + if (!copter.fence.pre_arm_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: check fence"); } @@ -799,8 +813,8 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) #endif // check lean angle - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { - if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { + if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > copter.aparm.angle_max) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning"); } @@ -809,8 +823,8 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) } // check battery voltage - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { - if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) { + if (copter.failsafe.battery || (!copter.ap.usb_connected && copter.battery.exhausted(copter.g.fs_batt_voltage, copter.g.fs_batt_mah))) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery"); } @@ -819,15 +833,15 @@ 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 ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { if (!pre_arm_terrain_check(display_failure)) { return false; } } // check adsb - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { - if (failsafe.adsb) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { + if (copter.failsafe.adsb) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: ADSB threat detected"); } @@ -836,9 +850,9 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) } // check throttle - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { // check throttle is not too low - must be above failsafe throttle - if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->get_radio_in() < g.failsafe_throttle_value) { + if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe"); @@ -852,7 +866,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // check throttle is not too high - skips checks if arming from GCS in Guided if (!(arming_from_gcs && (control_mode == GUIDED || control_mode == GUIDED_NOGPS))) { // above top of deadband is too always high - if (get_pilot_desired_climb_rate(channel_throttle->get_control_in()) > 0.0f) { + if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); @@ -863,7 +877,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) return false; } // in manual modes throttle must be at zero - if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->get_control_in() > 0) { + if ((copter.mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && copter.channel_throttle->get_control_in() > 0) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); @@ -887,3 +901,13 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) // if we've gotten this far all is ok return true; } + +enum HomeState AP_Arming_Copter::home_status() const +{ + return copter.ap.home_state; +} + +void AP_Arming_Copter::gcs_send_text(MAV_SEVERITY severity, const char *str) +{ + copter.gcs_send_text(severity, str); +} diff --git a/ArduCopter/arming_checks.h b/ArduCopter/arming_checks.h new file mode 100644 index 0000000000..6591ad3d92 --- /dev/null +++ b/ArduCopter/arming_checks.h @@ -0,0 +1,55 @@ +#pragma once + +#include + +class AP_Arming_Copter : public AP_Arming +{ +public: + AP_Arming_Copter(const AP_AHRS_NavEKF &ahrs_ref, const AP_Baro &baro, Compass &compass, + const AP_BattMonitor &battery, const AP_InertialNav_NavEKF &inav, + const AP_InertialSensor &ins) : + AP_Arming(ahrs_ref, baro, compass, battery), + inertial_nav(inav), + _ins(ins), + ahrs_navekf(ahrs_ref) + { + } + + void update(void); + bool all_checks_passing(bool arming_from_gcs); + void pre_arm_rc_checks(); + +protected: + + bool pre_arm_checks(bool display_failure); + bool pre_arm_gps_checks(bool display_failure); + bool pre_arm_ekf_attitude_check(); + bool pre_arm_terrain_check(bool display_failure); + bool pre_arm_proximity_check(bool display_failure); + bool arm_checks(bool display_failure, bool arming_from_gcs); + + // NOTE! the following check functions *DO NOT* call into AP_Arming! + bool gps_checks(bool display_failure); + bool fence_checks(bool display_failure); + bool compass_checks(bool display_failure); + bool ins_checks(bool display_failure) override; + bool board_voltage_checks(bool display_failure); + bool parameter_checks(bool display_failure); + bool pilot_throttle_checks(bool display_failure); + bool barometer_checks(bool display_failure); + bool rc_calibration_checks(bool display_failure); + + void set_pre_arm_check(bool b); + void set_pre_arm_rc_check(bool b); + + enum HomeState home_status() const override; + +private: + + void gcs_send_text(MAV_SEVERITY severity, const char *str); + + const AP_InertialNav_NavEKF &inertial_nav; + const AP_InertialSensor &_ins; + const AP_AHRS_NavEKF &ahrs_navekf; + +}; diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index bf9aeefdf2..fc5f6fb141 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -56,7 +56,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan) } // check if radio is calibrated - pre_arm_rc_checks(); + arming.pre_arm_rc_checks(); if (!ap.pre_arm_rc_check) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated"); ap.compass_mot = false; diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 521abdf638..cb5d39404c 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -20,7 +20,7 @@ void Copter::esc_calibration_startup_check() { #if FRAME_CONFIG != HELI_FRAME // exit immediately if pre-arm rc checks fail - pre_arm_rc_checks(); + arming.pre_arm_rc_checks(); if (!ap.pre_arm_rc_check) { // clear esc flag for next time if ((g.esc_calibrate != ESCCAL_NONE) && (g.esc_calibrate != ESCCAL_DISABLED)) { diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 8e140a3e6e..8469d4aaa7 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -73,7 +73,7 @@ void Copter::motor_test_output() bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) { // check rc has been calibrated - pre_arm_rc_checks(); + arming.pre_arm_rc_checks(); if(check_rc && !ap.pre_arm_rc_check) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated"); return false; diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 7dbd330ede..e8813d247c 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -137,7 +137,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) } // run pre-arm-checks and display failures - if (!all_arming_checks_passing(arming_from_gcs)) { + if (!arming.all_checks_passing(arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 7ddc2680a9..8ace7de26e 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -68,7 +68,7 @@ void Copter::init_rc_out() esc_calibration_startup_check(); // enable output to motors - pre_arm_rc_checks(); + arming.pre_arm_rc_checks(); if (ap.pre_arm_rc_check) { enable_motor_output(); }