Hello,
Longtime lurker but now I ran into an issue where I am a little lost. Below is my code and I am having an issue where I cannot switch modes of Ardupilot with my serialized packets. When the camera finds blue I want it to tell ardupilot to switch modes to guided and/or loiter. I have communication with my cubepilot since when red is discovered I can see the landing packets coming in on mission planner. Any assistance is greatly appreciated.
import math
import sensor
import struct
import time
import machine
UART_BAUDRATE = 115200
MAV_system_id = 1
MAV_component_id = 0x54
packet_sequence = 0
lens_mm = 2.8
sensor_w_mm = 4.592
sensor_h_mm = 3.423
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
x_res, y_res = 320, 240
f_x = (lens_mm / sensor_w_mm) * x_res
f_y = (lens_mm / sensor_h_mm) * y_res
c_x, c_y = x_res / 2, 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)
uart = machine.UART(1, UART_BAUDRATE, timeout_char = 1000)
def checksum(data, extra):
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
color_thresholds = {
'red': [(0, 100, 20, 127, 0, 127)],
'green': [(5, 100, -33, -15, 8, 21)],
'blue': [(10, 100, -128, 127, -128, -23)]
}
docking_mode_triggered = False
guided_mode_triggered = False
blue_detected = False
def send_landing_target_packet(blob, color):
global packet_sequence
angle_x = ((blob.cx() / x_res) - 0.5) * h_fov
angle_y = ((blob.cy() / y_res) - 0.5) * v_fov
if color == 'red':
packet = struct.pack("<qfffffbb", 0, angle_x, angle_y, 10, 0.0, 0.0, 0, 8)
message_id = 149
extra_crc = 200
elif color == 'green':
packet = struct.pack("<bbhbb", 1, 216, 5, 0, 29)
message_id = 11
extra_crc = 89
elif color == 'blue':
target_system = 0
target_component = 0
command = 176 # MAV_CMD_DO_SET_MODE
confirmation = 0
param1 = 1 # Base mode
param2 = 5 # Custom mode (8 for loiter)
param3 = 0
param4 = 0
param5 = 0
param6 = 0
param7 = 0
# Pack the command parameters correctly
packet = struct.pack("<BBBHBfffffff", target_system, target_component, confirmation, command, param1, param2, param3, param4, param5, param6, param7)
message_id = 76 # Message ID for COMMAND_LONG
extra_crc = 152
# Print debug information
print("Sending MAV_CMD_DO_SET_MODE packet:", packet)
packed_data = struct.pack("<bbbbb30s", 30, packet_sequence & 0xFF, MAV_system_id, MAV_component_id, message_id, packet)
final_packet = struct.pack("<b35sh", 0xFE, packed_data, checksum(packed_data, extra_crc))
# Print the final packet for debugging
print("Final packet:", final_packet)
packet_sequence += 1
uart.write(final_packet)
led_success = machine.LED("LED_GREEN")
led_fail = machine.LED("LED_RED")
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
clock = time.clock()
while True:
clock.tick()
img = sensor.snapshot()
for color, threshold in color_thresholds.items():
blobs = img.find_blobs(threshold, area_threshold=100, pixels_threshold=100, merge=True)
if blobs:
largest_blob = max(blobs, key=lambda b: b.pixels())
send_landing_target_packet(largest_blob, color)
img.draw_rectangle(largest_blob.rect(), color=127)
print(f"{color[0].upper() + color[1:]} blob detected - FPS {clock.fps()}")
update_led(True)
break
else:
print("No blob detected - FPS %f" % clock.fps())
update_led(False)