diff --git a/scripts/examples/18-MAVLink/mavlink_opticalflow.py b/scripts/examples/18-MAVLink/mavlink_opticalflow.py index 775957a42..c16c2cd8a 100644 --- a/scripts/examples/18-MAVLink/mavlink_opticalflow.py +++ b/scripts/examples/18-MAVLink/mavlink_opticalflow.py @@ -17,6 +17,19 @@ MAV_OPTICAL_FLOW_confidence_threshold = 0.1 # Below 0.1 or so (YMMV) and the re ############################################################################## +# LED control +led = pyb.LED(2) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4. +led_state = 0 + +def update_led(): + global led_state + led_state = led_state + 1 + if led_state == 10: + led.on() + elif led_state >= 20: + led.off() + led_state = 0 + # Link Setup uart = pyb.UART(3, uart_baudrate, timeout_char = 1000) @@ -49,8 +62,8 @@ def send_optical_flow_packet(x, y, c): 0, 0, 0, - int(x * 10), - int(y * 10), + int(x), + int(y), MAV_OPTICAL_FLOW_id, int(c * 255)) temp = struct.pack(" MAV_OPTICAL_FLOW_confidence_threshold): - send_optical_flow_packet(sub_pixel_x, sub_pixel_y, displacement.response()) + send_optical_flow_packet(sub_pixel_x, sub_pixel_y, displacement.response()) - print("{0:+f}x {1:+f}y {2} {3} FPS".format(sub_pixel_x, sub_pixel_y, - displacement.response(), - clock.fps())) - else: - print(clock.fps()) + print("{0:+f}x {1:+f}y {2} {3} FPS".format(sub_pixel_x, sub_pixel_y, + displacement.response(), + clock.fps()))