From 8a478abce99cb3409d45b812ff36e7263e8257a6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Mar 2024 09:55:48 +1100 Subject: [PATCH] RC_Channel: allow customisation of position text in aux switch announcement this means that we get "EKFPosSource 1" rather than "EKFPosSource LOW" --- libraries/RC_Channel/RC_Channel.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index b0b4787909134d..21928b0fcfeac2 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -753,7 +753,6 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { { AUX_FUNC::RUNCAM_OSD_CONTROL,"RunCamOSDControl"}, { AUX_FUNC::VISODOM_ALIGN,"VisOdomAlign"}, { AUX_FUNC::AIRMODE, "AirMode"}, - { AUX_FUNC::EKF_POS_SOURCE,"EKFPosSource"}, { AUX_FUNC::CAM_MODE_TOGGLE,"CamModeToggle"}, { AUX_FUNC::GENERATOR,"Generator"}, { AUX_FUNC::BATTERY_MPPT_ENABLE,"Battery MPPT Enable"}, @@ -1434,22 +1433,26 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch break; #endif - case AUX_FUNC::EKF_POS_SOURCE: + case AUX_FUNC::EKF_POS_SOURCE: { + uint8_t source_set = 0; switch (ch_flag) { case AuxSwitchPos::LOW: // low switches to primary source - AP::ahrs().set_posvelyaw_source_set(0); + source_set = 0; break; case AuxSwitchPos::MIDDLE: // middle switches to secondary source - AP::ahrs().set_posvelyaw_source_set(1); + source_set = 1; break; case AuxSwitchPos::HIGH: // high switches to tertiary source - AP::ahrs().set_posvelyaw_source_set(2); + source_set = 2; break; } + AP::ahrs().set_posvelyaw_source_set(source_set); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Using EKF Source Set %u", source_set+1); break; + } #if AP_OPTICALFLOW_CALIBRATOR_ENABLED case AUX_FUNC::OPTFLOW_CAL: {