[Tutorial] get orientation angles from RF9

Jack732 · Dec 3, 2023

Since nobody on here has copy-pasted a simple code snippet to retrieve the angular orientation data from an aircraft whilst flying in RF9, I'll do it, somebody has to do it-*looks up at the sky* I am the chosen one. The code right now just has placeholder controller input values set, you will have to change these to work with whatever controller you're using or algorithm. Use ChatGPT to figure that part out. But, if you just want to see the angles change while you fly a plane with ur regular spektrum controller, or whatever controller you have connected, you can do the following:

1.) load up realflight, select aircraft
2.) execute my python script
3.) select to change the aircraft youre flying

now you should be able to fly using ur controller, i dont know why but that works for me. it like stops accepting the input dummy values from my script and starts accepting regular input values, Also the naming within the script is swapped for roll and pitch but its correct in the terminal where ya see it printed.

AND HERES THE SACRED GOLDEN LINK TO MY SCRIPT:

Get_RF9_angles_viaPython.py

drive.google.com

you're welcome.

but if that link gets fricked at some point, here it is again:

Code:

#original link where all this was found: https://github.com/camdeno/F16Capstone/blob/main/FlightAxis/flightaxis.py

import time
import math
import signal
import sys
import csv
from datetime import datetime

import pygame
import requests
import navpy
from bs4 import BeautifulSoup
from pymavlink import mavutil
import numpy as np
import quaternionic


