From 8813b07500c72d65ddbe4db57292498601431be6 Mon Sep 17 00:00:00 2001 From: Colin Wong Date: Tue, 14 May 2024 19:13:13 -0500 Subject: [PATCH] Add servo 2 and 3 min/max toggle function implementation --- ArduSub/joystick.cpp | 40 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index ad2710c6c1ae3..4d25b433a24cc 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -525,6 +525,16 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed } break; + case JSButton::button_function_t::k_servo_2_min_toggle: + if(!held) { + SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed + if(chan->get_output_pwm() != chan->get_output_min()) { + ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed + } else { + ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed + } + } + break; case JSButton::button_function_t::k_servo_2_max: case JSButton::button_function_t::k_servo_2_max_momentary: { @@ -532,6 +542,16 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed } break; + case JSButton::button_function_t::k_servo_2_max_toggle: + if(!held) { + SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed + if(chan->get_output_pwm() != chan->get_output_max()) { + ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed + } else { + ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed + } + } + break; case JSButton::button_function_t::k_servo_2_center: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed @@ -562,6 +582,16 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed } break; + case JSButton::button_function_t::k_servo_3_min_toggle: + if(!held) { + SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed + if(chan->get_output_pwm() != chan->get_output_min()) { + ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed + } else { + ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed + } + } + break; case JSButton::button_function_t::k_servo_3_max: case JSButton::button_function_t::k_servo_3_max_momentary: { @@ -569,6 +599,16 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed } break; + case JSButton::button_function_t::k_servo_3_max_toggle: + if(!held) { + SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed + if(chan->get_output_pwm() != chan->get_output_max()) { + ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed + } else { + ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed + } + } + break; case JSButton::button_function_t::k_servo_3_center: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed