aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: start conversion to AP_Arming_Copter

Browse Source
This commit is contained in:
Peter Barker 2017-01-09 16:45:48 +09:00 committed by Randy Mackay
parent ae18c25070
commit bd6ffc025e
12 changed files with 185 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/AP_State.cpp
View File

@ -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;
}
}

2
ArduCopter/ArduCopter.cpp
View File

@ -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

27
ArduCopter/Copter.h
View File

@ -90,6 +90,7 @@
#include <AC_InputManager/AC_InputManager.h> // Pilot input handling library
#include <AC_InputManager/AC_InputManager_Heli.h> // Heli specific pilot input handling library
#include <AP_Button/AP_Button.h>
#include <AP_Arming/AP_Arming.h>
// 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();

10
ArduCopter/Parameters.cpp
View File

@ -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

4
ArduCopter/Parameters.h
View File

@ -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;

190
ArduCopter/arming_checks.cpp
View File

@ -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);
}

55
ArduCopter/arming_checks.h Normal file
View File

@ -0,0 +1,55 @@
#pragma once
#include <AP_Arming/AP_Arming.h>
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;
};

2
ArduCopter/compassmot.cpp
View File

@ -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;

2
ArduCopter/esc_calibration.cpp
View File

@ -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)) {

2
ArduCopter/motor_test.cpp
View File

@ -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;

2
ArduCopter/motors.cpp
View File

@ -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;

2
ArduCopter/radio.cpp
View File

@ -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();
}