MAVSDK-Odom

odom-pixi.py

import time
import mavsdk
import asyncio

from mavsdk import mocap
from mavsdk import System
from mavsdk.mocap import (Odometry)

initial = time.time()

async def run():

    drone = System()
    await drone.connect(system_address="udp://:14540")

    print("Waiting for drone to connect...")
    async for state in drone.core.connection_state():
        if state.is_connected:
            print(f"Drone discovered!")
            break
    now = time.time()
    this = now - initial
    position_body =  mocap.PositionBody(0,0,0)
    q = mocap.Quaternion(1,0,0,0)
    speed = mocap.SpeedBody(0,0,0)
    angular_velocity = mocap.AngularVelocityBody(0,0,0)
    pose_covariance = mocap.Covariance([0.01] * 21)
    print("Sending odom...")
    #await drone.mocap.set_odometry(Odometry(shit,1,(0,0,0),(1,0,0,0),(0,0,0),(0,0,0),0.1,0.1))
    await drone.mocap.set_odometry(Odometry(int(this),mocap.Odometry.MavFrame(1),position_body,q,speed,angular_velocity,pose_covariance,pose_covariance))
    #await drone.mocap.set_odometry(Odometry(timestamp,0,))
    

if __name__ == "__main__":
    loop = asyncio.get_event_loop()
    loop.run_until_complete(run())