From b60bec96cf33ee1fba9578236aa17e1f6894f1b2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 5 Apr 2019 09:02:00 +0900 Subject: [PATCH 1/3] mavlink_opticalflow: fix scaling and direction --- scripts/examples/18-MAVLink/mavlink_opticalflow.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/examples/18-MAVLink/mavlink_opticalflow.py b/scripts/examples/18-MAVLink/mavlink_opticalflow.py index 775957a42..eb43c9413 100644 --- a/scripts/examples/18-MAVLink/mavlink_opticalflow.py +++ b/scripts/examples/18-MAVLink/mavlink_opticalflow.py @@ -49,8 +49,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()) From bb5f910e9ed1e1c960a2b47354a6ea889418e159 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 5 Apr 2019 09:03:11 +0900 Subject: [PATCH 2/3] mavlink_opticalflow: always send flow packets regardless of quality the flight controller performs best if it has regular updates --- scripts/examples/18-MAVLink/mavlink_opticalflow.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/scripts/examples/18-MAVLink/mavlink_opticalflow.py b/scripts/examples/18-MAVLink/mavlink_opticalflow.py index eb43c9413..4fd003d5f 100644 --- a/scripts/examples/18-MAVLink/mavlink_opticalflow.py +++ b/scripts/examples/18-MAVLink/mavlink_opticalflow.py @@ -91,11 +91,8 @@ while(True): sub_pixel_x = int(-displacement.x_translation() * 35) sub_pixel_y = int(displacement.y_translation() * 53) - if(displacement.response() > 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())) From eba57709f3a7775436bd8202caa62eb492cc20dd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 5 Apr 2019 09:04:02 +0900 Subject: [PATCH 3/3] mavlink_opticalflow: add green flashing LED --- scripts/examples/18-MAVLink/mavlink_opticalflow.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/scripts/examples/18-MAVLink/mavlink_opticalflow.py b/scripts/examples/18-MAVLink/mavlink_opticalflow.py index 4fd003d5f..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) @@ -66,6 +79,7 @@ def send_optical_flow_packet(x, y, c): checksum(temp, MAV_OPTICAL_FLOW_extra_crc)) packet_sequence += 1 uart.write(temp) + update_led() sensor.reset() # Reset and initialize the sensor. sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)