mirror of https://github.com/ArduPilot/ardupilot
RC_Channel: added AUX function 112 for AHRS EKF type changes
This commit is contained in:
parent
80730d6c02
commit
376426a088
|
@ -207,6 +207,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
|
|||
// @Values{Plane}: 108:QRTL Mode
|
||||
// @Values{Copter}: 109:use Custom Controller
|
||||
// @Values{Copter, Rover, Plane, Blimp}: 110:KillIMU3
|
||||
// @Values{Copter,Plane,Rover,Blimp,Sub,Tracker}: 112:SwitchExternalAHRS
|
||||
// @Values{Plane}: 150:CRUISE Mode
|
||||
// @Values{Copter}: 151:TURTLE Mode
|
||||
// @Values{Copter}: 152:SIMPLE heading reset
|
||||
|
@ -700,6 +701,7 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo
|
|||
case AUX_FUNC::CAMERA_MANUAL_FOCUS:
|
||||
case AUX_FUNC::CAMERA_AUTO_FOCUS:
|
||||
case AUX_FUNC::CAMERA_LENS:
|
||||
case AUX_FUNC::AHRS_TYPE:
|
||||
run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
|
||||
break;
|
||||
default:
|
||||
|
@ -1613,6 +1615,14 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos
|
|||
AP::ahrs().request_yaw_reset();
|
||||
break;
|
||||
|
||||
case AUX_FUNC::AHRS_TYPE: {
|
||||
#if HAL_NAVEKF3_AVAILABLE && HAL_EXTERNAL_AHRS_ENABLED
|
||||
AP::ahrs().set_ekf_type(ch_flag==AuxSwitchPos::HIGH? 11 : 3);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
#if HAL_TORQEEDO_ENABLED
|
||||
// clear torqeedo error
|
||||
case AUX_FUNC::TORQEEDO_CLEAR_ERR: {
|
||||
|
|
|
@ -216,6 +216,7 @@ public:
|
|||
CUSTOM_CONTROLLER = 109, // use Custom Controller
|
||||
KILL_IMU3 = 110, // disable third IMU (for IMU failure testing)
|
||||
LOWEHEISER_STARTER = 111, // allows for manually running starter
|
||||
AHRS_TYPE = 112, // change AHRS_EKF_TYPE
|
||||
|
||||
// if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp!
|
||||
// also, if you add an option >255, you will need to fix duplicate_options_exist
|
||||
|
|
Loading…
Reference in New Issue