class FlightAxis:
    """
    Python version of Ardupilot's FlightAxis implementation, available here:
    - https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_FlightAxis.h
    - https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_FlightAxis.cpp

    Example of FlightAxis response is here: http://uav.tridgell.net/RealFlight/data-exchange.txt

    More details about FlightAxis interface:
    - https://www.knifeedge.com/forums/index.php?threads/flightaxis-link-q-a.32854/
    - https://www.knifeedge.com/forums/index.php?threads/flightaxis-link-network-access-to-realflight-for-developers.32809/#post-282152


    # Performance
    A typical performance measured on a reasonable desktop PC is:
    avg dt = 0.0042965165828869264[s], avg Freq = 232.746686928432

    # Workflow:
    1. get HIL_ACTUATOR_CONTROLS from PX4
    2. exchange data with flight axis
    3. send HIL_SENSOR and HIL_GPS to PX4

    """
    REALFLIGHT_URL = "http://127.0.0.1:18083"
    RC_CHANNLES = 12
    TIMEOUT_S = 0.05

    LAT_0 = 45.25608036722125 # [deg]
    LON_0 = -122.84116451943059 # [deg]
    ALT_0 = 36 # [m]

    # Tuples with mapping values
    # (joystick_channel_idx, RC_IN channel idx, invert?)
    # Incoming:
    # rc_in idx -> SOAP order
    # 0 -> yaw (3)
    # 1 -> throttle (2)
    # 2 -> roll (0) # invert
    # 3 -> pitch (1)

    # Desired:
    # 0 -> roll
    # 1 -> pitch
    # 2 -> yaw
    # 3 -> throttle

    #Throttle idx 3
    # Rudder idx 2
    # Roll idx 0
    # Pitch ?
    YAW_IDX = (0,3,True)
    THROTTLE_IDX = (1,2,False)
    ROLL_IDX = (2,0,True)
    PITCH_IDX = (3,1,False)
    GEAR_IDX = (4,6,False)
    MODE_IDX = (5,7,False)
    CHANNELS = [YAW_IDX, THROTTLE_IDX, ROLL_IDX, PITCH_IDX, GEAR_IDX, MODE_IDX]

    def quaternion_to_euler(self):
        w, x, y, z = self.quats_as_list  # Assuming quats_as_list is [w, x, y, z]

        # Roll (x-axis rotation)
        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        # Pitch (y-axis rotation)
        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp) if abs(sinp) <= 1 else np.pi / 2 * np.sign(sinp)

        # Yaw (z-axis rotation)
        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        # Convert from radians to degrees and then to integers
        roll_deg = int(np.degrees(roll))
        pitch_deg = int(np.degrees(pitch))
        yaw_deg = int(np.degrees(yaw))

        return roll_deg, pitch_deg, yaw_deg


    def getHeader(self) -> list:
        header = [key for key in self.getKeytable()] + [f"rc_channel_{idx}" for idx in range(FlightAxis.RC_CHANNLES)]
        return header

    def getKeytable(self) -> dict:
        keytable = {
            "m-airspeed-MPS": "self.m_airspeed_MPS",
            "m-altitudeASL-MTR": "self.m_altitudeASL_MTR",
            "m-altitudeAGL-MTR": "self.m_altitudeAGL_MTR",
            "m-groundspeed-MPS": "self.m_groundspeed_MPS",
            "m-pitchRate-DEGpSEC": "self.m_pitchRate_DEGpSEC",
            "m-rollRate-DEGpSEC": "self.m_rollRate_DEGpSEC",
            "m-yawRate-DEGpSEC": "self.m_yawRate_DEGpSEC",
            "m-azimuth-DEG": "self.m_azimuth_DEG",
            "m-inclination-DEG": "self.m_inclination_DEG",
            "m-roll-DEG": "self.m_roll_DEG",
            "m-aircraftPositionX-MTR": "self.m_aircraftPositionX_MTR",
            "m-aircraftPositionY-MTR": "self.m_aircraftPositionY_MTR",
            "m-velocityWorldU-MPS": "self.m_velocityWorldU_MPS",
            "m-velocityWorldV-MPS": "self.m_velocityWorldV_MPS",
            "m-velocityWorldW-MPS": "self.m_velocityWorldW_MPS",
            "m-velocityBodyU-MPS": "self.m_velocityBodyU_MPS",
            "m-velocityBodyV-MPS": "self.m_velocityBodyV_MPS",
            "m-velocityBodyW-MPS": "self.m_velocityBodyW_MPS",
            "m-accelerationWorldAX-MPS2": "self.m_accelerationWorldAX_MPS2",
            "m-accelerationWorldAY-MPS2": "self.m_accelerationWorldAY_MPS2",
            "m-accelerationWorldAZ-MPS2": "self.m_accelerationWorldAZ_MPS2",
            "m-accelerationBodyAX-MPS2": "self.m_accelerationBodyAX_MPS2",
            "m-accelerationBodyAY-MPS2": "self.m_accelerationBodyAY_MPS2",
            "m-accelerationBodyAZ-MPS2": "self.m_accelerationBodyAZ_MPS2",
            "m-windX-MPS": "self.m_windX_MPS",
            "m-windY-MPS": "self.m_windY_MPS",
            "m-windZ-MPS": "self.m_windZ_MPS",
            "m-propRPM": "self.m_propRPM",
            "m-heliMainRotorRPM": "self.m_heliMainRotorRPM",
            "m-batteryVoltage-VOLTS": "self.m_batteryVoltage_VOLTS",
            "m-batteryCurrentDraw-AMPS": "self.m_batteryCurrentDraw_AMPS",
            "m-batteryRemainingCapacity-MAH": "self.m_batteryRemainingCapacity_MAH",
            "m-fuelRemaining-OZ": "self.m_fuelRemaining_OZ",
            "m-isLocked": "self.m_isLocked",
            "m-hasLostComponents": "self.m_hasLostComponents",
            "m-anEngineIsRunning": "self.m_anEngineIsRunning",
            "m-isTouchingGround": "self.m_isTouchingGround",
            # TODO: needs better string handling here
            # "m-currentAircraftStatus": "self.m_currentAircraftStatus",
            "m-currentPhysicsTime-SEC": "self.m_currentPhysicsTime_SEC",
            "m-currentPhysicsSpeedMultiplier": "self.m_currentPhysicsSpeedMultiplier",
            "m-orientationQuaternion-X": "self.m_orientationQuaternion_X",
            "m-orientationQuaternion-Y": "self.m_orientationQuaternion_Y",
            "m-orientationQuaternion-Z": "self.m_orientationQuaternion_Z",
            "m-orientationQuaternion-W": "self.m_orientationQuaternion_W",
        }
        return keytable

    def __init__(self):
        self.last_frame_time = time.time()
        self.avg_dt = 0.0

        # Default position
        self.lat = FlightAxis.LAT_0
        self.lon = FlightAxis.LON_0

        self.rcin = [0.0 for _ in range(FlightAxis.RC_CHANNLES)]
        self.m_airspeed_MPS = 0.0
        self.m_altitudeASL_MTR = 0.0
        self.m_altitudeAGL_MTR = 0.0
        self.m_groundspeed_MPS = 0.0
        self.m_pitchRate_DEGpSEC = 0.0
        self.m_rollRate_DEGpSEC = 0.0
        self.m_yawRate_DEGpSEC = 0.0
        self.m_azimuth_DEG = 0.0
        self.m_inclination_DEG = 0.0
        self.m_roll_DEG = 0.0
        self.m_aircraftPositionX_MTR = 0.0
        self.m_aircraftPositionY_MTR = 0.0
        self.m_velocityWorldU_MPS = 0.0
        self.m_velocityWorldV_MPS = 0.0
        self.m_velocityWorldW_MPS = 0.0
        self.m_velocityBodyU_MPS = 0.0
        self.m_velocityBodyV_MPS = 0.0
        self.m_velocityBodyW_MPS = 0.0
        self.m_accelerationWorldAX_MPS2 = 0.0
        self.m_accelerationWorldAY_MPS2 = 0.0
        self.m_accelerationWorldAZ_MPS2 = 0.0
        self.m_accelerationBodyAX_MPS2 = 0.0
        self.m_accelerationBodyAY_MPS2 = 0.0
        self.m_accelerationBodyAZ_MPS2 = 0.0
        self.m_windX_MPS = 0.0
        self.m_windY_MPS = 0.0
        self.m_windZ_MPS = 0.0
        self.m_propRPM = 0.0
        self.m_heliMainRotorRPM = 0.0
        self.m_batteryVoltage_VOLTS = 0.0
        self.m_batteryCurrentDraw_AMPS = 0.0
        self.m_batteryRemainingCapacity_MAH = 0.0
        self.m_fuelRemaining_OZ = 0.0
        self.m_isLocked = 0.0
        self.m_hasLostComponents = 0.0
        self.m_anEngineIsRunning = 0.0
        self.m_isTouchingGround = 0.0
        self.m_currentAircraftStatus = "None"
        self.m_currentPhysicsTime_SEC = 0.0
        self.m_currentPhysicsSpeedMultiplier = 0.0
        self.m_orientationQuaternion_X = 0.0
        self.m_orientationQuaternion_Y = 0.0
        self.m_orientationQuaternion_Z = 0.0
        self.m_orientationQuaternion_W = 0.0
        self.m_flightAxisControllerIsActive = False
        self.m_resetButtonHasBeenPressed = False
        self.rows = []  # Initialize the rows attribute as an empty list

    def enableRC(self, doPrint=False) -> bool:
        """
        Set Spektrum as the RC input
        Return True if the response was OK
        """
        headers = {'content-type': "text/xml;charset='UTF-8'",
                'soapaction': 'RestoreOriginalControllerDevice',
                'Connection': 'Keep-Alive'}

        body = "<?xml version='1.0' encoding='UTF-8'?>\
        <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>\
        <soap:Body>\
        <RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>\
        </soap:Body>\
        </soap:Envelope>"

        response = requests.post(FlightAxis.REALFLIGHT_URL,data=body,headers=headers)
        if doPrint:
            print(response.content)
        return response.ok

    def resetSim(self, doPrint=False) -> bool:
        """
        Reset Real Flight simulator,
        per post here: https://www.knifeedge.com/forums/index.php?threads/realflight-reset-soap-envelope.52333/
        NOTE: untested
        """
        headers = {'content-type': "text/xml;charset='UTF-8'",
                   'soapaction': 'ResetAircraft'}
        body = "<?xml version='1.0' encoding='UTF-8'?>\
        <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>\
        <soap:Body>\
        <ResetAircraft><a>1</a><b>2</b></ResetAircraft>\
        </soap:Body>\
        </soap:Envelope>"

    def disableRC(self, doPrint=False) -> bool:
        """
        Disable Spektrum as the RC input, and use FlightAxis instead
        Return True if the response was OK
        """
        headers = {'content-type': "text/xml;charset='UTF-8'",
                'soapaction': 'InjectUAVControllerInterface'}

        body = "<?xml version='1.0' encoding='UTF-8'?>\
        <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>\
        <soap:Body>\
        <InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>\
        </soap:Body>\
        </soap:Envelope>"

        response = requests.post(FlightAxis.REALFLIGHT_URL, data=body,headers=headers)
        if doPrint:
            print(response.content)
        return response.ok

    def scaleJoystickValue(self, value, invert=False) -> float:
        # Condition channel
        #     # -0.66..0.66 to 0-1
        # Interval scaling from https://stats.stackexchange.com/a/281164
        R_MIN=-0.66
        R_MAX=0.66
        T_MIN=0.0
        T_MAX=1.0

        k = 1.0
        if invert:
            k = -1.0
        scaled_value = (k*value-R_MIN)/(R_MAX-R_MIN)*(T_MAX-T_MIN)+T_MIN
        if scaled_value < 0.0:
            scaled_value = 0.0
        elif scaled_value > 1.0:
            scaled_value = 1.0
        return scaled_value

    def updateActuators(self,values):
        for channel in FlightAxis.CHANNELS:
            joy_idx, rc_idx, channelInverted = channel
            self.rcin[rc_idx] = self.scaleJoystickValue(values[joy_idx],invert=channelInverted)

    def getStates(self, doPrint=False) -> bool:
        """
        Set the control inputs, and get the states
        Return True if the response was OK
        """
        headers = {'content-type': "text/xml;charset='UTF-8'",
                'soapaction': 'ExchangeData'}

        body = f"<?xml version='1.0' encoding='UTF-8'?><soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>\
        <soap:Body>\
        <ExchangeData>\
        <pControlInputs>\
        <m-selectedChannels>4095</m-selectedChannels>\
        <m-channelValues-0to1>\
        <item>{self.rcin[0]}</item>\
        <item>{self.rcin[1]}</item>\
        <item>{self.rcin[2]}</item>\
        <item>{self.rcin[3]}</item>\
        <item>{self.rcin[4]}</item>\
        <item>{self.rcin[5]}</item>\
        <item>{self.rcin[6]}</item>\
        <item>{self.rcin[7]}</item>\
        <item>{self.rcin[8]}</item>\
        <item>{self.rcin[9]}</item>\
        <item>{self.rcin[10]}</item>\
        <item>{self.rcin[11]}</item>\
        </m-channelValues-0to1>\
        </pControlInputs>\
        </ExchangeData>\
        </soap:Body>\
        </soap:Envelope>"

        response = requests.post(FlightAxis.REALFLIGHT_URL,data=body,headers=headers)

        if doPrint:
            print(response.content)

        if response.ok:
            now = time.time()
            dt = now - self.last_frame_time
            self.avg_dt = (self.avg_dt + dt)/2.0
            self.last_frame_time = now
            self.parseResponse(response.content)
            return True
        # Catch all returns false
        return False

    def parseResponse(self, xml):
        """
        Update internal states from FlightAxis response
        """
        parsed = BeautifulSoup(xml, 'xml')
        body = parsed.find('m-aircraftState')
        assert(body)

        self.row = []
        # Use keytable for rows
        keytable = self.getKeytable()
        for k,v in keytable.items():
            tag = body.find(k)
            tag_val = tag.string
            if tag_val == "false" or tag_val == "true":
                tag_val = tag_val.capitalize()
            cmd = f"{v} = {tag_val}"
            exec(cmd)
            cmd = f"self.row.append({tag_val})"
            exec(cmd)

        # Add actuators
        for idx in range(FlightAxis.RC_CHANNLES):
            self.row.append(self.rcin[idx])

        # Actually append the rows
        self.rows.append(self.row)

        # Update attitude
        self.quats_as_list = [self.m_orientationQuaternion_W,
                              self.m_orientationQuaternion_X,
                              self.m_orientationQuaternion_Y,
                              self.m_orientationQuaternion_Z]

        roll, pitch, yaw = self.quaternion_to_euler()
        # Formatting the output
        print(f"Roll: {pitch:5d}, Pitch: {roll:5d}, Yaw: {yaw:5d}")

        # TODO: is the order of the axis correct?
        self.quats = quaternionic.array(self.quats_as_list)
        #print(f"{self.quats_as_list}")
        # Update position
        ned = [self.m_aircraftPositionX_MTR,
              self.m_aircraftPositionY_MTR,
              self.m_altitudeAGL_MTR]
        self.lat,self.lon, _ = navpy.ned2lla(ned, FlightAxis.LAT_0, FlightAxis.LON_0, FlightAxis.ALT_0,
                                    latlon_unit='deg', alt_unit='m', model='wgs84')

    def getHilActuatorControls(self) -> bool:
        """
        Assign dummy values to actuator controls using the CHANNELS mapping.
        Returns True always as the message reception is simulated.
        """
        # Define and update self.rcin with dummy values
        for idx in range(FlightAxis.RC_CHANNLES):
            self.rcin[idx] = 0.5  # Dummy value for all channels

        # Update self.rcin based on the FlightAxis.CHANNELS mapping
        dummy_joystick_values = {
            # Assuming the format is {joystick_channel_idx: value}
            # These values can be adjusted as per your requirement
            0: 0.5,  # Example value for a joystick channel
            1: 0.5,
            2: 0.5,
            3: 0.5,
            # ... Add other joystick channels if necessary
        }

        for channel in FlightAxis.CHANNELS:
            joy_idx, rc_idx, channelInverted = channel
            joystick_value = dummy_joystick_values.get(joy_idx, 0.0)
            self.rcin[rc_idx] = self.scaleJoystickValue(joystick_value, invert=channelInverted)

        return True

