mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
8afe383737
@ -22,8 +22,8 @@ var update_quad = func( ) {
|
|||||||
# airspeed-kt is actually in feet per second (FDM NET bug)
|
# airspeed-kt is actually in feet per second (FDM NET bug)
|
||||||
setprop("/apm/airspeed", round10(0.3048*getprop("/velocities/airspeed-kt")));
|
setprop("/apm/airspeed", round10(0.3048*getprop("/velocities/airspeed-kt")));
|
||||||
|
|
||||||
setprop("/apm/motor_left", round10(getprop("/engines/engine[0]/rpm")/10.0));
|
setprop("/apm/motor_right", round10(getprop("/engines/engine[0]/rpm")/10.0));
|
||||||
setprop("/apm/motor_right", round10(getprop("/engines/engine[1]/rpm")/10.0));
|
setprop("/apm/motor_left", round10(getprop("/engines/engine[1]/rpm")/10.0));
|
||||||
setprop("/apm/motor_front", round10(getprop("/engines/engine[2]/rpm")/10.0));
|
setprop("/apm/motor_front", round10(getprop("/engines/engine[2]/rpm")/10.0));
|
||||||
setprop("/apm/motor_back", round10(getprop("/engines/engine[3]/rpm")/10.0));
|
setprop("/apm/motor_back", round10(getprop("/engines/engine[3]/rpm")/10.0));
|
||||||
}
|
}
|
||||||
|
@ -267,7 +267,7 @@ def fly_ArduCopter(viewerip=None):
|
|||||||
simquad = pexpect.spawn(simquad_cmd, logfile=sys.stdout, timeout=10)
|
simquad = pexpect.spawn(simquad_cmd, logfile=sys.stdout, timeout=10)
|
||||||
simquad.delaybeforesend = 0
|
simquad.delaybeforesend = 0
|
||||||
util.pexpect_autoclose(simquad)
|
util.pexpect_autoclose(simquad)
|
||||||
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=1'
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
|
||||||
if viewerip:
|
if viewerip:
|
||||||
options += ' --out=%s:14550' % viewerip
|
options += ' --out=%s:14550' % viewerip
|
||||||
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
|
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
|
||||||
@ -308,59 +308,72 @@ def fly_ArduCopter(viewerip=None):
|
|||||||
homeloc = current_location(mav)
|
homeloc = current_location(mav)
|
||||||
|
|
||||||
if not calibrate_level(mavproxy, mav):
|
if not calibrate_level(mavproxy, mav):
|
||||||
|
print("calibrate_level failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
if not arm_motors(mavproxy, mav):
|
if not arm_motors(mavproxy, mav):
|
||||||
|
print("arm_motors failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
if not takeoff(mavproxy, mav):
|
if not takeoff(mavproxy, mav):
|
||||||
|
print("takeoff failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
print("# Fly A square")
|
print("# Fly A square")
|
||||||
if not fly_square(mavproxy, mav):
|
if not fly_square(mavproxy, mav):
|
||||||
|
print("fly_square failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
# save the stored mission
|
# save the stored mission
|
||||||
print("# Save out the C7 mission")
|
print("# Save out the C7 mission")
|
||||||
if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")):
|
if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")):
|
||||||
|
print("save_mission_to_file failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
# Loiter for 10 seconds
|
# Loiter for 10 seconds
|
||||||
print("# Loiter for 10 seconds")
|
print("# Loiter for 10 seconds")
|
||||||
if not loiter(mavproxy, mav):
|
if not loiter(mavproxy, mav):
|
||||||
|
print("loiter failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
#Fly a circle for 60 seconds
|
#Fly a circle for 60 seconds
|
||||||
print("# Fly a Circle")
|
print("# Fly a Circle")
|
||||||
if not circle(mavproxy, mav):
|
if not circle(mavproxy, mav):
|
||||||
|
print("circle failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
# save the stored mission
|
# save the stored mission
|
||||||
print("# Fly CH 7 saved mission")
|
print("# Fly CH 7 saved mission")
|
||||||
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
||||||
|
print("fly_mission failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
print("# Upload mission1")
|
print("# Upload mission1")
|
||||||
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")):
|
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")):
|
||||||
|
print("upload_mission_from_file failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
# this grabs our mission count
|
# this grabs our mission count
|
||||||
print("# store mission1 locally")
|
print("# store mission1 locally")
|
||||||
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")):
|
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")):
|
||||||
|
print("load_mission_from_file failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
print("# Fly mission 2")
|
print("# Fly mission 2")
|
||||||
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
||||||
|
print("fly_mission failed")
|
||||||
failed = True
|
failed = True
|
||||||
else:
|
else:
|
||||||
print("Flew mission2 OK")
|
print("Flew mission2 OK")
|
||||||
|
|
||||||
print("# Land")
|
print("# Land")
|
||||||
if not land(mavproxy, mav):
|
if not land(mavproxy, mav):
|
||||||
|
print("land failed")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
print("# disarm motors")
|
print("# disarm motors")
|
||||||
if not disarm_motors(mavproxy, mav):
|
if not disarm_motors(mavproxy, mav):
|
||||||
|
print("disarm_motors failed")
|
||||||
failed = True
|
failed = True
|
||||||
except pexpect.TIMEOUT, e:
|
except pexpect.TIMEOUT, e:
|
||||||
failed = True
|
failed = True
|
||||||
|
Loading…
Reference in New Issue
Block a user