From f49211b021d826daad715a0b4cc488f1dacf8d7f Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Wed, 5 Apr 2023 17:47:22 +0200 Subject: [PATCH] Tools: remove old jsbsim python scripts (unused anymore) --- Tools/autotest/jsb_sim/__init__.py | 0 Tools/autotest/jsb_sim/runsim.py | 385 ----------------------------- Tools/autotest/pysim/aircraft.py | 112 --------- Tools/autotest/pysim/testwind.py | 20 -- 4 files changed, 517 deletions(-) delete mode 100644 Tools/autotest/jsb_sim/__init__.py delete mode 100755 Tools/autotest/jsb_sim/runsim.py delete mode 100644 Tools/autotest/pysim/aircraft.py delete mode 100755 Tools/autotest/pysim/testwind.py diff --git a/Tools/autotest/jsb_sim/__init__.py b/Tools/autotest/jsb_sim/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/Tools/autotest/jsb_sim/runsim.py b/Tools/autotest/jsb_sim/runsim.py deleted file mode 100755 index 416a4e9769..0000000000 --- a/Tools/autotest/jsb_sim/runsim.py +++ /dev/null @@ -1,385 +0,0 @@ -#!/usr/bin/env python -""" - Run a jsbsim model as a child process. -""" -from __future__ import print_function -import atexit -import errno -import fdpexpect -import math -import os -import select -import signal -import socket -import struct -import sys -import time - -import pexpect -from pymavlink import fgFDM - -from .. pysim import util - - -class control_state(object): - def __init__(self): - self.aileron = 0 - self.elevator = 0 - self.throttle = 0 - self.rudder = 0 - self.ground_height = 0 - -sitl_state = control_state() - - -def interpret_address(addrstr): - """Interpret a IP:port string.""" - a = addrstr.split(':') - a[1] = int(a[1]) - return tuple(a) - - -def jsb_set(variable, value): - """Set a JSBSim variable.""" - global jsb_console - jsb_console.send('set %s %s\r\n' % (variable, value)) - - -def setup_template(home): - """Setup aircraft/Rascal/reset.xml .""" - global opts - v = home.split(',') - if len(v) != 4: - print("home should be lat,lng,alt,hdg - '%s'" % home) - sys.exit(1) - latitude = float(v[0]) - longitude = float(v[1]) - altitude = float(v[2]) - heading = float(v[3]) - sitl_state.ground_height = altitude - template = os.path.join('aircraft', 'Rascal', 'reset_template.xml') - reset = os.path.join('aircraft', 'Rascal', 'reset.xml') - xml = open(template).read() % {'LATITUDE': str(latitude), - 'LONGITUDE': str(longitude), - 'HEADING': str(heading)} - open(reset, mode='w').write(xml) - print("Wrote %s" % reset) - - baseport = int(opts.simout.split(':')[1]) - - template = os.path.join('jsb_sim', 'fgout_template.xml') - out = os.path.join('jsb_sim', 'fgout.xml') - xml = open(template).read() % {'FGOUTPORT': str(baseport+3)} - open(out, mode='w').write(xml) - print("Wrote %s" % out) - - template = os.path.join('jsb_sim', 'rascal_test_template.xml') - out = os.path.join('jsb_sim', 'rascal_test.xml') - xml = open(template).read() % {'JSBCONSOLEPORT': str(baseport+4)} - open(out, mode='w').write(xml) - print("Wrote %s" % out) - - -def process_sitl_input(buf): - """Process control changes from SITL sim.""" - control = list(struct.unpack('<14H', buf)) - pwm = control[:11] - (speed, direction, turbulance) = control[11:] - - global wind - wind.speed = speed*0.01 - wind.direction = direction*0.01 - wind.turbulance = turbulance*0.01 - - aileron = (pwm[0]-1500)/500.0 - elevator = (pwm[1]-1500)/500.0 - throttle = (pwm[2]-1000)/1000.0 - if opts.revthr: - throttle = 1.0 - throttle - rudder = (pwm[3]-1500)/500.0 - - if opts.elevon: - # fake an elevon plane - ch1 = aileron - ch2 = elevator - aileron = (ch2-ch1)/2.0 - # the minus does away with the need for RC2_REVERSED=-1 - elevator = -(ch2+ch1)/2.0 - - if opts.vtail: - # fake an elevon plane - ch1 = elevator - ch2 = rudder - # this matches VTAIL_OUTPUT==2 - elevator = (ch2-ch1)/2.0 - rudder = (ch2+ch1)/2.0 - - buf = '' - if aileron != sitl_state.aileron: - buf += 'set fcs/aileron-cmd-norm %s\n' % aileron - sitl_state.aileron = aileron - if elevator != sitl_state.elevator: - buf += 'set fcs/elevator-cmd-norm %s\n' % elevator - sitl_state.elevator = elevator - if rudder != sitl_state.rudder: - buf += 'set fcs/rudder-cmd-norm %s\n' % rudder - sitl_state.rudder = rudder - if throttle != sitl_state.throttle: - buf += 'set fcs/throttle-cmd-norm %s\n' % throttle - sitl_state.throttle = throttle - buf += 'step\n' - global jsb_console - jsb_console.send(buf) - - -def update_wind(wind): - """Update wind simulation.""" - (speed, direction) = wind.current() - jsb_set('atmosphere/psiw-rad', math.radians(direction)) - jsb_set('atmosphere/wind-mag-fps', speed/0.3048) - - -def process_jsb_input(buf, simtime): - """Process FG FDM input from JSBSim.""" - global fdm, fg_out, sim_out - fdm.parse(buf) - if fg_out: - try: - agl = fdm.get('agl', units='meters') - fdm.set('altitude', agl+sitl_state.ground_height, units='meters') - fdm.set('rpm', sitl_state.throttle*1000) - fg_out.send(fdm.pack()) - except socket.error as e: - if e.errno not in [errno.ECONNREFUSED]: - raise - - timestamp = int(simtime*1.0e6) - - simbuf = struct.pack(' 0.1: - update_wind(wind) - last_wind_update = tnow - - if tnow - last_report > 3: - print("FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f) AR=%.1f" % ( - frame_count / (time.time() - last_report), - fdm.get('altitude', units='meters'), - fdm.get('agl', units='meters'), - fdm.get('phi', units='degrees'), - fdm.get('theta', units='degrees'), - fdm.get('A_X_pilot', units='mpss'), - fdm.get('A_Y_pilot', units='mpss'), - fdm.get('A_Z_pilot', units='mpss'), - achieved_rate)) - - frame_count = 0 - last_report = time.time() - - if new_frame: - now = time.time() - if now < last_wall_time + scaled_frame_time: - time.sleep(last_wall_time+scaled_frame_time - now) - now = time.time() - - if now > last_wall_time and now - last_wall_time < 0.1: - rate = 1.0/(now - last_wall_time) - achieved_rate = (0.98*achieved_rate) + (0.02*rate) - if achieved_rate < opts.rate*opts.speedup: - scaled_frame_time *= 0.999 - else: - scaled_frame_time *= 1.001 - - last_wall_time = now - - -def exit_handler(): - """Exit the sim.""" - print("running exit handler") - signal.signal(signal.SIGINT, signal.SIG_IGN) - signal.signal(signal.SIGTERM, signal.SIG_IGN) - # JSBSim really doesn't like to die ... - if getattr(jsb, 'pid', None) is not None: - os.kill(jsb.pid, signal.SIGKILL) - jsb_console.send('quit\n') - jsb.close(force=True) - util.pexpect_close_all() - sys.exit(1) - -signal.signal(signal.SIGINT, exit_handler) -signal.signal(signal.SIGTERM, exit_handler) - -try: - main_loop() -except Exception as ex: - print(ex) - exit_handler() - raise diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py deleted file mode 100644 index f398164873..0000000000 --- a/Tools/autotest/pysim/aircraft.py +++ /dev/null @@ -1,112 +0,0 @@ -import math -import random -import time -import util - -from pymavlink.rotmat import Vector3, Matrix3 - - -class Aircraft(object): - """A basic aircraft class.""" - def __init__(self): - self.home_latitude = 0 - self.home_longitude = 0 - self.home_altitude = 0 - self.ground_level = 0 - self.frame_height = 0.0 - - self.latitude = self.home_latitude - self.longitude = self.home_longitude - self.altitude = self.home_altitude - - self.dcm = Matrix3() - - # rotation rate in body frame - self.gyro = Vector3(0, 0, 0) # rad/s - - self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down - self.position = Vector3(0, 0, 0) # m North, East, Down - self.mass = 0.0 - self.update_frequency = 50 # in Hz - self.gravity = 9.80665 # m/s/s - self.accelerometer = Vector3(0, 0, -self.gravity) - - self.wind = util.Wind('0,0,0') - self.time_base = time.time() - self.time_now = self.time_base + 100*1.0e-6 - - self.gyro_noise = math.radians(0.1) - self.accel_noise = 0.3 - - def on_ground(self, position=None): - """Return true if we are on the ground.""" - if position is None: - position = self.position - return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height - - def update_position(self): - """Update lat/lon/alt from position.""" - - bearing = math.degrees(math.atan2(self.position.y, self.position.x)) - distance = math.sqrt(self.position.x**2 + self.position.y**2) - - (self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude, - bearing, distance) - - self.altitude = self.home_altitude - self.position.z - - velocity_body = self.dcm.transposed() * self.velocity - - self.accelerometer = self.accel_body.copy() - - def set_yaw_degrees(self, yaw_degrees): - """Rotate to the given yaw.""" - (roll, pitch, yaw) = self.dcm.to_euler() - yaw = math.radians(yaw_degrees) - self.dcm.from_euler(roll, pitch, yaw) - - def time_advance(self, deltat): - """Advance time by deltat in seconds.""" - self.time_now += deltat - - def setup_frame_time(self, rate, speedup): - """Setup frame_time calculation.""" - self.rate = rate - self.speedup = speedup - self.frame_time = 1.0/rate - self.scaled_frame_time = self.frame_time/speedup - self.last_wall_time = time.time() - self.achieved_rate = rate - - def adjust_frame_time(self, rate): - """Adjust frame_time calculation.""" - self.rate = rate - self.frame_time = 1.0/rate - self.scaled_frame_time = self.frame_time/self.speedup - - def sync_frame_time(self): - """Try to synchronise simulation time with wall clock time, taking - into account desired speedup.""" - now = time.time() - if now < self.last_wall_time + self.scaled_frame_time: - time.sleep(self.last_wall_time+self.scaled_frame_time - now) - now = time.time() - - if now > self.last_wall_time and now - self.last_wall_time < 0.1: - rate = 1.0/(now - self.last_wall_time) - self.achieved_rate = (0.98*self.achieved_rate) + (0.02*rate) - if self.achieved_rate < self.rate*self.speedup: - self.scaled_frame_time *= 0.999 - else: - self.scaled_frame_time *= 1.001 - - self.last_wall_time = now - - def add_noise(self, throttle): - """Add noise based on throttle level (from 0..1).""" - self.gyro += Vector3(random.gauss(0, 1), - random.gauss(0, 1), - random.gauss(0, 1)) * throttle * self.gyro_noise - self.accel_body += Vector3(random.gauss(0, 1), - random.gauss(0, 1), - random.gauss(0, 1)) * throttle * self.accel_noise diff --git a/Tools/autotest/pysim/testwind.py b/Tools/autotest/pysim/testwind.py deleted file mode 100755 index c76f68f3fc..0000000000 --- a/Tools/autotest/pysim/testwind.py +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env python -""" -simple test of wind generation code -""" -from __future__ import print_function -import time -import util -from pymavlink.rotmat import Vector3 - -wind = util.Wind('7,90,0.1') - -t0 = time.time() -velocity = Vector3(0, 0, 0) - -t = 0 -deltat = 0.01 - -while t < 60: - print("%.4f %f" % (t, wind.drag(velocity, deltat=deltat).length())) - t += deltat