From 9dfcb487cfb9c7a466a4035f432b5e0139c83b74 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Mon, 28 Dec 2020 16:07:46 -0600 Subject: [PATCH] AP_Mount: add RC failsafe action --- libraries/AP_Mount/AP_Mount_Backend.h | 1 + libraries/AP_Mount/AP_Mount_Params.cpp | 2 +- libraries/AP_Mount/AP_Mount_Servo.cpp | 8 ++++++++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 16082d58f4..a25bd4870a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -249,6 +249,7 @@ protected: // options parameter bitmask handling enum class Options : uint8_t { RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode + NEUTRAL_ON_RC_FS = (1U << 1), // move mount to netral position on RC failsafe }; bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; } diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index e2c727f4b4..d077c603ec 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -168,7 +168,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @Param: _OPTIONS // @DisplayName: Mount options // @Description: Mount options bitmask - // @Bitmask: 0:RC lock state from previous mode + // @Bitmask: 0:RC lock state from previous mode, 1:Return to neutral angles on RC failsafe // @User: Standard AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0), diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 1fba00e92a..7c97656c12 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -56,6 +56,13 @@ void AP_Mount_Servo::update() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { + // update targets using pilot's rc inputs or go to neutral or retracted targets if no rc + if (rc().in_rc_failsafe()) { + if (option_set(Options::NEUTRAL_ON_RC_FS)) { + mnt_target.angle_rad.set(_angle_bf_output_rad, false); + mnt_target.target_type = MountTargetType::ANGLE; + } + } else { // update targets using pilot's RC inputs MountTarget rc_target; get_rc_target(mnt_target.target_type, rc_target); @@ -67,6 +74,7 @@ void AP_Mount_Servo::update() mnt_target.rate_rads = rc_target; break; } + } break; }