mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 08:38:36 -04:00
autotest: removed old python antennatracker simulator
This commit is contained in:
Andrew Tridgell
parent
96737a3982
commit
f4c61acbc2
@ -1,143 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
'''
|
||||
simple antenna tracker simulator
|
||||
'''
|
||||
|
||||
from tracker import Tracker
|
||||
import util, time, os, sys, math
|
||||
import socket, struct
|
||||
import select, errno
|
||||
|
||||
def sim_send(a):
|
||||
'''send flight information to mavproxy'''
|
||||
from math import degrees
|
||||
|
||||
(roll, pitch, yaw) = a.dcm.to_euler()
|
||||
|
||||
buf = struct.pack('<17dI',
|
||||
a.latitude, a.longitude, a.altitude, degrees(yaw),
|
||||
a.velocity.x, a.velocity.y, a.velocity.z,
|
||||
a.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
|
||||
degrees(a.gyro.x), degrees(a.gyro.y), degrees(a.gyro.z),
|
||||
degrees(roll), degrees(pitch), degrees(yaw),
|
||||
math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y),
|
||||
0x4c56414f)
|
||||
try:
|
||||
sim_out.send(buf)
|
||||
except socket.error as e:
|
||||
if not e.errno in [ errno.ECONNREFUSED ]:
|
||||
raise
|
||||
|
||||
|
||||
def sim_recv(state):
|
||||
'''receive control information from SITL'''
|
||||
try:
|
||||
buf = sim_in.recv(28)
|
||||
except socket.error as e:
|
||||
if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
|
||||
raise
|
||||
return
|
||||
|
||||
if len(buf) != 28:
|
||||
print('len=%u' % len(buf))
|
||||
return
|
||||
control = list(struct.unpack('<14H', buf))
|
||||
pwm = control[0:11]
|
||||
|
||||
# map yaw and pitch control to -1/1
|
||||
state.yaw_input = (pwm[0]-1500)/500.0
|
||||
state.pitch_input = (pwm[1]-1500)/500.0
|
||||
|
||||
|
||||
|
||||
def interpret_address(addrstr):
|
||||
'''interpret a IP:port string'''
|
||||
a = addrstr.split(':')
|
||||
a[1] = int(a[1])
|
||||
return tuple(a)
|
||||
|
||||
class ControlState:
|
||||
def __init__(self):
|
||||
# yaw control from -1 to 1, where -1 is full left, 1 is full right
|
||||
self.yaw_input = 0
|
||||
# pitch control from -1 to 1, where -1 is full down, 1 is full up
|
||||
self.pitch_input = 0
|
||||
|
||||
##################
|
||||
# main program
|
||||
from optparse import OptionParser
|
||||
parser = OptionParser("sim_tracker.py [options]")
|
||||
parser.add_option("--simin", dest="simin", help="SIM input (IP:port)", default="127.0.0.1:5502")
|
||||
parser.add_option("--simout", dest="simout", help="SIM output (IP:port)", default="127.0.0.1:5501")
|
||||
parser.add_option("--home", dest="home", type='string', default=None, help="home lat,lng,alt,hdg (required)")
|
||||
parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", default=100)
|
||||
parser.add_option("--onoff", action='store_true', default=False, help="Use onoff servo system")
|
||||
parser.add_option("--yawrate", type='float', default=9.0, help="yaw rate of servos (degrees/s)")
|
||||
parser.add_option("--pitchrate", type='float', default=1.0, help="pitch rate of servos (degrees/s)")
|
||||
|
||||
(opts, args) = parser.parse_args()
|
||||
|
||||
for m in [ 'home' ]:
|
||||
if not opts.__dict__[m]:
|
||||
print("Missing required option '%s'" % m)
|
||||
parser.print_help()
|
||||
sys.exit(1)
|
||||
|
||||
# UDP socket addresses
|
||||
sim_out_address = interpret_address(opts.simout)
|
||||
sim_in_address = interpret_address(opts.simin)
|
||||
|
||||
# setup input from SITL
|
||||
sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sim_in.bind(sim_in_address)
|
||||
sim_in.setblocking(0)
|
||||
|
||||
# setup output to SITL
|
||||
sim_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sim_out.connect(sim_out_address)
|
||||
sim_out.setblocking(0)
|
||||
|
||||
# create the antenna tracker model
|
||||
a = Tracker(onoff=opts.onoff, yawrate=opts.yawrate, pitchrate=opts.pitchrate)
|
||||
|
||||
# initial controls state
|
||||
state = ControlState()
|
||||
|
||||
|
||||
# parse home
|
||||
v = opts.home.split(',')
|
||||
if len(v) != 4:
|
||||
print("home should be lat,lng,alt,hdg")
|
||||
sys.exit(1)
|
||||
a.home_latitude = float(v[0])
|
||||
a.home_longitude = float(v[1])
|
||||
a.home_altitude = float(v[2])
|
||||
a.altitude = a.home_altitude
|
||||
a.yaw = float(v[3])
|
||||
a.latitude = a.home_latitude
|
||||
a.longitude = a.home_longitude
|
||||
|
||||
a.set_yaw_degrees(a.yaw)
|
||||
|
||||
print("Starting at lat=%f lon=%f alt=%f heading=%.1f" % (
|
||||
a.home_latitude,
|
||||
a.home_longitude,
|
||||
a.altitude,
|
||||
a.yaw))
|
||||
|
||||
frame_time = 1.0/opts.rate
|
||||
sleep_overhead = 0
|
||||
|
||||
while True:
|
||||
frame_start = time.time()
|
||||
sim_recv(state)
|
||||
a.update(state)
|
||||
sim_send(a)
|
||||
t = time.time()
|
||||
frame_end = time.time()
|
||||
if frame_end - frame_start < frame_time:
|
||||
dt = frame_time - (frame_end - frame_start)
|
||||
dt -= sleep_overhead
|
||||
if dt > 0:
|
||||
time.sleep(dt)
|
||||
sleep_overhead = 0.99*sleep_overhead + 0.01*(time.time() - frame_end)
|
||||
@ -1,146 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
'''
|
||||
simple antenna tracker simulator core
|
||||
'''
|
||||
|
||||
from aircraft import Aircraft
|
||||
import util, time, math
|
||||
from math import degrees, radians
|
||||
from rotmat import Vector3
|
||||
|
||||
class Tracker(Aircraft):
|
||||
'''a simple antenna tracker'''
|
||||
def __init__(self,
|
||||
onoff=False,
|
||||
yawrate=9.0,
|
||||
pitchrate=1.0,
|
||||
pitch_range = 45,
|
||||
yaw_range = 170,
|
||||
zero_yaw = 270, # yaw direction at startup
|
||||
zero_pitch = 10 # pitch at startup
|
||||
):
|
||||
Aircraft.__init__(self)
|
||||
self.onoff = onoff
|
||||
self.yawrate = yawrate
|
||||
self.pitchrate = pitchrate
|
||||
self.last_time = time.time()
|
||||
self.pitch_range = pitch_range
|
||||
self.yaw_range = yaw_range
|
||||
self.zero_yaw = zero_yaw
|
||||
self.zero_pitch = zero_pitch
|
||||
self.verbose = False
|
||||
self.last_debug = time.time()
|
||||
self.pitch_current = 0
|
||||
self.yaw_current = 0
|
||||
|
||||
def slew_limit(self, current, target, range, delta_time, turn_rate):
|
||||
'''limit speed of servo movement'''
|
||||
dangle = turn_rate * delta_time
|
||||
dv = dangle / range
|
||||
if target - current > dv:
|
||||
return current + dv
|
||||
if target - current < -dv:
|
||||
return current - dv
|
||||
return target
|
||||
|
||||
|
||||
def update_position_servos(self, state, delta_time):
|
||||
'''update function for position (normal) servos.
|
||||
Returns (yaw_rate,pitch_rate) tuple'''
|
||||
self.pitch_current = self.slew_limit(self.pitch_current, state.pitch_input, self.pitch_range, delta_time, self.yawrate)
|
||||
self.yaw_current = self.slew_limit(self.yaw_current, state.yaw_input, self.yaw_range, delta_time, self.pitchrate)
|
||||
|
||||
pitch_target = self.zero_pitch + self.pitch_current*self.pitch_range
|
||||
yaw_target = self.zero_yaw + self.yaw_current*self.yaw_range
|
||||
while yaw_target > 180:
|
||||
yaw_target -= 360
|
||||
|
||||
(r,p,y) = self.dcm.to_euler()
|
||||
pitch_current = degrees(p)
|
||||
yaw_current = degrees(y)
|
||||
roll_current = degrees(r)
|
||||
|
||||
pitch_rate = pitch_target - pitch_current
|
||||
pitch_rate = min(self.pitchrate, pitch_rate)
|
||||
pitch_rate = max(-self.pitchrate, pitch_rate)
|
||||
|
||||
yaw_diff = yaw_target - yaw_current
|
||||
if yaw_diff > 180:
|
||||
yaw_diff -= 360
|
||||
if yaw_diff < -180:
|
||||
yaw_diff += 360
|
||||
yaw_rate = yaw_diff
|
||||
yaw_rate = min(self.yawrate, yaw_rate)
|
||||
yaw_rate = max(-self.yawrate, yaw_rate)
|
||||
|
||||
return (yaw_rate, pitch_rate)
|
||||
|
||||
def update_onoff_servos(self, state):
|
||||
'''update function for onoff servos.
|
||||
These servos either move at a constant rate or are still
|
||||
Returns (yaw_rate,pitch_rate) tuple'''
|
||||
if abs(state.yaw_input) < 0.1:
|
||||
yaw_rate = 0
|
||||
elif state.yaw_input >= 0.1:
|
||||
yaw_rate = self.yawrate
|
||||
else:
|
||||
yaw_rate = -self.yawrate
|
||||
|
||||
if abs(state.pitch_input) < 0.1:
|
||||
pitch_rate = 0
|
||||
elif state.pitch_input >= 0.1:
|
||||
pitch_rate = self.pitchrate
|
||||
else:
|
||||
pitch_rate = -self.pitchrate
|
||||
return (yaw_rate, pitch_rate)
|
||||
|
||||
def update(self, state):
|
||||
# how much time has passed?
|
||||
t = time.time()
|
||||
delta_time = t - self.last_time
|
||||
self.last_time = t
|
||||
|
||||
if self.onoff:
|
||||
(yaw_rate,pitch_rate) = self.update_onoff_servos(state)
|
||||
else:
|
||||
(yaw_rate,pitch_rate) = self.update_position_servos(state, delta_time)
|
||||
|
||||
# implement yaw and pitch limits
|
||||
(r,p,y) = self.dcm.to_euler()
|
||||
pitch_current = degrees(p)
|
||||
yaw_current = degrees(y)
|
||||
roll_current = degrees(r)
|
||||
|
||||
if yaw_rate > 0 and yaw_current >= self.yaw_range:
|
||||
yaw_rate = 0
|
||||
if yaw_rate < 0 and yaw_current <= -self.yaw_range:
|
||||
yaw_rate = 0
|
||||
|
||||
if pitch_rate > 0 and pitch_current >= self.pitch_range:
|
||||
pitch_rate = 0
|
||||
if pitch_rate < 0 and pitch_current <= -self.pitch_range:
|
||||
pitch_rate = 0
|
||||
|
||||
# keep it level
|
||||
roll_rate = 0 - roll_current
|
||||
|
||||
if time.time() - self.last_debug > 2 and not self.onoff:
|
||||
self.last_debug = time.time()
|
||||
print("roll=%.1f/%.1f pitch=%.1f yaw=%.1f rates=%.1f/%.1f/%.1f" % (
|
||||
roll_current, 0,
|
||||
pitch_current,
|
||||
yaw_current,
|
||||
roll_rate, pitch_rate, yaw_rate))
|
||||
|
||||
self.gyro = Vector3(radians(roll_rate),radians(pitch_rate),radians(yaw_rate))
|
||||
|
||||
# update attitude
|
||||
self.dcm.rotate(self.gyro * delta_time)
|
||||
self.dcm.normalize()
|
||||
|
||||
accel_earth = Vector3(0, 0, -self.gravity)
|
||||
self.accel_body = self.dcm.transposed() * accel_earth
|
||||
|
||||
# new velocity vector
|
||||
self.velocity = Vector3()
|
||||
self.update_position()
|
||||
Loading…
Reference in New Issue
Block a user