From 1b1687f43d6336e6eec63e2ba872441c04ef9749 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 30 Mar 2020 12:30:20 +0900 Subject: [PATCH] AP_Arming: integrate visual odometry pre_arm_check --- libraries/AP_Arming/AP_Arming.cpp | 25 ++++++++++++++++++++++++- libraries/AP_Arming/AP_Arming.h | 3 +++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index d3ee9b729e..43c1cffa69 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #if HAL_WITH_UAVCAN #include @@ -115,7 +116,7 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = { // @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,65536:Camera,131072:AuxAuth // @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,131072:AuxAuth - // @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,17:AuxAuth + // @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,17:AuxAuth,18:VisualOdometry // @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,17:AuxAuth // @User: Standard AP_GROUPINFO("CHECK", 8, AP_Arming, checks_to_perform, ARMING_CHECK_ALL), @@ -1053,6 +1054,7 @@ bool AP_Arming::pre_arm_checks(bool report) & can_checks(report) & proximity_checks(report) & camera_checks(report) + & visodom_checks(report) & aux_auth_checks(report); } @@ -1193,6 +1195,27 @@ bool AP_Arming::rc_checks_copter_sub(const bool display_failure, const RC_Channe return ret; } +// check visual odometry is working +bool AP_Arming::visodom_checks(bool display_failure) const +{ + if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_VISION)) { + return true; + } + +#if HAL_VISUALODOM_ENABLED + AP_VisualOdom *visual_odom = AP::visualodom(); + if (visual_odom != nullptr) { + char fail_msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + if (!visual_odom->pre_arm_check(fail_msg, ARRAY_SIZE(fail_msg))) { + check_failed(ARMING_CHECK_VISION, display_failure, "VisualOdom: %s", fail_msg); + return false; + } + } +#endif + + return true; +} + void AP_Arming::Log_Write_Arm(const bool forced, const AP_Arming::Method method) { const struct log_Arm_Disarm pkt { diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index d1477bdd54..83af3f1d31 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -35,6 +35,7 @@ public: ARMING_CHECK_RANGEFINDER = (1U << 15), ARMING_CHECK_CAMERA = (1U << 16), ARMING_CHECK_AUX_AUTH = (1U << 17), + ARMING_CHECK_VISION = (1U << 18), }; enum class Method { @@ -173,6 +174,8 @@ protected: bool servo_checks(bool report) const; bool rc_checks_copter_sub(bool display_failure, const RC_Channel *channels[4]) const; + bool visodom_checks(bool report) const; + // mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced virtual bool mandatory_checks(bool report) { return true; }