def test_parse1():
    res = b'<?xml version="1.0" encoding="UTF-8"?>\n<SOAP-ENV:Envelope xmlns:SOAP-ENV="http://schemas.xmlsoap.org/soap/envelope/" xmlns:SOAP-ENC="http://schemas.xmlsoap.org/soap/encoding/" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:xsd="http://www.w3.org/2001/XMLSchema"><SOAP-ENV:Body><ReturnData><m-previousInputsState><m-selectedChannels>-1</m-selectedChannels><m-channelValues-0to1 xsi:type="SOAP-ENC:Array" SOAP-ENC:arrayType="xsd:double[12]"><item>1</item><item>0</item><item>0.51503002643585205</item><item>1</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item></m-channelValues-0to1></m-previousInputsState><m-aircraftState><m-currentPhysicsTime-SEC>9.4901527954498306</m-currentPhysicsTime-SEC><m-currentPhysicsSpeedMultiplier>1</m-currentPhysicsSpeedMultiplier><m-airspeed-MPS>4.2668099647568329</m-airspeed-MPS><m-altitudeASL-MTR>0.23033138335698844</m-altitudeASL-MTR><m-altitudeAGL-MTR>0.23033138335698844</m-altitudeAGL-MTR><m-groundspeed-MPS>4.2655322162874292</m-groundspeed-MPS><m-pitchRate-DEGpSEC>46.914449474279536</m-pitchRate-DEGpSEC><m-rollRate-DEGpSEC>-64.392902600666275</m-rollRate-DEGpSEC><m-yawRate-DEGpSEC>-95.093221536022611</m-yawRate-DEGpSEC><m-azimuth-DEG>156.77792358398437</m-azimuth-DEG><m-inclination-DEG>-13.11492919921875</m-inclination-DEG><m-roll-DEG>6.6998014450073242</m-roll-DEG><m-orientationQuaternion-X>0.079808555543422699</m-orientationQuaternion-X><m-orientationQuaternion-Y>0.099987812340259552</m-orientationQuaternion-Y><m-orientationQuaternion-Z>-0.97012227773666382</m-orientationQuaternion-Z><m-orientationQuaternion-W>-0.20614470541477203</m-orientationQuaternion-W><m-aircraftPositionX-MTR>18.125473022460937</m-aircraftPositionX-MTR><m-aircraftPositionY-MTR>13.188897132873535</m-aircraftPositionY-MTR><m-velocityWorldU-MPS>-4.0696659088134766</m-velocityWorldU-MPS><m-velocityWorldV-MPS>1.2777262926101685</m-velocityWorldV-MPS><m-velocityWorldW-MPS>0.10441353917121887</m-velocityWorldW-MPS><m-velocityBodyU-MPS>3.1754355430603027</m-velocityBodyU-MPS><m-velocityBodyV-MPS>-2.8336892127990723</m-velocityBodyV-MPS><m-velocityBodyW-MPS>-0.30408719182014465</m-velocityBodyW-MPS><m-accelerationWorldAX-MPS2>-4.4059576988220215</m-accelerationWorldAX-MPS2><m-accelerationWorldAY-MPS2>-6.3290410041809082</m-accelerationWorldAY-MPS2><m-accelerationWorldAZ-MPS2>8.5558643341064453</m-accelerationWorldAZ-MPS2><m-accelerationBodyAX-MPS2>1.2007522583007813</m-accelerationBodyAX-MPS2><m-accelerationBodyAY-MPS2>8.1494748592376709</m-accelerationBodyAY-MPS2><m-accelerationBodyAZ-MPS2>-8.7651233673095703</m-accelerationBodyAZ-MPS2><m-windX-MPS>0</m-windX-MPS><m-windY-MPS>0</m-windY-MPS><m-windZ-MPS>0</m-windZ-MPS><m-propRPM>10893.9326171875</m-propRPM><m-heliMainRotorRPM>-1</m-heliMainRotorRPM><m-batteryVoltage-VOLTS>-1</m-batteryVoltage-VOLTS><m-batteryCurrentDraw-AMPS>-1</m-batteryCurrentDraw-AMPS><m-batteryRemainingCapacity-MAH>-1</m-batteryRemainingCapacity-MAH><m-fuelRemaining-OZ>11.978337287902832</m-fuelRemaining-OZ><m-isLocked>false</m-isLocked><m-hasLostComponents>false</m-hasLostComponents><m-anEngineIsRunning>true</m-anEngineIsRunning><m-isTouchingGround>true</m-isTouchingGround><m-flightAxisControllerIsActive>true</m-flightAxisControllerIsActive><m-currentAircraftStatus>CAS-FLYING</m-currentAircraftStatus></m-aircraftState><m-notifications><m-resetButtonHasBeenPressed>false</m-resetButtonHasBeenPressed></m-notifications></ReturnData></SOAP-ENV:Body></SOAP-ENV:Envelope>'
    fa = FlightAxis()
    fa.parseResponse(res)

