ArduCopter: return MAV_RESULT_FAILED from do_aux_function if invalid function

This commit is contained in:
Peter Barker 2021-02-24 20:48:15 +11:00 committed by Peter Barker
parent e5a8acb227
commit ce9800e597
2 changed files with 7 additions and 7 deletions

View File

@ -162,7 +162,7 @@ void RC_Channel_Copter::do_aux_function_armdisarm(const AuxSwitchPos ch_flag)
}
// do_aux_function - implement the function invoked by auxiliary switches
void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
{
switch(ch_option) {
case AUX_FUNC::FLIP:
@ -214,12 +214,12 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
// do not allow saving new waypoints while we're in auto or disarmed
if (copter.flightmode->mode_number() == Mode::Number::AUTO || !copter.motors->armed()) {
return;
break;
}
// do not allow saving the first waypoint with zero throttle
if ((copter.mode_auto.mission.num_commands() == 0) && (copter.channel_throttle->get_control_in() == 0)) {
return;
break;
}
// create new mission command
@ -581,9 +581,9 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
break;
default:
RC_Channel::do_aux_function(ch_option, ch_flag);
break;
return RC_Channel::do_aux_function(ch_option, ch_flag);
}
return true;
}
// change air-mode status

View File

@ -12,7 +12,7 @@ public:
protected:
void init_aux_function(aux_func_t ch_option, AuxSwitchPos) override;
void do_aux_function(aux_func_t ch_option, AuxSwitchPos) override;
bool do_aux_function(aux_func_t ch_option, AuxSwitchPos) override;
private: