example.py

"""
 Copyright (c) 2024 Pablo Ramirez Escudero
 
 This software is released under the MIT License.
 https://opensource.org/licenses/MIT
"""

import time

from simulator.aircraft import AircraftDynamics, load_airframe_parameters_from_yaml
from simulator.autopilot import Autopilot
from simulator.autopilot.mission_control import MissionControl
from simulator.autopilot.waypoints import load_waypoints_from_txt
from simulator.cli import SimConsole
from simulator.gui import AttitudePositionPanel
from simulator.utils.simulation_data import SimulationData

params_file = r"config/aerosonde_parameters.yaml"
aerosonde_params = load_airframe_parameters_from_yaml(params_file)

dt = 0.01
uav = AircraftDynamics(dt, aerosonde_params, use_quat=True)
x_trim, delta_trim = uav.trim(Va=25.0)

autopilot = Autopilot(dt, aerosonde_params, uav.state)

waypoints_file = r"config/go_waypoint.wp"
waypoints_list = load_waypoints_from_txt(waypoints_file)
mission = MissionControl(dt, autopilot.config)
mission.initialize(waypoints_list, Va=25.0, h=0.0, chi=0.0)

cli = SimConsole()
gui = AttitudePositionPanel(use_blit=False, pos_3d=True)

t_sim = 0.0  # simulation time
k_sim = 0  # simulation steps
t0 = time.time()
while True:
    t_sim += dt
    k_sim += 1

    uav.update(autopilot.control_deltas)  # update simulation states

    flight_cmd = mission.update(uav.state.ned_position, uav.state.course_angle)
    autopilot.status.update_aircraft_state(uav.state)
    autopilot.control_course_altitude_airspeed(
        flight_cmd.course, flight_cmd.altitude, flight_cmd.airspeed
    )

    gui.add_data(state=uav.state)

    t_real = time.time() - t0
    if t_sim > t_real:
        cli.update(
            SimulationData(
                dt_sim=dt,
                t_sim=t_sim,
                k_sim=k_sim,
                uav_state=uav.state,
                control_deltas=autopilot.control_deltas,
                autopilot_status=autopilot.status,
                mission_control=mission,
            )
        )
        gui.update(state=uav.state, pause=0.01)