def test_parse2():
    res = b'<?xml version="1.0" encoding="UTF-8"?>\n<SOAP-ENV:Envelope xmlns:SOAP-ENV="http://schemas.xmlsoap.org/soap/envelope/" xmlns:SOAP-ENC="http://schemas.xmlsoap.org/soap/encoding/" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:xsd="http://www.w3.org/2001/XMLSchema"><SOAP-ENV:Body><ReturnData><m-previousInputsState><m-selectedChannels>-1</m-selectedChannels><m-channelValues-0to1 xsi:type="SOAP-ENC:Array" SOAP-ENC:arrayType="xsd:double[12]"><item>1</item><item>0</item><item>0.51503002643585205</item><item>1</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item></m-channelValues-0to1></m-previousInputsState><m-aircraftState><m-currentPhysicsTime-SEC>9.5060089953476563</m-currentPhysicsTime-SEC><m-currentPhysicsSpeedMultiplier>1</m-currentPhysicsSpeedMultiplier><m-airspeed-MPS>3.973699447829139</m-airspeed-MPS><m-altitudeASL-MTR>0.23263369561650649</m-altitudeASL-MTR><m-altitudeAGL-MTR>0.23263369561650649</m-altitudeAGL-MTR><m-groundspeed-MPS>3.9671952662134293</m-groundspeed-MPS><m-pitchRate-DEGpSEC>65.486709524862817</m-pitchRate-DEGpSEC><m-rollRate-DEGpSEC>-113.29177463687665</m-rollRate-DEGpSEC><m-yawRate-DEGpSEC>-115.36654693618766</m-yawRate-DEGpSEC><m-azimuth-DEG>154.92184448242187</m-azimuth-DEG><m-inclination-DEG>-12.265393257141113</m-inclination-DEG><m-roll-DEG>4.7752151489257812</m-roll-DEG><m-orientationQuaternion-X>0.063606671988964081</m-orientationQuaternion-X><m-orientationQuaternion-Y>0.095200046896934509</m-orientationQuaternion-Y><m-orientationQuaternion-Z>-0.96875286102294922</m-orientationQuaternion-Z><m-orientationQuaternion-W>-0.22001981735229492</m-orientationQuaternion-W><m-aircraftPositionX-MTR>18.143898010253906</m-aircraftPositionX-MTR><m-aircraftPositionY-MTR>13.126790046691895</m-aircraftPositionY-MTR><m-velocityWorldU-MPS>-3.8283603191375732</m-velocityWorldU-MPS><m-velocityWorldV-MPS>1.0403343439102173</m-velocityWorldV-MPS><m-velocityWorldW-MPS>-0.22726421058177948</m-velocityWorldW-MPS><m-velocityBodyU-MPS>2.9091477394104004</m-velocityBodyU-MPS><m-velocityBodyV-MPS>-2.6280345916748047</m-velocityBodyV-MPS><m-velocityBodyW-MPS>-0.64850509166717529</m-velocityBodyW-MPS><m-accelerationWorldAX-MPS2>-4.3766188621520996</m-accelerationWorldAX-MPS2><m-accelerationWorldAY-MPS2>-6.6690726280212402</m-accelerationWorldAY-MPS2><m-accelerationWorldAZ-MPS2>9.2979526519775391</m-accelerationWorldAZ-MPS2><m-accelerationBodyAX-MPS2>-0.53152298927307129</m-accelerationBodyAX-MPS2><m-accelerationBodyAY-MPS2>10.025743305683136</m-accelerationBodyAY-MPS2><m-accelerationBodyAZ-MPS2>-8.8302364349365234</m-accelerationBodyAZ-MPS2><m-windX-MPS>0</m-windX-MPS><m-windY-MPS>0</m-windY-MPS><m-windZ-MPS>0</m-windZ-MPS><m-propRPM>10886.970703125</m-propRPM><m-heliMainRotorRPM>-1</m-heliMainRotorRPM><m-batteryVoltage-VOLTS>-1</m-batteryVoltage-VOLTS><m-batteryCurrentDraw-AMPS>-1</m-batteryCurrentDraw-AMPS><m-batteryRemainingCapacity-MAH>-1</m-batteryRemainingCapacity-MAH><m-fuelRemaining-OZ>11.978252410888672</m-fuelRemaining-OZ><m-isLocked>false</m-isLocked><m-hasLostComponents>false</m-hasLostComponents><m-anEngineIsRunning>true</m-anEngineIsRunning><m-isTouchingGround>true</m-isTouchingGround><m-flightAxisControllerIsActive>true</m-flightAxisControllerIsActive><m-currentAircraftStatus>CAS-FLYING</m-currentAircraftStatus></m-aircraftState><m-notifications><m-resetButtonHasBeenPressed>false</m-resetButtonHasBeenPressed></m-notifications></ReturnData></SOAP-ENV:Body></SOAP-ENV:Envelope>'
    fa = FlightAxis()
    fa.parseResponse(res)


# NOTE: try something cleaner, such as using context manager
# and __enter__ and __exit__ dunder methods:
# '''
# with FlightAxisManager() as axis_manager:
#   do stuff
# '''

def signal_handler(sig, frame):
    print('You pressed Ctrl+C!')
    print("RC terminating")
    pygame.quit()
    fa.saveStates()
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C')


JOYSTICK_DIRECT = False

if JOYSTICK_DIRECT:
    pygame.init()
    pygame.joystick.init()

    joystick = pygame.joystick.Joystick(0)
    joystick.init()
    print(f"{joystick.get_name()}")


fa = FlightAxis()
# 1) Disable RC control and start FlightAxis
fa.enableRC()
fa.disableRC()
# 2) exchange data once to set RC inputs to zeros
fa.getStates()
# 3) reset the simulator to a known initial state
fa.resetSim()

rc_values = {0: 0, 1:0, 2:0, 3:0, 4:0, 5:0, 6:0, 7:0, 8:0}
done = False
now = time.time()
while not done:
    t = time.time()
    dt = t - now
    if (dt > 1.0):
        # print stats
        #print(f"avg_f = {1/fa.avg_dt}[HZ]")
        now = t
    # TODO: hide this inside FlightAxis(), perhaps a conf in the constructor?
    if JOYSTICK_DIRECT:
        for event in pygame.event.get():
            #print(event)
            if event.type == pygame.QUIT:
                done = True
            if event.type == pygame.JOYAXISMOTION:
                rc_values[event.axis] = event.value
        fa.updateActuators(rc_values)
    fa.getStates()
    fa.getHilActuatorControls()
print("DONE")

and i swear to god if the mods hit me for duplicates when people have been asking for how to get these angles for years

Ryan Douglas · Dec 4, 2023

In all seriousness, thanks for contributing to the shared pool of knowledge. We make the RealFlight Link functionality available but aren't able to actively support it. It works largely because the target userbase is able to figure out what they need to and shares their wisdom.

I haven't reviewed your script closely but I'm sure it will help some people.

Jack732 · Dec 8, 2023

These forums really helped me when I first started using RF9. Thought I should give back.

Also here's an updated script, it uses some new libraries so you can just plug in a cheap xbox one controller via usb into your computer, and that will act as the input into the simulator.

Python:

#original link where all this was found: https://github.com/camdeno/F16Capstone/blob/main/FlightAxis/flightaxis.py

import time
import math
import signal
import sys
import csv
from datetime import datetime

import pygame
import requests
import navpy
from bs4 import BeautifulSoup
from pymavlink import mavutil
import numpy as np
import quaternionic

pygame.init()
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0)
joystick.init()

# Define a variable to track the last print time
last_print_time = time.time()
print_interval = 0.5  # seconds between prints

def convert_to_range(value):
    # Converts the joystick value (-1 to 1) to the range 1000 to 2000 and ensures it's an integer
    return int((value + 1) * 500 + 1000)

def button_to_range(value):
    # Converts the button state (0 or 1) to the range 1000 to 2000 and ensures it's an integer
    return 1000 if value == 0 else 2000

