diff --git a/Tools/autotest/pysim/sim_tracker.py b/Tools/autotest/pysim/sim_tracker.py index 4d1ce82d67..0c952eb08b 100755 --- a/Tools/autotest/pysim/sim_tracker.py +++ b/Tools/autotest/pysim/sim_tracker.py @@ -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() diff --git a/Tools/autotest/pysim/tracker.py b/Tools/autotest/pysim/tracker.py index 7fc92278ee..99d7b8c2cd 100644 --- a/Tools/autotest/pysim/tracker.py +++ b/Tools/autotest/pysim/tracker.py @@ -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, diff --git a/Tools/autotest/sim_vehicle.sh b/Tools/autotest/sim_vehicle.sh index 8e2d9cc45b..59bbea6faf 100755 --- a/Tools/autotest/sim_vehicle.sh +++ b/Tools/autotest/sim_vehicle.sh @@ -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