diff --git a/rosflight_sim/src/rc.py b/rosflight_sim/src/rc.py index edf6e11..2c69264 100755 --- a/rosflight_sim/src/rc.py +++ b/rosflight_sim/src/rc.py @@ -157,7 +157,8 @@ def __init__(self): self.declare_parameter('use_vimfly', False) self.use_vimfly = self.get_parameter('use_vimfly').get_parameter_value().bool_value - + + # Intercept if using VimFly. if self.use_vimfly and not self.transmitter_detected: self.get_logger().info('Using VimFly...') self.vim_fly = VimFly() @@ -166,7 +167,6 @@ def __init__(self): self.rc_publisher = self.create_publisher(RCRaw, 'RC', 10) self.timer = self.create_timer(1.0 / 50, self.timer_callback) - # Transmitter detected, initialize joystick if self.transmitter_detected: self.joy.init() @@ -187,7 +187,7 @@ def __init__(self): self.transmitter_detected = False # Transmitter not detected, use simulated joystick - if not self.transmitter_detected and not self.use_vimfly: + if not self.transmitter_detected: self.THROTTLE_CHANNEL = 2 self.ARM_SWITCH_CHANNEL = 4 self.OVERRIDE_CHANNEL = 5 diff --git a/rosflight_sim/src/vimfly.py b/rosflight_sim/src/vimfly.py index 3220380..31e2e65 100644 --- a/rosflight_sim/src/vimfly.py +++ b/rosflight_sim/src/vimfly.py @@ -38,12 +38,12 @@ def __init__(self): self.thrust_start_time = self.node.get_clock().now() self.armed = 1000 # Unarmed - self.ARM_DEBOUNCE_THRESHOLD = 0.250 + self.ARM_DEBOUNCE_THRESHOLD = 0.500 self.arm_debouncing = [False] self.arm_start_time = self.node.get_clock().now() self.RC_override = 2000 # Start under manual control - self.RC_OVERRIDE_DEBOUNCE_THRESHOLD = 0.250 + self.RC_OVERRIDE_DEBOUNCE_THRESHOLD = 0.500 self.RC_override_debouncing = [False] self.RC_override_start_time = self.node.get_clock().now()