diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index a1f614606a..1c7fd4d784 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #if HAL_WITH_UAVCAN #include @@ -110,10 +111,10 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = { // @Param: CHECK // @DisplayName: Arm Checks to Perform (bitmask) // @Description: Checks prior to arming motor. This is a bitmask of checks that will be performed before allowing arming. The default is no checks, allowing arming at any time. You can select whatever checks you prefer by adding together the values of each check type to set this parameter. For example, to only allow arming when you have GPS lock and no RC failsafe you would set ARMING_CHECK to 72. For most users it is recommended that you set this to 1 to enable all checks. - // @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System,16384:Mission,32768:RangeFinder - // @Values{Plane}: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,512:Airspeed,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System,16384:Mission,32768:RangeFinder - // @Bitmask: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder - // @Bitmask{Plane}: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder + // @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System,16384:Mission,32768:RangeFinder,65536:Camera + // @Values{Plane}: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,512:Airspeed,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System,16384:Mission,32768:RangeFinder,65536:Camera + // @Bitmask: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder,16:Camera + // @Bitmask{Plane}: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder,16:Camera // @User: Standard AP_GROUPINFO("CHECK", 8, AP_Arming, checks_to_perform, ARMING_CHECK_ALL), @@ -829,6 +830,26 @@ bool AP_Arming::fence_checks(bool display_failure) return false; } +bool AP_Arming::camera_checks(bool display_failure) +{ + if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_CAMERA)) { +#if HAL_RUNCAM_ENABLED + AP_RunCam *runcam = AP::runcam(); + if (runcam == nullptr) { + return true; + } + + // check camera is ready + char fail_msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + if (!runcam->pre_arm_check(fail_msg, ARRAY_SIZE(fail_msg))) { + check_failed(ARMING_CHECK_CAMERA, display_failure, "%s", fail_msg); + return false; + } +#endif + } + return true; +} + bool AP_Arming::pre_arm_checks(bool report) { #if !APM_BUILD_TYPE(APM_BUILD_ArduCopter) @@ -853,7 +874,8 @@ bool AP_Arming::pre_arm_checks(bool report) & board_voltage_checks(report) & system_checks(report) & can_checks(report) - & proximity_checks(report); + & proximity_checks(report) + & camera_checks(report); } bool AP_Arming::arm_checks(AP_Arming::Method method) diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 80396e7239..b76c6e1a47 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -33,6 +33,7 @@ public: ARMING_CHECK_SYSTEM = (1U << 13), ARMING_CHECK_MISSION = (1U << 14), ARMING_CHECK_RANGEFINDER = (1U << 15), + ARMING_CHECK_CAMERA = (1U << 16), }; enum class Method { @@ -124,6 +125,8 @@ protected: bool fence_checks(bool report); + bool camera_checks(bool display_failure); + virtual bool system_checks(bool report); bool can_checks(bool report);