Python hız komutu Pixhawk'a gidiyor, drone yavaşlamıyor

Hız komutu kabul ediliyor, drone yine hızlı gidiyor

Python kodun drone'a 3 m/s ile gitmesini söylüyor. Pixhawk komutu kabul ediyor, ekranda ACCEPTED yazıyor. Drone ise hedefe yine 10 m/s civarında gidiyor.

Hız değerini 2'ye, 1'e indirmek bir şey değiştirmiyor. Komutu iki kez yollamak da işe yaramıyor.

Çoğu zaman sorun iki satırın sırasındadır. Hızı hedeften önce gönderirsen otopilot onu birkaç milisaniye sonra kendisi siler.

Otopilot verdiğin hızı neden siliyor

GUIDED moddaki drone yeni bir konum hedefi aldığında, bir önceki komut konum hedefi değilse otopilot konum kontrolünü baştan kurar. Bu kurulum yatay hız sınırını WP_SPD parametresine çeker. ArduCopter 4.7'de WP_SPD'nin varsayılanı 10 m/s'dir.

Şekildeki üst satıra bak. Hız komutu ACCEPTED alıyor, ardından gelen hedef sarı çizgide hızı WP_SPD'ye döndürüyor. Alt satırda hız sıfırlamadan sonra geliyor ve yerinde kalıyor.

Aynı iki komut, iki sıra, iki hız

Sıfırlama üç anda olur: kalkıştan sonraki ilk hedefte, araya bir hız vektörü (vx, vy) komutu girdikten sonraki ilk hedefte ve drone GUIDED'e yeniden alındığında. Hedefin arkasından yeni hedef gönderdiğin sürece verdiğin hız korunur.

Yük bırakma görevinde nasıl karşına çıkar

Kamera hedefi gördü, sen de drone'u hız vektörüyle hedefin üstüne ortalıyorsun. Ortalama bitince bırakma noktasını konum hedefi olarak verip drone'un 2 m/s ile yaklaşmasını istiyorsun.

Hız komutunu ortalama döngüsünün içinde yolladıysan konum hedefi geldiği an hız WP_SPD'ye döner. Drone bırakma noktasına 10 m/s'ye kadar hızlanarak gelir ve yük kayar. Hız komutunu konum hedefinin hemen arkasına koyduğunda yaklaşma 2 m/s olur.

Kodda doğru sıra

Önce hedefi, sonra hızı gönder. Hız komutunda param1 = 1 yer hızı demektir, param2 metre bölü saniye cinsinden hızdır, param3 = -1 gaz ayarına dokunma demektir.

import math
import time
from pymavlink import mavutil

M = mavutil.mavlink
b = mavutil.mavlink_connection("tcp:127.0.0.1:5760")   # Pi'de: "/dev/serial0", baud=921600
b.wait_heartbeat()
b.mav.request_data_stream_send(b.target_system, b.target_component,
                               M.MAV_DATA_STREAM_ALL, 4, 1)

k = b.recv_match(type="GLOBAL_POSITION_INT", blocking=True)
hedef_lat = k.lat / 1e7 + 150 / 111320.0          # 150 m kuzey
hedef_lon = k.lon / 1e7
lat_i, lon_i = int(hedef_lat * 1e7), int(hedef_lon * 1e7)
MASKE = 0b0000111111111000                         # yalnız konum kullanılır

# 1) önce hedef
b.mav.set_position_target_global_int_send(
    0, b.target_system, b.target_component,
    M.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MASKE,
    lat_i, lon_i, 20, 0, 0, 0, 0, 0, 0, 0, 0)
time.sleep(0.2)

# 2) sonra hız: param1 = 1 (yer hızı), param2 = m/s
b.mav.command_long_send(b.target_system, b.target_component,
    M.MAV_CMD_DO_CHANGE_SPEED, 0, 1, 3.0, -1, 0, 0, 0, 0)
ack = b.recv_match(type="COMMAND_ACK", blocking=True, timeout=3)
print("hız komutu:", M.enums["MAV_RESULT"][ack.result].name if ack else "ACK yok")

