Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: support antenna trackers with on/off servos in SITL

Browse Source
This commit is contained in:
Andrew Tridgell 2014-07-23 18:40:13 +10:00
parent e9861de2ac
commit 8ea098ad21
3 changed files with 71 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Tools/autotest/pysim/sim_tracker.py
View File

@ -72,7 +72,9 @@ parser.add_option("--simin", dest="simin", help="SIM input (IP:port)",
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("--rate-controlled", action='store_true', default=False, help="Use rate controlled servos")
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()
@ -97,7 +99,7 @@ sim_out.connect(sim_out_address)
sim_out.setblocking(0)
# create the antenna tracker model
a = Tracker(rate_controlled=opts.rate_controlled)
a = Tracker(onoff=opts.onoff, yawrate=opts.yawrate, pitchrate=opts.pitchrate)
# initial controls state
state = ControlState()

73
Tools/autotest/pysim/tracker.py
View File

@ -11,16 +11,18 @@ from rotmat import Vector3
class Tracker(Aircraft):
'''a simple antenna tracker'''
def __init__(self,
rate_controlled=False,
onoff=False,
yawrate=9.0,
pitchrate=1.0,
pitch_range = 45,
yaw_range = 180,
yaw_range = 170,
zero_yaw = 270, # yaw direction at startup
zero_pitch = 10, # pitch at startup
turn_rate=90 # servo max turn rate in degrees/sec
zero_pitch = 10 # pitch at startup
):
Aircraft.__init__(self)
self.rate_controlled = rate_controlled
self.turn_rate = turn_rate
self.onoff = onoff
self.yawrate = yawrate
self.pitchrate = pitchrate
self.last_time = time.time()
self.pitch_range = pitch_range
self.yaw_range = yaw_range
@ -40,14 +42,11 @@ class Tracker(Aircraft):
if target - current < -dv:
return current - dv
return target
def update(self, state):
# how much time has passed?
t = time.time()
delta_time = t - self.last_time
self.last_time = t
def update_position_servos(self, state):
'''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.yaw_current = self.slew_limit(self.yaw_current, state.yaw_input, self.yaw_range, delta_time)
@ -74,10 +73,58 @@ class Tracker(Aircraft):
yaw_rate = min(self.turn_rate, yaw_rate)
yaw_rate = max(-self.turn_rate, 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)
# 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:
if time.time() - self.last_debug > 2 and not self.onoff:
self.last_debug = time.time()
print("roll=%.1f/%.1f pitch=%.1f/%.1f yaw=%.1f/%.1f rates=%.1f/%.1f/%.1f" % (
roll_current, 0,

9
Tools/autotest/sim_vehicle.sh
View File

@ -17,6 +17,7 @@ CLEAN_BUILD=0
START_ANTENNA_TRACKER=0
WIPE_EEPROM=0
REVERSE_THROTTLE=0
TRACKER_ARGS=""
usage()
{
@ -30,6 +31,7 @@ Options:
-G use gdb for debugging ardupilot
-g use gdb for debugging ardupilot, but don't auto-start
-T start an antenna tracker instance
-A pass arguments to antenna tracker
-t set antenna tracker start location
-L select start location from Tools/autotest/locations.txt
-c do a make clean before building
@ -55,7 +57,7 @@ EOF
# parse options. Thanks to http://wiki.bash-hackers.org/howto/getopts_tutorial
while getopts ":I:VgGcj:TL:v:hwf:R" opt; do
while getopts ":I:VgGcj:TA:t:L:v:hwf:R" opt; do
case $opt in
v)
VEHICLE=$OPTARG
@ -69,6 +71,9 @@ while getopts ":I:VgGcj:TL:v:hwf:R" opt; do
T)
START_ANTENNA_TRACKER=1
;;
A)
TRACKER_ARGS="$OPTARG"
;;
R)
REVERSE_THROTTLE=1
;;
@ -226,7 +231,7 @@ if [ $START_ANTENNA_TRACKER == 1 ]; then
TRACKER_UARTA="tcp:127.0.0.1:"$((5760+10*$TRACKER_INSTANCE))
cmd="nice /tmp/AntennaTracker.build/AntennaTracker.elf -I1"
$autotest/run_in_terminal_window.sh "AntennaTracker" $cmd || exit 1
$autotest/run_in_terminal_window.sh "pysim(Tracker)" nice $autotest/pysim/sim_tracker.py --home=$TRACKER_HOME --simin=$TRACKIN_PORT --simout=$TRACKOUT_PORT || exit 1
$autotest/run_in_terminal_window.sh "pysim(Tracker)" nice $autotest/pysim/sim_tracker.py --home=$TRACKER_HOME --simin=$TRACKIN_PORT --simout=$TRACKOUT_PORT $TRACKER_ARGS || exit 1
popd
fi