SITL/Gazebo offboard control with attitude and throttle

PX4 Autopilot
1

Hello,

I’m trying to fly a quadcopter in a circle using offboard control and MAVSDK. I want to use the Attitude and Throttle commands.

To do this, I calculate the desired centripetal acceleration from which I extract the pitch and roll angles. I set the yaw so that it is aligned with the velocity vector (tangential).

I estimated the hover acceleration by performing simulation experiments.

Unfortunately, I’m not getting the expected results at all when I simply integrate the desired acceleration and not use PX4.

Can you help me?

Thank you

Figure_1
Figure_11648×962 106 KB

import numpy as np
import time
import asyncio
from mavsdk import System
from mavsdk.offboard import OffboardError, VelocityNedYaw, Attitude

import matplotlib
matplotlib.use("TkAgg")
import matplotlib.pyplot as plt


DESIRED_SPEED = 1.0
CENTRIPETAL_ACCEL = 0.5
HOVER_THROTTLE = 0.728

positions = []
positions_theo = []
accel_cmds = []


def compute_euler_angles_from_acceleration(yaw_deg, ned_accel):
    g = np.array([0.0, 0.0, 9.81])
    a = g - ned_accel
    pitch = -np.arcsin(a[0]/np.linalg.norm(a))
    roll = np.arctan2(a[1], a[2])
    return (np.degrees(roll), np.degrees(pitch), yaw_deg)

async def run():
    drone = System(port=50051)
    await drone.connect(system_address=f"udpin://0.0.0.0:14540")
    async for state in drone.core.connection_state():
        if state.is_connected:
            break
    async for health in drone.telemetry.health():
        if health.is_global_position_ok and health.is_home_position_ok:
            break
    await drone.action.arm()
    await drone.action.takeoff()
    await asyncio.sleep(10)
    await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 0.0, 0.0))
    try:
        await drone.offboard.start()
    except OffboardError:
        return
    await drone.offboard.set_velocity_ned(VelocityNedYaw(DESIRED_SPEED, 0.0, 0.0, 0.0))

    t = 0.0
    dt = 0.005
    yaw = 0.0 
    yaw_rate = CENTRIPETAL_ACCEL / DESIRED_SPEED

    pos_theo = [0, 0, 1.5]
    vel_theo = np.array([DESIRED_SPEED, 0.0, 0.0])

    T = time.time()

    while t < 6.0:

        accel_dir = np.array([-np.sin(yaw), np.cos(yaw), 0.0])
        accel_cmd = CENTRIPETAL_ACCEL * accel_dir + np.array([0, 0, -9.81])
        accel_cmds.append(accel_dir)

        roll_deg, pitch_deg, yaw_deg = compute_euler_angles_from_acceleration(yaw_deg=np.degrees(yaw), ned_accel=accel_cmd)
        
        throttle = (HOVER_THROTTLE * 1.0) / (np.cos(np.radians(roll_deg)) * np.cos(np.radians(pitch_deg)))


        pv = await drone.telemetry.position_velocity_ned().__anext__()
        positions.append(np.array([pv.position.north_m, pv.position.east_m, pv.position.down_m]))

        vel_theo += CENTRIPETAL_ACCEL * accel_dir  * dt
        pos_theo += vel_theo * dt
        positions_theo.append(pos_theo.copy())


        await drone.offboard.set_attitude(Attitude(roll_deg=roll_deg, pitch_deg=pitch_deg, yaw_deg=yaw_deg, thrust_value=throttle))
        await asyncio.sleep(0)

        yaw += yaw_rate * dt
        t += dt

        print(time.time() - T)
        T = time.time()


    try:
        await drone.offboard.stop()
    except OffboardError:
        return
    await drone.action.land()


if __name__ == "__main__":

    asyncio.run(run())

    def ned_to_enu(NED):
        ENU = np.zeros_like(NED)
        ENU[:, 0] = NED[:, 1]         
        ENU[:, 1] = NED[:, 0]       
        ENU[:, 2] = -NED[:, 2]    
        return ENU

    positions = ned_to_enu(np.array(positions))
    positions_theo = ned_to_enu(np.array(positions_theo))
    accel_cmds = ned_to_enu(np.array(accel_cmds))
    
    fig = plt.figure()
    ax = fig.add_subplot(111, projection="3d")

    ax.plot(
        positions[:, 0], positions[:, 1], positions[:, 2], linewidth=2,
        label="actual traj"
    )

    ax.plot(
        positions_theo[:, 0], positions_theo[:, 1], positions_theo[:, 2], "--", linewidth=3,
        label="theoretical traj"
    )

    ax.quiver(
        positions[:, 0], positions[:, 1], positions[:, 2],
        accel_cmds[:, 0], accel_cmds[:, 1], accel_cmds[:, 2],
        length=1.0, normalize=True,
        color="red", alpha=0.8, label="commanded accel"
    )

    ax.legend()

    def set_axes_equal(ax):
        x_limits = ax.get_xlim3d()
        y_limits = ax.get_ylim3d()
        z_limits = ax.get_zlim3d()

        x_range = abs(x_limits[1] - x_limits[0])
        y_range = abs(y_limits[1] - y_limits[0])
        z_range = abs(z_limits[1] - z_limits[0])

        max_range = max(x_range, y_range, z_range)

        x_middle = sum(x_limits) / 2
        y_middle = sum(y_limits) / 2
        z_middle = sum(z_limits) / 2

        ax.set_xlim3d([x_middle - max_range/2, x_middle + max_range/2])
        ax.set_ylim3d([y_middle - max_range/2, y_middle + max_range/2])
        ax.set_zlim3d([z_middle - max_range/2, z_middle + max_range/2])

    set_axes_equal(ax)

    plt.show()
2

It sounds like you’re doing open loop control but you would get better results if you close the loop by adjusting your inputs based on where you are at a given moment minus the setpoint.