(connpath)
| 15 | |
| 16 | @with_sitl |
| 17 | def test_goto(connpath): |
| 18 | vehicle = connect(connpath, wait_ready=True) |
| 19 | |
| 20 | # NOTE these are *very inappropriate settings* |
| 21 | # to make on a real vehicle. They are leveraged |
| 22 | # exclusively for simulation. Take heed!!! |
| 23 | vehicle.parameters['FS_GCS_ENABLE'] = 0 |
| 24 | vehicle.parameters['FS_EKF_THRESH'] = 100 |
| 25 | |
| 26 | def arm_and_takeoff(aTargetAltitude): |
| 27 | """ |
| 28 | Arms vehicle and fly to aTargetAltitude. |
| 29 | """ |
| 30 | |
| 31 | # Don't let the user try to fly autopilot is booting |
| 32 | i = 60 |
| 33 | while not vehicle.is_armable and i > 0: |
| 34 | time.sleep(1) |
| 35 | i = i - 1 |
| 36 | assert_equals(vehicle.is_armable, True) |
| 37 | |
| 38 | # Copter should arm in GUIDED mode |
| 39 | vehicle.mode = VehicleMode("GUIDED") |
| 40 | i = 60 |
| 41 | while vehicle.mode.name != 'GUIDED' and i > 0: |
| 42 | # print " Waiting for guided %s seconds..." % (i,) |
| 43 | time.sleep(1) |
| 44 | i = i - 1 |
| 45 | assert_equals(vehicle.mode.name, 'GUIDED') |
| 46 | |
| 47 | # Arm copter. |
| 48 | vehicle.armed = True |
| 49 | i = 60 |
| 50 | while not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0: |
| 51 | # print " Waiting for arming %s seconds..." % (i,) |
| 52 | time.sleep(1) |
| 53 | i = i - 1 |
| 54 | assert_equals(vehicle.armed, True) |
| 55 | |
| 56 | # Take off to target altitude |
| 57 | vehicle.simple_takeoff(aTargetAltitude) |
| 58 | |
| 59 | # Wait until the vehicle reaches a safe height before |
| 60 | # processing the goto (otherwise the command after |
| 61 | # Vehicle.simple_takeoff will execute immediately). |
| 62 | while True: |
| 63 | # print " Altitude: ", vehicle.location.global_relative_frame.alt |
| 64 | # Test for altitude just below target, in case of undershoot. |
| 65 | if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: |
| 66 | # print "Reached target altitude" |
| 67 | break |
| 68 | |
| 69 | assert_equals(vehicle.mode.name, 'GUIDED') |
| 70 | time.sleep(1) |
| 71 | |
| 72 | arm_and_takeoff(10) |
| 73 | |
| 74 | # print "Going to first point..." |
nothing calls this directly
no test coverage detected