diff --git a/APMrover2/RC_Channel.cpp b/APMrover2/RC_Channel.cpp index ca92e7a281..44d13660ca 100644 --- a/APMrover2/RC_Channel.cpp +++ b/APMrover2/RC_Channel.cpp @@ -45,6 +45,10 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const aux_s case AUX_FUNC::LOITER: case AUX_FUNC::FOLLOW: case AUX_FUNC::SAILBOAT_TACK: + case AUX_FUNC::MAINSAIL: + break; + case AUX_FUNC::SAILBOAT_MOTOR_3POS: + do_aux_function_sailboat_motor_3pos(ch_flag); break; default: RC_Channel::init_aux_function(ch_option, ch_flag); @@ -95,6 +99,21 @@ void RC_Channel_Rover::add_waypoint_for_current_loc() } } +void RC_Channel_Rover::do_aux_function_sailboat_motor_3pos(const aux_switch_pos_t ch_flag) +{ + switch(ch_flag) { + case HIGH: + rover.g2.sailboat.throttle_state = rover.g2.sailboat.Sailboat_Throttle::FORCE_MOTOR; + break; + case MIDDLE: + rover.g2.sailboat.throttle_state = rover.g2.sailboat.Sailboat_Throttle::ASSIST; + break; + case LOW: + rover.g2.sailboat.throttle_state = rover.g2.sailboat.Sailboat_Throttle::NEVER; + break; + } +} + void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) { switch (ch_option) { @@ -195,6 +214,15 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi rover.control_mode->handle_tack_request(); break; + // sailboat motor state 3pos + case AUX_FUNC::SAILBOAT_MOTOR_3POS: + do_aux_function_sailboat_motor_3pos(ch_flag); + break; + + // mainsail input, nothing to do + case AUX_FUNC::MAINSAIL: + break; + default: RC_Channel::do_aux_function(ch_option, ch_flag); break; diff --git a/APMrover2/RC_Channel.h b/APMrover2/RC_Channel.h index 96c3b3b165..9b7f4aa9e2 100644 --- a/APMrover2/RC_Channel.h +++ b/APMrover2/RC_Channel.h @@ -23,6 +23,8 @@ private: const aux_switch_pos_t ch_flag); void add_waypoint_for_current_loc(); + + void do_aux_function_sailboat_motor_3pos(const aux_switch_pos_t ch_flag); }; class RC_Channels_Rover : public RC_Channels