diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 5dd56c8f54..db6b9b31fb 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -236,6 +236,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Copter, Rover, Plane, Blimp}: 174:Camera Image Tracking // @Values{Copter, Rover, Plane, Blimp}: 175:Camera Lens // @Values{Plane}: 176:Quadplane Fwd Throttle Override enable + // @Values{Copter, Rover, Plane, Blimp}: 177:Mount LRF enable // @Values{Rover}: 201:Roll // @Values{Rover}: 202:Pitch // @Values{Rover}: 207:MainSail @@ -665,6 +666,7 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos case AUX_FUNC::LOWEHEISER_STARTER: case AUX_FUNC::MAG_CAL: case AUX_FUNC::CAMERA_IMAGE_TRACKING: + case AUX_FUNC::MOUNT_LRF_ENABLE: break; // not really aux functions: @@ -770,6 +772,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { { AUX_FUNC::CAMERA_AUTO_FOCUS, "Camera Auto Focus"}, { AUX_FUNC::CAMERA_IMAGE_TRACKING, "Camera Image Tracking"}, { AUX_FUNC::CAMERA_LENS, "Camera Lens"}, + { AUX_FUNC::MOUNT_LRF_ENABLE, "Mount LRF Enable"}, }; /* lookup the announcement for switch change */ @@ -1555,6 +1558,15 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch mount->set_yaw_lock(ch_flag == AuxSwitchPos::HIGH); break; } + + case AUX_FUNC::MOUNT_LRF_ENABLE: { + AP_Mount *mount = AP::mount(); + if (mount == nullptr) { + break; + } + mount->set_rangefinder_enable(0, ch_flag == AuxSwitchPos::HIGH); + break; + } #endif #if HAL_LOGGING_ENABLED diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 4ad9e532fa..1613b107e1 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -249,6 +249,7 @@ public: CAMERA_IMAGE_TRACKING = 174, // camera image tracking CAMERA_LENS = 175, // camera lens selection VFWD_THR_OVERRIDE = 176, // force enabled VTOL forward throttle method + MOUNT_LRF_ENABLE = 177, // mount LRF enable/disable // inputs from 200 will eventually used to replace RCMAP