# Function to read controller values
def read_controller():
    pygame.event.pump()  # Process event queue
    # Read joystick values directly
    left_x = -joystick.get_axis(0)
    left_y = -joystick.get_axis(1)
    right_x = -joystick.get_axis(2)
    right_y = -joystick.get_axis(3)
    # Read and convert button states if necessary
    left_stick_press = joystick.get_button(8)
    right_stick_press = joystick.get_button(9)

    return {
        0: left_x,
        1: left_y,
        2: right_x,
        3: right_y,
        4: left_stick_press,
        5: right_stick_press
    }


class FlightAxis:

    """
    Python version of Ardupilot's FlightAxis implementation, available here:
    - https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_FlightAxis.h
    - https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_FlightAxis.cpp

    Example of FlightAxis response is here: http://uav.tridgell.net/RealFlight/data-exchange.txt

    More details about FlightAxis interface:
    - https://www.knifeedge.com/forums/index.php?threads/flightaxis-link-q-a.32854/
    - https://www.knifeedge.com/forums/index.php?threads/flightaxis-link-network-access-to-realflight-for-developers.32809/#post-282152


    # Performance
    A typical performance measured on a reasonable desktop PC is:
    avg dt = 0.0042965165828869264[s], avg Freq = 232.746686928432

    # Workflow:
    1. get HIL_ACTUATOR_CONTROLS from PX4
    2. exchange data with flight axis
    3. send HIL_SENSOR and HIL_GPS to PX4

    """
    REALFLIGHT_URL = "http://127.0.0.1:18083"
    RC_CHANNLES = 12
    TIMEOUT_S = 0.05

    LAT_0 = 45.25608036722125 # [deg]
    LON_0 = -122.84116451943059 # [deg]
    ALT_0 = 36 # [m]

    # Tuples with mapping values
    # (joystick_channel_idx, RC_IN channel idx, invert?)
    # Incoming:
    # rc_in idx -> SOAP order
    # 0 -> yaw (3)
    # 1 -> throttle (2)
    # 2 -> roll (0) # invert
    # 3 -> pitch (1)

    # Desired:
    # 0 -> roll
    # 1 -> pitch
    # 2 -> yaw
    # 3 -> throttle

    #Throttle idx 3
    # Rudder idx 2
    # Roll idx 0
    # Pitch ?
    YAW_IDX = (0,3,True)
    THROTTLE_IDX = (1,2,False)
    ROLL_IDX = (2,0,True)
    PITCH_IDX = (3,1,False)
    GEAR_IDX = (4,6,False)
    MODE_IDX = (5,7,False)
    CHANNELS = [YAW_IDX, THROTTLE_IDX, ROLL_IDX, PITCH_IDX, GEAR_IDX, MODE_IDX]


    def quaternion_to_euler(self):
        w, x, y, z = self.quats_as_list  # Assuming quats_as_list is [w, x, y, z]

        # Roll (x-axis rotation)
        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        # Pitch (y-axis rotation)
        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp) if abs(sinp) <= 1 else np.pi / 2 * np.sign(sinp)

        # Yaw (z-axis rotation)
        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        # Convert from radians to degrees and then to integers
        roll_deg = int(np.degrees(roll))
        pitch_deg = int(np.degrees(pitch))
        yaw_deg = int(np.degrees(yaw))

        return roll_deg, pitch_deg, yaw_deg


    def getHeader(self) -> list:
        header = [key for key in self.getKeytable()] + [f"rc_channel_{idx}" for idx in range(FlightAxis.RC_CHANNLES)]
        return header

    def getKeytable(self) -> dict:
        keytable = {
            "m-airspeed-MPS": "self.m_airspeed_MPS",
            "m-altitudeASL-MTR": "self.m_altitudeASL_MTR",
            "m-altitudeAGL-MTR": "self.m_altitudeAGL_MTR",
            "m-groundspeed-MPS": "self.m_groundspeed_MPS",
            "m-pitchRate-DEGpSEC": "self.m_pitchRate_DEGpSEC",
            "m-rollRate-DEGpSEC": "self.m_rollRate_DEGpSEC",
            "m-yawRate-DEGpSEC": "self.m_yawRate_DEGpSEC",
            "m-azimuth-DEG": "self.m_azimuth_DEG",
            "m-inclination-DEG": "self.m_inclination_DEG",
            "m-roll-DEG": "self.m_roll_DEG",
            "m-aircraftPositionX-MTR": "self.m_aircraftPositionX_MTR",
            "m-aircraftPositionY-MTR": "self.m_aircraftPositionY_MTR",
            "m-velocityWorldU-MPS": "self.m_velocityWorldU_MPS",
            "m-velocityWorldV-MPS": "self.m_velocityWorldV_MPS",
            "m-velocityWorldW-MPS": "self.m_velocityWorldW_MPS",
            "m-velocityBodyU-MPS": "self.m_velocityBodyU_MPS",
            "m-velocityBodyV-MPS": "self.m_velocityBodyV_MPS",
            "m-velocityBodyW-MPS": "self.m_velocityBodyW_MPS",
            "m-accelerationWorldAX-MPS2": "self.m_accelerationWorldAX_MPS2",
            "m-accelerationWorldAY-MPS2": "self.m_accelerationWorldAY_MPS2",
            "m-accelerationWorldAZ-MPS2": "self.m_accelerationWorldAZ_MPS2",
            "m-accelerationBodyAX-MPS2": "self.m_accelerationBodyAX_MPS2",
            "m-accelerationBodyAY-MPS2": "self.m_accelerationBodyAY_MPS2",
            "m-accelerationBodyAZ-MPS2": "self.m_accelerationBodyAZ_MPS2",
            "m-windX-MPS": "self.m_windX_MPS",
            "m-windY-MPS": "self.m_windY_MPS",
            "m-windZ-MPS": "self.m_windZ_MPS",
            "m-propRPM": "self.m_propRPM",
            "m-heliMainRotorRPM": "self.m_heliMainRotorRPM",
            "m-batteryVoltage-VOLTS": "self.m_batteryVoltage_VOLTS",
            "m-batteryCurrentDraw-AMPS": "self.m_batteryCurrentDraw_AMPS",
            "m-batteryRemainingCapacity-MAH": "self.m_batteryRemainingCapacity_MAH",
            "m-fuelRemaining-OZ": "self.m_fuelRemaining_OZ",
            "m-isLocked": "self.m_isLocked",
            "m-hasLostComponents": "self.m_hasLostComponents",
            "m-anEngineIsRunning": "self.m_anEngineIsRunning",
            "m-isTouchingGround": "self.m_isTouchingGround",
            # TODO: needs better string handling here
            # "m-currentAircraftStatus": "self.m_currentAircraftStatus",
            "m-currentPhysicsTime-SEC": "self.m_currentPhysicsTime_SEC",
            "m-currentPhysicsSpeedMultiplier": "self.m_currentPhysicsSpeedMultiplier",
            "m-orientationQuaternion-X": "self.m_orientationQuaternion_X",
            "m-orientationQuaternion-Y": "self.m_orientationQuaternion_Y",
            "m-orientationQuaternion-Z": "self.m_orientationQuaternion_Z",
            "m-orientationQuaternion-W": "self.m_orientationQuaternion_W",
        }
        return keytable

    def __init__(self):
        self.last_frame_time = time.time()
        self.avg_dt = 0.0

        # Default position
        self.lat = FlightAxis.LAT_0
        self.lon = FlightAxis.LON_0

        self.rcin = [0.0 for _ in range(FlightAxis.RC_CHANNLES)]
        self.m_airspeed_MPS = 0.0
        self.m_altitudeASL_MTR = 0.0
        self.m_altitudeAGL_MTR = 0.0
        self.m_groundspeed_MPS = 0.0
        self.m_pitchRate_DEGpSEC = 0.0
        self.m_rollRate_DEGpSEC = 0.0
        self.m_yawRate_DEGpSEC = 0.0
        self.m_azimuth_DEG = 0.0
        self.m_inclination_DEG = 0.0
        self.m_roll_DEG = 0.0
        self.m_aircraftPositionX_MTR = 0.0
        self.m_aircraftPositionY_MTR = 0.0
        self.m_velocityWorldU_MPS = 0.0
        self.m_velocityWorldV_MPS = 0.0
        self.m_velocityWorldW_MPS = 0.0
        self.m_velocityBodyU_MPS = 0.0
        self.m_velocityBodyV_MPS = 0.0
        self.m_velocityBodyW_MPS = 0.0
        self.m_accelerationWorldAX_MPS2 = 0.0
        self.m_accelerationWorldAY_MPS2 = 0.0
        self.m_accelerationWorldAZ_MPS2 = 0.0
        self.m_accelerationBodyAX_MPS2 = 0.0
        self.m_accelerationBodyAY_MPS2 = 0.0
        self.m_accelerationBodyAZ_MPS2 = 0.0
        self.m_windX_MPS = 0.0
        self.m_windY_MPS = 0.0
        self.m_windZ_MPS = 0.0
        self.m_propRPM = 0.0
        self.m_heliMainRotorRPM = 0.0
        self.m_batteryVoltage_VOLTS = 0.0
        self.m_batteryCurrentDraw_AMPS = 0.0
        self.m_batteryRemainingCapacity_MAH = 0.0
        self.m_fuelRemaining_OZ = 0.0
        self.m_isLocked = 0.0
        self.m_hasLostComponents = 0.0
        self.m_anEngineIsRunning = 0.0
        self.m_isTouchingGround = 0.0
        self.m_currentAircraftStatus = "None"
        self.m_currentPhysicsTime_SEC = 0.0
        self.m_currentPhysicsSpeedMultiplier = 0.0
        self.m_orientationQuaternion_X = 0.0
        self.m_orientationQuaternion_Y = 0.0
        self.m_orientationQuaternion_Z = 0.0
        self.m_orientationQuaternion_W = 0.0
        self.m_flightAxisControllerIsActive = False
        self.m_resetButtonHasBeenPressed = False
        self.rows = []  # Initialize the rows attribute as an empty list

    def enableRC(self, doPrint=False) -> bool:
        """
        Set Spektrum as the RC input
        Return True if the response was OK
        """
        headers = {'content-type': "text/xml;charset='UTF-8'",
                'soapaction': 'RestoreOriginalControllerDevice',
                'Connection': 'Keep-Alive'}

        body = "<?xml version='1.0' encoding='UTF-8'?>\
        <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>\
        <soap:Body>\
        <RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>\
        </soap:Body>\
        </soap:Envelope>"

        response = requests.post(FlightAxis.REALFLIGHT_URL,data=body,headers=headers)
        if doPrint:
            print(response.content)
        return response.ok

    def resetSim(self, doPrint=False) -> bool:
        """
        Reset Real Flight simulator,
        per post here: https://www.knifeedge.com/forums/index.php?threads/realflight-reset-soap-envelope.52333/
        NOTE: untested
        """
        headers = {'content-type': "text/xml;charset='UTF-8'",
                   'soapaction': 'ResetAircraft'}
        body = "<?xml version='1.0' encoding='UTF-8'?>\
        <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>\
        <soap:Body>\
        <ResetAircraft><a>1</a><b>2</b></ResetAircraft>\
        </soap:Body>\
        </soap:Envelope>"

    def disableRC(self, doPrint=False) -> bool:
        """
        Disable Spektrum as the RC input, and use FlightAxis instead
        Return True if the response was OK
        """
        headers = {'content-type': "text/xml;charset='UTF-8'",
                'soapaction': 'InjectUAVControllerInterface'}

        body = "<?xml version='1.0' encoding='UTF-8'?>\
        <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>\
        <soap:Body>\
        <InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>\
        </soap:Body>\
        </soap:Envelope>"

        response = requests.post(FlightAxis.REALFLIGHT_URL, data=body,headers=headers)
        if doPrint:
            print(response.content)
        return response.ok

    def scaleJoystickValue(self, value, invert=False) -> float:
        # Condition channel
        #     # -0.66..0.66 to 0-1
        # Interval scaling from https://stats.stackexchange.com/a/281164
        R_MIN=-0.66
        R_MAX=0.66
        T_MIN=0.0
        T_MAX=1.0

        k = 1.0
        if invert:
            k = -1.0
        scaled_value = (k*value-R_MIN)/(R_MAX-R_MIN)*(T_MAX-T_MIN)+T_MIN
        if scaled_value < 0.0:
            scaled_value = 0.0
        elif scaled_value > 1.0:
            scaled_value = 1.0
        return scaled_value

    def updateActuators(self, controller_values):
        # Pre-calculate inverted values for left joystick
        inverted_left_x = controller_values.get(0, 0.0) * -1 * 0.1  # Roll control (scaled down)
        inverted_left_y = controller_values.get(1, 0.0) * -1        # Throttle control

        # Extract right joystick values
        right_x = controller_values.get(2, 0.0) * 0.1  # Yaw control (scaled down)
        right_y = controller_values.get(3, 0.0) * -1   # Pitch control

        # Calculate the mixed inputs for Roll and Pitch
        mixed_roll = -1 * inverted_left_x + right_y
        mixed_pitch = right_y + inverted_left_x

        #This line defines the neutrual output thrust, at 210 lbs, hover is about .20, higher values decrease it.
        #current thrust range is between 0 (highest thrust) and 1.00 (lowest thrust)
        base_throttle = (inverted_left_y) + .20

        # Adjust the throttle for each motor based on yaw input
        motor1_throttle = base_throttle - right_x
        motor2_throttle = base_throttle + right_x

        # Ensure throttle values are within acceptable range
        motor1_throttle = max(0.0, min(1.0, motor1_throttle))
        motor2_throttle = max(0.0, min(1.0, motor2_throttle))

        # Print the motor throttle values
        #print(f"Motor1 Throttle: {motor1_throttle:.2f}, Motor2 Throttle: {motor2_throttle:.2f}")

        # Iterate over the CHANNELS and update self.rcin
        for channel in FlightAxis.CHANNELS:
            joy_idx, rc_idx, channelInverted = channel
            
            if joy_idx == 2:  # If the channel is for Roll
                joystick_value = mixed_roll
            elif joy_idx == 3:  # If the channel is for Pitch
                joystick_value = mixed_pitch
            elif joy_idx == 0:  # If the channel is for Yaw
                joystick_value = motor1_throttle
            elif joy_idx == 1:  # If the channel is for Throttle
                joystick_value = motor2_throttle
            else:
                joystick_value = controller_values.get(joy_idx, 0.0)
            
            self.rcin[rc_idx] = self.scaleJoystickValue(joystick_value, invert=channelInverted)




    def getStates(self, doPrint=False) -> bool:
        """
        Set the control inputs, and get the states
        Return True if the response was OK
        """
        headers = {'content-type': "text/xml;charset='UTF-8'",
                'soapaction': 'ExchangeData'}

        body = f"<?xml version='1.0' encoding='UTF-8'?><soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>\
        <soap:Body>\
        <ExchangeData>\
        <pControlInputs>\
        <m-selectedChannels>4095</m-selectedChannels>\
        <m-channelValues-0to1>\
        <item>{self.rcin[0]}</item>\
        <item>{self.rcin[1]}</item>\
        <item>{self.rcin[2]}</item>\
        <item>{self.rcin[3]}</item>\
        <item>{self.rcin[4]}</item>\
        <item>{self.rcin[5]}</item>\
        <item>{self.rcin[6]}</item>\
        <item>{self.rcin[7]}</item>\
        <item>{self.rcin[8]}</item>\
        <item>{self.rcin[9]}</item>\
        <item>{self.rcin[10]}</item>\
        <item>{self.rcin[11]}</item>\
        </m-channelValues-0to1>\
        </pControlInputs>\
        </ExchangeData>\
        </soap:Body>\
        </soap:Envelope>"

        response = requests.post(FlightAxis.REALFLIGHT_URL,data=body,headers=headers)

        if doPrint:
            print(response.content)

        if response.ok:
            now = time.time()
            dt = now - self.last_frame_time
            self.avg_dt = (self.avg_dt + dt)/2.0
            self.last_frame_time = now
            self.parseResponse(response.content)
            return True
        # Catch all returns false
        return False

    def parseResponse(self, xml):
        """
        Update internal states from FlightAxis response
        """
        parsed = BeautifulSoup(xml, 'xml')
        body = parsed.find('m-aircraftState')
        assert(body)

        self.row = []
        # Use keytable for rows
        keytable = self.getKeytable()
        for k,v in keytable.items():
            tag = body.find(k)
            tag_val = tag.string
            if tag_val == "false" or tag_val == "true":
                tag_val = tag_val.capitalize()
            cmd = f"{v} = {tag_val}"
            exec(cmd)
            cmd = f"self.row.append({tag_val})"
            exec(cmd)

        # Add actuators
        for idx in range(FlightAxis.RC_CHANNLES):
            self.row.append(self.rcin[idx])

        # Actually append the rows
        self.rows.append(self.row)

        # Update attitude
        self.quats_as_list = [self.m_orientationQuaternion_W,
                              self.m_orientationQuaternion_X,
                              self.m_orientationQuaternion_Y,
                              self.m_orientationQuaternion_Z]

        roll, pitch, yaw = self.quaternion_to_euler()
        # Formatting the output
        # Inside the parseResponse method:
        self.roll, self.pitch, self.yaw = roll, pitch, yaw


        # TODO: is the order of the axis correct?
        self.quats = quaternionic.array(self.quats_as_list)
        #print(f"{self.quats_as_list}")
        # Update position
        ned = [self.m_aircraftPositionX_MTR,
              self.m_aircraftPositionY_MTR,
              self.m_altitudeAGL_MTR]
        self.lat,self.lon, _ = navpy.ned2lla(ned, FlightAxis.LAT_0, FlightAxis.LON_0, FlightAxis.ALT_0,
                                    latlon_unit='deg', alt_unit='m', model='wgs84')

    def getHilActuatorControls(self) -> bool:
        """
        Assign dummy values to actuator controls using the CHANNELS mapping.
        Returns True always as the message reception is simulated.
        """
        # Define and update self.rcin with dummy values
        for idx in range(FlightAxis.RC_CHANNLES):
            self.rcin[idx] = 0.5  # Dummy value for all channels

        # Update self.rcin based on the FlightAxis.CHANNELS mapping
        dummy_joystick_values = {
            # Assuming the format is {joystick_channel_idx: value}
            # These values can be adjusted as per your requirement
            0: 0.5,  # Example value for a joystick channel
            1: 0.5,
            2: 0.5,
            3: 0.5,
            # ... Add other joystick channels if necessary
        }

        for channel in FlightAxis.CHANNELS:
            joy_idx, rc_idx, channelInverted = channel
            joystick_value = dummy_joystick_values.get(joy_idx, 0.0)
            self.rcin[rc_idx] = self.scaleJoystickValue(joystick_value, invert=channelInverted)

        return True

