diff --git a/scripts/examples/07-Interface-Library/02-MAVLink/mavlink_apriltags_landing_target.py b/scripts/examples/07-Interface-Library/02-MAVLink/mavlink_apriltags_landing_target.py index 236a0f663..94a0fa2e7 100644 --- a/scripts/examples/07-Interface-Library/02-MAVLink/mavlink_apriltags_landing_target.py +++ b/scripts/examples/07-Interface-Library/02-MAVLink/mavlink_apriltags_landing_target.py @@ -13,12 +13,11 @@ 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. +sensor_w_mm = 4.592 # For OV5650 sensor +sensor_h_mm = 3.423 # For OV5650 sensor # 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... @@ -50,8 +49,8 @@ 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 +def translation_to_mm(translation, tag_size): # translation is in decimeters... + return (((translation * 100) * tag_size) / 210) # Link Setup @@ -72,42 +71,6 @@ def checksum(data, extra): # https://github.com/mavlink/c_library_v1/blob/master 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("