diff --git a/usr/examples/18-MAVLink/mavlink_apriltags_landing_target.py b/usr/examples/18-MAVLink/mavlink_apriltags_landing_target.py new file mode 100644 index 000000000..f78475d74 --- /dev/null +++ b/usr/examples/18-MAVLink/mavlink_apriltags_landing_target.py @@ -0,0 +1,159 @@ +# MAVLink AprilTags Landing Target Script. +# +# This script sends out AprilTag detections using the MAVLink protocol to +# an ArduPilot/PixHawk controller for precision landing 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 +MAX_DISTANCE_SENSOR_enable = True + +lens_mm = 2.8 # Standard Lens. +lens_to_camera_mm = 22 # Standard Lens. +sensor_w_mm = 3.984 # For OV7725 sensor - see datasheet. +sensor_h_mm = 2.952 # For OV7725 sensor - see datasheet. + +# Only tags with a tag ID in the dictionary below will be accepted by this +# code. You may add as many tag IDs to the below dictionary as you want... + +# For each tag ID you need to provide then length of the black tag border +# in mm. Any side of the tag black border square will work. + +valid_tag_ids = { + 0 : 165, # 8.5" x 11" tag black border size in mm + 1 : 165, # 8.5" x 11" tag black border size in mm + 2 : 165, # 8.5" x 11" tag black border size in mm + } + +############################################################################## + +# Camera Setup + +sensor.reset() +sensor.set_pixformat(sensor.GRAYSCALE) +sensor.set_framesize(sensor.QQVGA) +sensor.skip_frames(60) + +x_res = 160 # QQVGA +y_res = 120 # QQVGA +f_x = (lens_mm / sensor_w_mm) * x_res +f_y = (lens_mm / sensor_h_mm) * y_res +c_x = x_res / 2 +c_y = y_res / 2 +h_fov = 2 * math.atan((sensor_w_mm / 2) / lens_mm) +v_fov = 2 * math.atan((sensor_h_mm / 2) / lens_mm) + +def z_to_mm(z_translation, tag_size): # z_translation is in decimeters... + return ((((-z_translation) * 100) * tag_size) / 165) - lens_to_camera_mm + +# 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_DISTANCE_SENSOR_message_id = 132 +MAV_DISTANCE_SENSOR_min_distance = 1 # in cm +MAV_DISTANCE_SENSOR_max_distance = 10000 # in cm +MAV_DISTANCE_SENSOR_type = 0 # MAV_DISTANCE_SENSOR_LASER +MAV_DISTANCE_SENSOR_id = 0 # unused +MAV_DISTANCE_SENSOR_orientation = 25 # MAV_SENSOR_ROTATION_PITCH_270 +MAV_DISTANCE_SENSOR_covariance = 0 # unused +MAV_DISTANCE_SENSOR_extra_crc = 85 + +# http://mavlink.org/messages/common#DISTANCE_SENSOR +# https://github.com/mavlink/c_library_v1/blob/master/common/mavlink_msg_distance_sensor.h +def send_distance_sensor_packet(tag, tag_size): + global packet_sequence + temp = struct.pack("> 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(" 40x30 +while(True): + clock.tick() + new_img = sensor.snapshot().mean_pooled(4, 4) # 160x120 -> 40x30 + x, y, c = new_img.find_displacement(old_img) + old_img = new_img + + if (not (math.isnan(x) or math.isnan(y) or math.isnan(c))) and (c > MAV_OPTICAL_FLOW_confidence_threshold): + send_optical_flow_packet(-x, -y, c) + print("dx %10f, dy %10f, confidence %10f - FPS %f" % (-x, -y, c, clock.fps())) + else: + print("FPS %f" % (clock.fps()))