def test_parse1():
    res = b'<?xml version="1.0" encoding="UTF-8"?>\n<SOAP-ENV:Envelope xmlns:SOAP-ENV="http://schemas.xmlsoap.org/soap/envelope/" xmlns:SOAP-ENC="http://schemas.xmlsoap.org/soap/encoding/" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:xsd="http://www.w3.org/2001/XMLSchema"><SOAP-ENV:Body><ReturnData><m-previousInputsState><m-selectedChannels>-1</m-selectedChannels><m-channelValues-0to1 xsi:type="SOAP-ENC:Array" SOAP-ENC:arrayType="xsd:double[12]"><item>1</item><item>0</item><item>0.51503002643585205</item><item>1</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item></m-channelValues-0to1></m-previousInputsState><m-aircraftState><m-currentPhysicsTime-SEC>9.4901527954498306</m-currentPhysicsTime-SEC><m-currentPhysicsSpeedMultiplier>1</m-currentPhysicsSpeedMultiplier><m-airspeed-MPS>4.2668099647568329</m-airspeed-MPS><m-altitudeASL-MTR>0.23033138335698844</m-altitudeASL-MTR><m-altitudeAGL-MTR>0.23033138335698844</m-altitudeAGL-MTR><m-groundspeed-MPS>4.2655322162874292</m-groundspeed-MPS><m-pitchRate-DEGpSEC>46.914449474279536</m-pitchRate-DEGpSEC><m-rollRate-DEGpSEC>-64.392902600666275</m-rollRate-DEGpSEC><m-yawRate-DEGpSEC>-95.093221536022611</m-yawRate-DEGpSEC><m-azimuth-DEG>156.77792358398437</m-azimuth-DEG><m-inclination-DEG>-13.11492919921875</m-inclination-DEG><m-roll-DEG>6.6998014450073242</m-roll-DEG><m-orientationQuaternion-X>0.079808555543422699</m-orientationQuaternion-X><m-orientationQuaternion-Y>0.099987812340259552</m-orientationQuaternion-Y><m-orientationQuaternion-Z>-0.97012227773666382</m-orientationQuaternion-Z><m-orientationQuaternion-W>-0.20614470541477203</m-orientationQuaternion-W><m-aircraftPositionX-MTR>18.125473022460937</m-aircraftPositionX-MTR><m-aircraftPositionY-MTR>13.188897132873535</m-aircraftPositionY-MTR><m-velocityWorldU-MPS>-4.0696659088134766</m-velocityWorldU-MPS><m-velocityWorldV-MPS>1.2777262926101685</m-velocityWorldV-MPS><m-velocityWorldW-MPS>0.10441353917121887</m-velocityWorldW-MPS><m-velocityBodyU-MPS>3.1754355430603027</m-velocityBodyU-MPS><m-velocityBodyV-MPS>-2.8336892127990723</m-velocityBodyV-MPS><m-velocityBodyW-MPS>-0.30408719182014465</m-velocityBodyW-MPS><m-accelerationWorldAX-MPS2>-4.4059576988220215</m-accelerationWorldAX-MPS2><m-accelerationWorldAY-MPS2>-6.3290410041809082</m-accelerationWorldAY-MPS2><m-accelerationWorldAZ-MPS2>8.5558643341064453</m-accelerationWorldAZ-MPS2><m-accelerationBodyAX-MPS2>1.2007522583007813</m-accelerationBodyAX-MPS2><m-accelerationBodyAY-MPS2>8.1494748592376709</m-accelerationBodyAY-MPS2><m-accelerationBodyAZ-MPS2>-8.7651233673095703</m-accelerationBodyAZ-MPS2><m-windX-MPS>0</m-windX-MPS><m-windY-MPS>0</m-windY-MPS><m-windZ-MPS>0</m-windZ-MPS><m-propRPM>10893.9326171875</m-propRPM><m-heliMainRotorRPM>-1</m-heliMainRotorRPM><m-batteryVoltage-VOLTS>-1</m-batteryVoltage-VOLTS><m-batteryCurrentDraw-AMPS>-1</m-batteryCurrentDraw-AMPS><m-batteryRemainingCapacity-MAH>-1</m-batteryRemainingCapacity-MAH><m-fuelRemaining-OZ>11.978337287902832</m-fuelRemaining-OZ><m-isLocked>false</m-isLocked><m-hasLostComponents>false</m-hasLostComponents><m-anEngineIsRunning>true</m-anEngineIsRunning><m-isTouchingGround>true</m-isTouchingGround><m-flightAxisControllerIsActive>true</m-flightAxisControllerIsActive><m-currentAircraftStatus>CAS-FLYING</m-currentAircraftStatus></m-aircraftState><m-notifications><m-resetButtonHasBeenPressed>false</m-resetButtonHasBeenPressed></m-notifications></ReturnData></SOAP-ENV:Body></SOAP-ENV:Envelope>'
    fa = FlightAxis()
    fa.parseResponse(res)

