mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
scripts/examples: Fix MAVLINK apriltag example. (#1716)
* Remove DISTANCE_SENSOR message. * Update sensor specs. * Fix translation conversion function.
This commit is contained in:
parent
7fd5c77db0
commit
279d2071b7
@ -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("<lhhhbbbb",
|
||||
0,
|
||||
MAV_DISTANCE_SENSOR_min_distance,
|
||||
MAV_DISTANCE_SENSOR_max_distance,
|
||||
min(max(int(z_to_mm(tag.z_translation(), tag_size) / 10), MAV_DISTANCE_SENSOR_min_distance), MAV_DISTANCE_SENSOR_max_distance),
|
||||
MAV_DISTANCE_SENSOR_type,
|
||||
MAV_DISTANCE_SENSOR_id,
|
||||
MAV_DISTANCE_SENSOR_orientation,
|
||||
MAV_DISTANCE_SENSOR_covariance)
|
||||
temp = struct.pack("<bbbbb14s",
|
||||
14,
|
||||
packet_sequence & 0xFF,
|
||||
MAV_system_id,
|
||||
MAV_component_id,
|
||||
MAV_DISTANCE_SENSOR_message_id,
|
||||
temp)
|
||||
temp = struct.pack("<b19sh",
|
||||
0xFE,
|
||||
temp,
|
||||
checksum(temp, MAV_DISTANCE_SENSOR_extra_crc))
|
||||
packet_sequence += 1
|
||||
uart.write(temp)
|
||||
|
||||
MAV_LANDING_TARGET_message_id = 149
|
||||
MAV_LANDING_TARGET_min_distance = 1/100 # in meters
|
||||
MAV_LANDING_TARGET_max_distance = 10000/100 # in meters
|
||||
@ -116,13 +79,13 @@ MAV_LANDING_TARGET_extra_crc = 200
|
||||
|
||||
# http://mavlink.org/messages/common#LANDING_TARGET
|
||||
# https://github.com/mavlink/c_library_v1/blob/master/common/mavlink_msg_landing_target.h
|
||||
def send_landing_target_packet(tag, w, h, tag_size):
|
||||
def send_landing_target_packet(tag, dist_cm, w, h):
|
||||
global packet_sequence
|
||||
temp = struct.pack("<qfffffbb",
|
||||
0,
|
||||
((tag.cx() / w) - 0.5) * h_fov,
|
||||
((tag.cy() / h) - 0.5) * v_fov,
|
||||
min(max(z_to_mm(tag.z_translation(), tag_size) / 1000, MAV_LANDING_TARGET_min_distance), MAV_LANDING_TARGET_max_distance),
|
||||
min(max(dist_mm * 0.001, MAV_LANDING_TARGET_min_distance), MAV_LANDING_TARGET_max_distance),
|
||||
0.0,
|
||||
0.0,
|
||||
0,
|
||||
@ -141,6 +104,25 @@ def send_landing_target_packet(tag, w, h, tag_size):
|
||||
packet_sequence += 1
|
||||
uart.write(temp)
|
||||
|
||||
# LED control
|
||||
led_success = pyb.LED(2) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
|
||||
led_fail = pyb.LED(1)
|
||||
led_counter = 0
|
||||
|
||||
def update_led(target_found):
|
||||
global led_counter
|
||||
|
||||
if (target_found) :
|
||||
led = led_success
|
||||
led_fail.off()
|
||||
else :
|
||||
led = led_fail
|
||||
led_success.off()
|
||||
|
||||
if led_counter % 4 == 0:
|
||||
led.toggle()
|
||||
|
||||
led_counter += 1
|
||||
# Main Loop
|
||||
|
||||
clock = time.clock()
|
||||
@ -148,12 +130,16 @@ while(True):
|
||||
clock.tick()
|
||||
img = sensor.snapshot()
|
||||
tags = sorted(img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y), key = lambda x: x.w() * x.h(), reverse = True)
|
||||
|
||||
target_found = False
|
||||
if tags and (tags[0].id() in valid_tag_ids):
|
||||
if MAX_DISTANCE_SENSOR_enable: send_distance_sensor_packet(tags[0], valid_tag_ids[tags[0].id()])
|
||||
send_landing_target_packet(tags[0], img.width(), img.height(), valid_tag_ids[tags[0].id()])
|
||||
target_found = True
|
||||
tag_size = valid_tag_ids[tags[0].id()]
|
||||
dist_mm = math.sqrt(translation_to_mm(tags[0].x_translation(), tag_size) ** 2 + translation_to_mm(tags[0].y_translation(), tag_size) ** 2 + translation_to_mm(tags[0].z_translation(), tag_size) ** 2)
|
||||
send_landing_target_packet(tags[0], dist_mm, img.width(), img.height())
|
||||
img.draw_rectangle(tags[0].rect())
|
||||
img.draw_cross(tags[0].cx(), tags[0].cy())
|
||||
print("Distance %f mm - FPS %f" % (z_to_mm(tags[0].z_translation(), valid_tag_ids[tags[0].id()]), clock.fps()))
|
||||
print("Distance %f mm - FPS %f" % (dist_mm, clock.fps()))
|
||||
else:
|
||||
print("FPS %f" % clock.fps())
|
||||
|
||||
update_led(target_found)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user