mirror of https://github.com/ArduPilot/ardupilot
RC_Channel: add mount lock aux function
This commit is contained in:
parent
f83b4cdb40
commit
640a4b1a5f
|
@ -522,6 +522,7 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo
|
|||
case AUX_FUNC::FFT_NOTCH_TUNE:
|
||||
#if HAL_MOUNT_ENABLED
|
||||
case AUX_FUNC::RETRACT_MOUNT:
|
||||
case AUX_FUNC::MOUNT_LOCK:
|
||||
#endif
|
||||
run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
|
||||
break;
|
||||
|
@ -582,6 +583,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = {
|
|||
{ AUX_FUNC::WEATHER_VANE_ENABLE, "Weathervane"},
|
||||
{ AUX_FUNC::TURBINE_START, "Turbine Start"},
|
||||
{ AUX_FUNC::FFT_NOTCH_TUNE, "FFT Notch Tuning"},
|
||||
{ AUX_FUNC::MOUNT_LOCK, "MountLock"},
|
||||
};
|
||||
|
||||
/* lookup the announcement for switch change */
|
||||
|
@ -1216,6 +1218,17 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos
|
|||
break;
|
||||
}
|
||||
|
||||
case AUX_FUNC::MOUNT_LOCK: {
|
||||
#if HAL_MOUNT_ENABLED
|
||||
AP_Mount *mount = AP::mount();
|
||||
if (mount == nullptr) {
|
||||
break;
|
||||
}
|
||||
mount->set_yaw_lock(ch_flag == AuxSwitchPos::HIGH);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
case AUX_FUNC::EKF_LANE_SWITCH:
|
||||
// used to test emergency lane switch
|
||||
AP::ahrs().check_lane_switch();
|
||||
|
|
|
@ -226,6 +226,7 @@ public:
|
|||
WEATHER_VANE_ENABLE = 160, // enable/disable weathervaning
|
||||
TURBINE_START = 161, // initialize turbine start sequence
|
||||
FFT_NOTCH_TUNE = 162, // FFT notch tuning function
|
||||
MOUNT_LOCK = 163, // Mount yaw lock vs follow
|
||||
|
||||
// inputs from 200 will eventually used to replace RCMAP
|
||||
ROLL = 201, // roll input
|
||||
|
|
Loading…
Reference in New Issue