diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index b525bcc185..a44f732976 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -90,6 +90,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const AuxS case AUX_FUNC::ACRO: case AUX_FUNC::AUTO_RTL: case AUX_FUNC::TURTLE: + case AUX_FUNC::SIMPLE_HEADING_RESET: break; case AUX_FUNC::ACRO_TRAINER: case AUX_FUNC::ATTCON_ACCEL_LIM: @@ -575,6 +576,13 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi #endif break; + case AUX_FUNC::SIMPLE_HEADING_RESET: + if (ch_flag == AuxSwitchPos::HIGH) { + copter.init_simple_bearing(); + gcs().send_text(MAV_SEVERITY_INFO, "Simple heading reset"); + } + break; + default: return RC_Channel::do_aux_function(ch_option, ch_flag); }