def test_parse2():
    res = b'<?xml version="1.0" encoding="UTF-8"?>\n<SOAP-ENV:Envelope xmlns:SOAP-ENV="http://schemas.xmlsoap.org/soap/envelope/" xmlns:SOAP-ENC="http://schemas.xmlsoap.org/soap/encoding/" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:xsd="http://www.w3.org/2001/XMLSchema"><SOAP-ENV:Body><ReturnData><m-previousInputsState><m-selectedChannels>-1</m-selectedChannels><m-channelValues-0to1 xsi:type="SOAP-ENC:Array" SOAP-ENC:arrayType="xsd:double[12]"><item>1</item><item>0</item><item>0.51503002643585205</item><item>1</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item><item>0</item></m-channelValues-0to1></m-previousInputsState><m-aircraftState><m-currentPhysicsTime-SEC>9.5060089953476563</m-currentPhysicsTime-SEC><m-currentPhysicsSpeedMultiplier>1</m-currentPhysicsSpeedMultiplier><m-airspeed-MPS>3.973699447829139</m-airspeed-MPS><m-altitudeASL-MTR>0.23263369561650649</m-altitudeASL-MTR><m-altitudeAGL-MTR>0.23263369561650649</m-altitudeAGL-MTR><m-groundspeed-MPS>3.9671952662134293</m-groundspeed-MPS><m-pitchRate-DEGpSEC>65.486709524862817</m-pitchRate-DEGpSEC><m-rollRate-DEGpSEC>-113.29177463687665</m-rollRate-DEGpSEC><m-yawRate-DEGpSEC>-115.36654693618766</m-yawRate-DEGpSEC><m-azimuth-DEG>154.92184448242187</m-azimuth-DEG><m-inclination-DEG>-12.265393257141113</m-inclination-DEG><m-roll-DEG>4.7752151489257812</m-roll-DEG><m-orientationQuaternion-X>0.063606671988964081</m-orientationQuaternion-X><m-orientationQuaternion-Y>0.095200046896934509</m-orientationQuaternion-Y><m-orientationQuaternion-Z>-0.96875286102294922</m-orientationQuaternion-Z><m-orientationQuaternion-W>-0.22001981735229492</m-orientationQuaternion-W><m-aircraftPositionX-MTR>18.143898010253906</m-aircraftPositionX-MTR><m-aircraftPositionY-MTR>13.126790046691895</m-aircraftPositionY-MTR><m-velocityWorldU-MPS>-3.8283603191375732</m-velocityWorldU-MPS><m-velocityWorldV-MPS>1.0403343439102173</m-velocityWorldV-MPS><m-velocityWorldW-MPS>-0.22726421058177948</m-velocityWorldW-MPS><m-velocityBodyU-MPS>2.9091477394104004</m-velocityBodyU-MPS><m-velocityBodyV-MPS>-2.6280345916748047</m-velocityBodyV-MPS><m-velocityBodyW-MPS>-0.64850509166717529</m-velocityBodyW-MPS><m-accelerationWorldAX-MPS2>-4.3766188621520996</m-accelerationWorldAX-MPS2><m-accelerationWorldAY-MPS2>-6.6690726280212402</m-accelerationWorldAY-MPS2><m-accelerationWorldAZ-MPS2>9.2979526519775391</m-accelerationWorldAZ-MPS2><m-accelerationBodyAX-MPS2>-0.53152298927307129</m-accelerationBodyAX-MPS2><m-accelerationBodyAY-MPS2>10.025743305683136</m-accelerationBodyAY-MPS2><m-accelerationBodyAZ-MPS2>-8.8302364349365234</m-accelerationBodyAZ-MPS2><m-windX-MPS>0</m-windX-MPS><m-windY-MPS>0</m-windY-MPS><m-windZ-MPS>0</m-windZ-MPS><m-propRPM>10886.970703125</m-propRPM><m-heliMainRotorRPM>-1</m-heliMainRotorRPM><m-batteryVoltage-VOLTS>-1</m-batteryVoltage-VOLTS><m-batteryCurrentDraw-AMPS>-1</m-batteryCurrentDraw-AMPS><m-batteryRemainingCapacity-MAH>-1</m-batteryRemainingCapacity-MAH><m-fuelRemaining-OZ>11.978252410888672</m-fuelRemaining-OZ><m-isLocked>false</m-isLocked><m-hasLostComponents>false</m-hasLostComponents><m-anEngineIsRunning>true</m-anEngineIsRunning><m-isTouchingGround>true</m-isTouchingGround><m-flightAxisControllerIsActive>true</m-flightAxisControllerIsActive><m-currentAircraftStatus>CAS-FLYING</m-currentAircraftStatus></m-aircraftState><m-notifications><m-resetButtonHasBeenPressed>false</m-resetButtonHasBeenPressed></m-notifications></ReturnData></SOAP-ENV:Body></SOAP-ENV:Envelope>'
    fa = FlightAxis()
    fa.parseResponse(res)


