# MAVLink OpticalFlow Script. # # This script sends out OpticalFlow detections using the MAVLink protocol to # an ArduPilot/PixHawk controller for position control using your OpenMV Cam. # # P4 = TXD import image, math, pyb, sensor, struct, time # Parameters ################################################################# uart_baudrate = 115200 MAV_system_id = 1 MAV_component_id = 0x54 MAV_OPTICAL_FLOW_confidence_threshold = 0.1 # Below 0.1 or so (YMMV) and the results are just noise. ############################################################################## # 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) # Helper Stuff packet_sequence = 0 def checksum(data, extra): # https://github.com/mavlink/c_library_v1/blob/master/checksum.h output = 0xFFFF for i in range(len(data)): tmp = data[i] ^ (output & 0xFF) tmp = (tmp ^ (tmp << 4)) & 0xFF output = ((output >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF tmp = extra ^ (output & 0xFF) tmp = (tmp ^ (tmp << 4)) & 0xFF output = ((output >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF return output MAV_OPTICAL_FLOW_message_id = 100 MAV_OPTICAL_FLOW_id = 0 # unused MAV_OPTICAL_FLOW_extra_crc = 175 # http://mavlink.org/messages/common#OPTICAL_FLOW # https://github.com/mavlink/c_library_v1/blob/master/common/mavlink_msg_optical_flow.h def send_optical_flow_packet(x, y, c): global packet_sequence temp = struct.pack("