mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
ArduSub: implement servo 2 and 3 min/max-toggle button functions
This commit is contained in:
parent
0accc61107
commit
e27dea7003
@ -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
|
ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed
|
||||||
}
|
}
|
||||||
break;
|
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:
|
||||||
case JSButton::button_function_t::k_servo_2_max_momentary:
|
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
|
ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed
|
||||||
}
|
}
|
||||||
break;
|
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:
|
case JSButton::button_function_t::k_servo_2_center:
|
||||||
{
|
{
|
||||||
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
|
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
|
ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed
|
||||||
}
|
}
|
||||||
break;
|
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:
|
||||||
case JSButton::button_function_t::k_servo_3_max_momentary:
|
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
|
ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed
|
||||||
}
|
}
|
||||||
break;
|
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:
|
case JSButton::button_function_t::k_servo_3_center:
|
||||||
{
|
{
|
||||||
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
|
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
|
||||||
|
Loading…
Reference in New Issue
Block a user