# NOTE: try something cleaner, such as using context manager
# and __enter__ and __exit__ dunder methods:
# '''
# with FlightAxisManager() as axis_manager:
#   do stuff
# '''

def signal_handler(sig, frame):
    print('You pressed Ctrl+C!')
    print("RC terminating")
    pygame.quit()
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C')


JOYSTICK_DIRECT = False

if JOYSTICK_DIRECT:
    pygame.init()
    pygame.joystick.init()

    joystick = pygame.joystick.Joystick(0)
    joystick.init()
    print(f"{joystick.get_name()}")

done = False
now = time.time()
fa = FlightAxis()
# 1) Disable RC control and start FlightAxis
fa.enableRC()
fa.disableRC()
# 2) exchange data once to set RC inputs to zeros
fa.getStates()
# 3) reset the simulator to a known initial state
fa.resetSim()

# Main loop
while not done:
    t = time.time()
    dt = t - now
    if dt > 1.0:
        # Optional: Print average frequency stats
        # print(f"avg_f = {1 / fa.avg_dt}[HZ]")
        now = t

    # Read joystick/controller values
    controller_values = read_controller()
    
    
    # Update FlightAxis with the new controller values
    fa.updateActuators(controller_values)
    fa.getStates()
    fa.getHilActuatorControls()

    current_time = time.time()
    if current_time - last_print_time >= print_interval:
        
        
        # Print all values in a single line with two decimal places
        print(f"LS: ({controller_values[0]:.2f}, {controller_values[1]:.2f}), RS: ({controller_values[2]:.2f}, {controller_values[3]:.2f}), Roll: {fa.roll:.2f}, Pitch: {fa.pitch:.2f}, Yaw: {fa.yaw:.2f}")
        last_print_time = current_time
    
    # Check for exit condition (this can be modified based on your requirements)
    if pygame.event.get(pygame.QUIT):
        done = True

print("DONE")

[Tutorial] get orientation angles from RF9

Jack732

New member

Get_RF9_angles_viaPython.py

Ryan Douglas

Administrator

Jack732

New member