How do you use `telemetry_server` correctly?

[byt3bl33d3r]

I have the following code that reads a CSV over the network with telemetry I want to feed to PX4:

import asyncio
import io
import csv
import numpy as np
from functools import partial
from typing import Dict, Any
from decimal import Decimal
from mavsdk import System
from mavsdk.telemetry_server import (
    Position, VelocityNed, Heading, Imu,
    AccelerationFrd, AngularVelocityFrd, MagneticFieldFrd
)

G_CONSTANT = 9.80665
ACCEL_DATA_G = []
TIME_INTERVAL = 1 / 30 # We're sampling at 30Hz

async def send_telemetry(drone: System, data: Dict[Any,Any]):
    global ACCEL_DATA_G

    ACCEL_DATA_G.append(
        [
            float(data['accelerometerAccelerationX(G)']),
            float(data['accelerometerAccelerationY(G)']),
            float(data['accelerometerAccelerationZ(G)'])
        ]
    )

    accel_data_g = np.array(ACCEL_DATA_G)
    accel_data_m_s2 = accel_data_g * G_CONSTANT
    velocity = np.zeros_like(accel_data_m_s2)

    for i in range(1, len(accel_data_m_s2)):
        velocity[i] = velocity[i-1] + (accel_data_m_s2[i-1] + accel_data_m_s2[i]) / 2 * TIME_INTERVAL

    average_velocity = np.mean(velocity, axis=0)
    #print(f"Velocity in m/s (NED): {average_velocity}")

    await drone.telemetry_server.publish_position(
        position=Position(
            latitude_deg=float(data['locationLatitude(WGS84)']),
            longitude_deg=float(data['locationLongitude(WGS84)']),
            absolute_altitude_m=float(data['locationAltitude(m)']),
            relative_altitude_m=float(data['altimeterRelativeAltitude(m)'])
        ),
        velocity_ned=VelocityNed(
            north_m_s=average_velocity[0],
            east_m_s=average_velocity[1],
            down_m_s=average_velocity[2]
        ),
        heading=Heading(
            heading_deg=float(data['locationTrueHeading(°)'])
        )
    )

    """
    #drone.telemetry_server.publish_imu()
    await drone.telemetry_server.publish_raw_imu(
        Imu(
            acceleration_frd=AccelerationFrd(
                forward_m_s2=float(data['accelerometerAccelerationX(G)']),
                right_m_s2=float(data['accelerometerAccelerationY(G)']),
                down_m_s2=float(data['accelerometerAccelerationZ(G)'])
            ),
            angular_velocity_frd=AngularVelocityFrd(
                forward_rad_s=float(data['gyroRotationX(rad/s)']),
                right_rad_s=float(data['gyroRotationY(rad/s)']),
                down_rad_s=float(data['gyroRotationZ(rad/s)'])
            ),
            magnetic_field_frd=MagneticFieldFrd(
                forward_gauss=float(Decimal(data['motionMagneticFieldX(µT)']) / Decimal(100)), # 1 Gauss is equal to 100 µT
                right_gauss=float(Decimal(data['motionMagneticFieldY(µT)']) / Decimal(100)),
                down_gauss=float(Decimal(data['motionMagneticFieldZ(µT)']) / Decimal(100))
            ),
            temperature_degc=0,
            timestamp_us=int(float(data['locationTimestamp_since1970(s)'])) * 1_000_000
        )
    )
    """

async def handle_echo(reader, writer, drone: System):
    addr = writer.get_extra_info('peername')
    print(f"Received connection from {addr}")

    fields = None

    with open('telem.csv', 'w') as f:
        while True:
            data = await reader.readline()
            message = data.decode()
            #addr = writer.get_extra_info('peername')
            #print(f"Received connection from {addr}")
            #print(message)
            f.write(message)

            if 'loggingTime' in message:
                fields = message.strip().split(',')
                print(message)
                continue

            buf = io.StringIO(message)
            csv_reader = csv.DictReader(buf, fieldnames=fields)

            for row in csv_reader:
                #asyncio.create_task(send_telemetry(drone, row))
                await send_telemetry(drone, row)

async def read_telem(drone: System):
    async for position in drone.telemetry.position():
        print(position)
        await asyncio.sleep(5)

async def main():
    drone = System()
    await drone.connect(system_address="udp://:14540") # PX4
    #await drone.connect(system_address="udp://:14550") # Ardupilot

    #asyncio.create_task(read_telem(drone))

    server = await asyncio.start_server(
        partial(handle_echo, drone=drone), '0.0.0.0', 8888)

    addrs = ', '.join(str(sock.getsockname()) for sock in server.sockets)
    print(f'Serving on {addrs}')

    async with server:
        await server.serve_forever()

asyncio.run(main())

I'm starting PX4 in SITL mode with the following command:

HEADLESS=1 make px4_sitl gz_x500

At first glance this seems to work as telemetry_server.publish_position() doesn't return an error (telemetry_server.publish_imu() seems to be broken but that's another discussion).

However, it seems like PX4 is not actually reading the data I send it which can be confirmed by opening up a GCS or querying it's position via MAVLink.

Am i fundementally misundestanding what telemetry_server is supposed to do? or is this a bug and/or user error?

Thanks

[byt3bl33d3r]

Wrong repo. My bad.