# 3) drone'un saatine göre her 3 saniyede bir yer hızı ve kalan mesafe
son_t = 0
while True:
    k = b.recv_match(type="GLOBAL_POSITION_INT", blocking=True)
    kalan = math.hypot((k.lat / 1e7 - hedef_lat) * 111320.0,
                       (k.lon / 1e7 - hedef_lon) * 111320.0 * math.cos(math.radians(hedef_lat)))
    if k.time_boot_ms - son_t >= 3000:
        son_t = k.time_boot_ms
        print(f"yer hızı {math.hypot(k.vx, k.vy) / 100:4.1f} m/s | kalan {kalan:5.1f} m")
    if kalan < 2:
        print("hedefte")
        break

1. Gir: kodu hiz_sonra.py adıyla kaydet; drone havada ve GUIDED'deyken python3 hiz_sonra.py çalıştır. Raspberry Pi'de bağlantı adresini /dev/serial0 yap, baud'u Pixhawk'ın telemetri portundaki ayarla aynı ver.
2. Gör: ilk satırda hız komutu: MAV_RESULT_ACCEPTED, birkaç saniye sonra yer hızı 3,0 m/s.
3. Değilse: ACK satırında DENIED görüyorsan param2 sıfır ya da eksidir. FAILED görüyorsan drone hız komutunu kabul etmeyen bir moddadır, önce GUIDED'e al.

DroneKit kullanıyorsan vehicle.groundspeed = 3 satırını ilk simple_goto'dan önce yazmak aynı tuzaktır. simple_goto(konum, groundspeed=3) ise hedefi önce, hızı sonra yollar; sırayı kütüphane kendisi doğru kurar.

Ne göreceksin

Kodu kalkıştan hemen sonra ya da GUIDED'e yeni geçmiş bir drone'la çalıştırırsan şu satırları görürsün:

hız komutu: MAV_RESULT_ACCEPTED
yer hızı  1.6 m/s | kalan 149.5 m
yer hızı  3.0 m/s | kalan 141.1 m
yer hızı  3.0 m/s | kalan 132.0 m
yer hızı  3.0 m/s | kalan 123.0 m
yer hızı  3.0 m/s | kalan 114.0 m
...
yer hızı  3.0 m/s | kalan   6.0 m
hedefte

Aynı kodda iki bloğun yerini değiştirip hızı önce yollarsan ACK yine ACCEPTED gelir, drone ise WP_SPD ile gider:

hız komutu: MAV_RESULT_ACCEPTED
yer hızı  0.0 m/s | kalan 150.0 m
yer hızı  6.9 m/s | kalan 140.2 m
yer hızı  9.9 m/s | kalan 111.0 m
yer hızı 10.1 m/s | kalan  80.8 m
yer hızı 10.0 m/s | kalan  48.1 m
yer hızı  9.4 m/s | kalan  18.2 m
hedefte

İkinci çıktıda üçüncü satırdan sonrasına bak: drone 10 m/s'ye çıkıyor ve 150 metreyi ilk çıktıdakinin yaklaşık üçte biri sürede bitiriyor. WP_SPD'yi değiştirdiysen tavan o değer olur.

Hata Mesajları

hız komutu: MAV_RESULT_DENIED

param2'ye sıfır ya da eksi bir sayı gitmiştir. Hızı yanlışlıkla param1'e yazıp param2'yi 0 bıraktığında da bu satırı görürsün.

hız komutu: MAV_RESULT_FAILED

Drone hız komutunu desteklemeyen bir moddadır (LOITER, BRAKE gibi). Önce GUIDED'e al, sonra hedefi ve hızı gönder.

hız komutu: ACK yok

Cevap 3 saniye içinde gelmedi; telemetri hattında paket düşmüş olabilir. Komutu hedeften sonra bir kez daha gönder.

Sıradaki adım

GUIDED komutlarını hangi sırayla ve hangi kontrollerle göndereceğini Python ile Sıfırdan İHA Kontrolü eğitiminin Kalkış, hareket ve GUIDED komutları bölümünde öğrenirsin. Hızın hangi durumda neden silindiğini kaynak kod satırlarıyla izleyen ders de bu bölümün sonunda.