async def run():
""" Does Offboard control using position NED coordinates. """
drone = System()
await drone.connect(system_address=aa['address'])
print("-- Arming")
await drone.action.arm()
await drone.action.set_takeoff_altitude(5.0)
await drone.action.set_maximum_speed(0.5)
print("--Taking off")
await drone.action.takeoff()
await asyncio.sleep(8)
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(f"Starting offboard mode failed with error code: {error._result.result}")
print("-- Disarming")
await drone.action.disarm()
return
print("-- Go 0m North, 0m East, -5m Down within local coordinate system")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -5.0, 0.0))
await asyncio.sleep(5)
print("-- Go 5m North, 0m East, -5m Down within local coordinate system, turn to face East")
await drone.offboard.set_position_ned(PositionNedYaw(10.0, 10.0, -5.0, 0.0))
await asyncio.sleep(15)
# print("-- Go 5m North, 5m East, -5m Down within local coordinate system")
# await drone.offboard.set_position_ned(PositionNedYaw(5.0, 5.0, -5.0, 0.0))
# await asyncio.sleep(5)
# print("-- Go 5m North, 5m East, -5m Down within local coordinate system")
# await drone.offboard.set_position_ned(PositionNedYaw(0.0, 5.0, -5.0, 0.0))
# await asyncio.sleep(5)
print("-- Go 0m North, 0m East, -5m Down within local coordinate system")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -5.0, 0.0))
await asyncio.sleep(15)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
await asyncio.sleep(10)
print("--Landing")
await drone.action.land()
await asyncio.sleep(12)
print("--Disarming")
await drone.action.disarm()
except OffboardError as error:
print(f"Stopping offboard mode failed with error code: {error._result.result}")
async def print_flight_mode(yolo, helipad_edge, image_width, image_height):
drone = System()
await drone.connect(system_address=aa['address'])
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
async for flight_mode in drone.telemetry.flight_mode():
print("FlightMode:{}|||||||||".format(flight_mode))
if str(flight_mode) == "HOLD":
await asyncio.sleep(10)
break
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.action.disarm()
return
with picamera.PiCamera() as camera:
camera.resolution = (640, 480)
camera.start_preview()
count = 0
for image in image_generator(camera):
print('image' + str(count) + '.jpg')
image.save('./video/test' + str(count) + '.jpg', 'jpeg')
# image = Image.open('./video/outputframe2.jpg')
count += 1
result = detect_img(yolo, image)
result = calculate_distance(result, helipad_edge, image_width,
image_height)
if result is not None:
print(result)
print(
"-- Go 0m North, 10m East, 0m Down within local coordinate system, turn to face South"
)
await drone.offboard.set_position_ned(
PositionNedYaw(result['actual_delta_x'],
result['actual_delta_y'], 0.0, 0.0))
await asyncio.sleep(10)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
await drone.action.land()
await asyncio.sleep(5)
break
print(result)
async def run():
""" Does Offboard control using position NED coordinates. """
drone = System()
await drone.connect(system_address="udp://:14540")
print("-- Arming")
await drone.action.arm()
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.action.disarm()
return
print("-- Go 0m North, 0m East, -5m Down within local coordinate system")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -5.0, 0.0))
await asyncio.sleep(10)
print(
"-- Go 5m North, 0m East, -5m Down within local coordinate system, turn to face East"
)
await drone.offboard.set_position_ned(PositionNedYaw(5.0, 0.0, -5.0, 90.0))
await asyncio.sleep(10)
print("-- Go 5m North, 10m East, -5m Down within local coordinate system")
await drone.offboard.set_position_ned(PositionNedYaw(
5.0, 10.0, -5.0, 90.0))
await asyncio.sleep(15)
print(
"-- Go 0m North, 10m East, 0m Down within local coordinate system, turn to face South"
)
await drone.offboard.set_position_ned(PositionNedYaw(
0.0, 10.0, 0.0, 180.0))
await asyncio.sleep(10)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
async def reset_async(reset_pos):
unpause_msg = WorldControl()
unpause_msg.pause = False
await asyncio.sleep(0.001)
await pub_world_control.publish(unpause_msg)
print('-- Resetting position')
await drone.offboard.set_position_ned(
PositionNedYaw(reset_pos[0], reset_pos[1], -reset_pos[2],
reset_pos[3]))
while True:
await asyncio.sleep(0.1)
# ob = [lin_pos[2], lin_vel[2]]
# if ( ( np.abs(ob[0] + reset_pos[2]) < 0.5 ) and np.abs( ob[1] < 0.05 ) ):
# await asyncio.sleep(1)
# break
if np.abs(np.linalg.norm(lin_pos - reset_pos[0:3])) < 0.5 and np.abs(
np.linalg.norm(lin_vel)) < 0.05:
await asyncio.sleep(1)
break
print('-- Position reset')
pause_msg = WorldControl()
pause_msg.pause = True
await asyncio.sleep(0.001)
await pub_world_control.publish(pause_msg)
async def run():
global euler_angles
tracking = False # assume that target is not in frame when initializing
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
async for is_armed in drone.telemetry.armed():
print("Is_armed:", is_armed)
if not is_armed:
print("-- Arming")
await drone.action.arm()
break
print("-- Setting initial setpoint")
await drone.offboard.set_velocity_body(
VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(f"Starting offboard mode failed with error code: \
{error._result.result}")
print("-- Disarming")
await drone.action.disarm()
return
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -10.0, 90.0))
async for drone_position in drone.telemetry.odometry():
if (math.fabs(drone_position.position_body.x_m) < 0.2 and
math.fabs(drone_position.position_body.y_m) < 0.2 and
math.fabs(drone_position.position_body.z_m + 10) < 0.2):
break
for i in range(3, 0, -1):
print("Switching to IBVS in " + str(i) + "..")
await asyncio.sleep(1)
while True:
await asyncio.sleep(1 / 30.0)
async for euler_angles in drone.telemetry.attitude_euler():
break
v_xr, v_yr, yawrate, v_zr = getsetpoints()
print("vx: " + str(v_xr) + " vy: " + str(v_yr) + " vz: " + str(v_zr) + " yawrate: " + str(yawrate))
await drone.offboard.set_velocity_body(
# VelocityBodyYawspeed(v_xr, v_yr, v_zr, yawrate)
VelocityBodyYawspeed(v_xr, v_yr, v_zr, yawrate)
)
async def arm_offb(self):
print("-- Arming")
await self.drone.action.arm()
await self.drone.offboard.set_position_ned(
PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await self.drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await self.drone.action.disarm()
async def init_env():
global drone, manager, pub_world_control, home_pos, desired
drone = System()
await drone.connect(system_address="udp://:14550") ## connect to mavsdk
async for state in drone.core.connection_state():
if state.is_connected:
break
await asyncio.sleep(1)
print('-- Connecting to Gazebo')
manager = await pygazebo.connect(
('localhost', 11345)) ## connect to pygazebo
await asyncio.sleep(1)
print('-- Connected')
pub_world_control = await manager.advertise(
'/gazebo/default/world_control', 'gazebo.msgs.WorldControl')
await asyncio.sleep(1)
async for home_pos in drone.telemetry.home(
): ## get absolute home position
home_pos = np.array([
home_pos.latitude_deg, home_pos.longitude_deg,
home_pos.absolute_altitude_m
])
home_pos = np.array(lat_lon_to_coords(home_pos))
break
asyncio.ensure_future(get_lin_pos()) ## initiate linear position stream
asyncio.ensure_future(get_lin_vel()) ## initiate linear velocity stream
async for is_armed in drone.telemetry.armed(): ## check arm status
break
if not is_armed: ## if not armed, arm and change to OFFBOARD mode
print("-- Arming")
await drone.action.arm()
await drone.offboard.set_position_ned(
PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.action.disarm()
async def reset_async(self, reset_pos):
# await pub_world_control.publish(unpause_msg)
reset_steps = 0
print('-- Resetting position')
await self.drone.offboard.set_position_ned(
PositionNedYaw(reset_pos[0], reset_pos[1], -reset_pos[2],
reset_pos[3]))
while True:
async for is_armed in self.drone.telemetry.armed(
): ## check arm status
if not is_armed: ## if not armed, arm and change to OFFBOARD mode
await self.arm_offb()
break
lin_pos = await self.get_lin_pos()
lin_vel = await self.get_lin_vel()
if np.abs(np.linalg.norm(
lin_pos[2] - reset_pos[2])) < 0.7 and np.abs(
np.linalg.norm(lin_vel)
) < 0.2: ### wait for drone to reach desired position
await asyncio.sleep(0.2)
break
if (reset_steps + 1
) % 50 == 0: ### if reset takes too long, reset simulation
print('Reset failed, restarting simulation')
subprocess.Popen(args='make px4_sitl gazebo',
cwd='../Firmware',
shell=True)
reset_steps = 0
await asyncio.sleep(10)
self.loop.run_until_complete(self.init_env())
await asyncio.sleep(0.5)
print('Resetting position: ', reset_steps, '/50')
sys.stdout.write("\033[F")
print('-- Position reset')
# await pub_world_control.publish(pause_msg)
return lin_pos, lin_vel
async def takeoff(self):
await self.drone.connect(system_address="udp://:14540")
drone = System()
print("-- Arming")
await self.drone.action.arm()
print("-- Setting initial setpoint")
await self.drone.offboard.set_position_ned(
PositionNedYaw(0.0, 0.0, 0.0, 0.0))
try:
await self.drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await self.drone.action.disarm()
return
async def run():
""" Does Offboard control using position NED coordinates. """
drone = System()
await drone.connect(system_address=aa['address'])
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print("-- Arming")
await drone.action.arm()
await asyncio.sleep(8)
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0))
await asyncio.sleep(10)
print("-- Starting offboard")
try:
await drone.offboard.start()
pass
# print('offboard')
# async for flight_mode in drone.telemetry.flight_mode():
# print("FlightMode:", flight_mode)
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.action.disarm()
return
print("-- Go 0m North, 0m East, 2m up within local coordinate system")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -2.0, 0.0))
await asyncio.sleep(15)
print(
"-- Go 5m North, 0m East, 2m Down within local coordinate system, turn to face East"
)
await drone.offboard.set_position_ned(PositionNedYaw(5.0, 0.0, -2.0, 90.0))
await asyncio.sleep(15)
print(
"-- Go 0m North, 10m East, 0m Down within local coordinate system, turn to face South"
)
await drone.offboard.set_position_ned(PositionNedYaw(2.0, 0.0, 0.0, 180.0))
await asyncio.sleep(10)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
await asyncio.sleep(5)
await drone.action.land()
await asyncio.sleep(15)
await drone.action.disarm()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
async def square_mission_offboard(mission_alt=5,
mission_spd=50,
mission_north=20,
mission_east=10,
mission_south=-10,
RTL_alt=5,
CAM_pitch=-90,
CAM_yaw=-90):
# Note: offboard uses NED coordinates
drone = await connect_sitl()
await drone.action.arm()
home_lat, home_lon = await get_lat_lon(drone)
# initial setpoint
await drone.offboard.set_position_ned(
PositionNedYaw(north_m=0, east_m=0, down_m=0, yaw_deg=0))
# Starting offboard
try:
await drone.offboard.start()
except OffboardError as error:
print(
f'starting offboard mode failed with error code: {error._result.result}'
)
await drone.action.disarm()
return
# takeoff; -missionalt down
await drone.offboard.set_position_ned(
PositionNedYaw(north_m=0, east_m=0, down_m=-mission_alt, yaw_deg=0))
await drone.gimbal.set_pitch_and_yaw(CAM_pitch, CAM_yaw)
await asyncio.sleep(10)
# go north with same alt
await drone.offboard.set_position_ned(
PositionNedYaw(north_m=mission_north,
east_m=0,
down_m=-mission_alt,
yaw_deg=0))
await asyncio.sleep(10)
# go east from that last north location
await drone.offboard.set_position_ned(
PositionNedYaw(north_m=mission_north,
east_m=mission_east,
down_m=-mission_alt,
yaw_deg=0))
await asyncio.sleep(10)
# go south of that last north east location
await drone.offboard.set_position_ned(
PositionNedYaw(north_m=mission_north + mission_south,
east_m=mission_east,
down_m=-mission_alt,
yaw_deg=0))
await asyncio.sleep(10)
# land
# await drone.mission.set_return_to_launch_after_mission(True) # RTL after last wp
await drone.offboard.set_position_ned(
PositionNedYaw(north_m=0, east_m=0, down_m=0, yaw_deg=0))
await asyncio.sleep(12)
# Stop offboard
try:
await drone.offboard.stop()
except OffboardError as error:
print(
f'stopping offboard mode failed with error code: {error._result.result}'
)
return drone, home_lat, home_lon
async def run():
# Init the drone
drone = System()
await drone.connect(system_address="udp://:14540")
# Set parameters
await drone.param.set_float_param("MIS_TAKEOFF_ALT",
1.0) # set takeoff height to 1 meter
await drone.param.set_int_param("COM_TAKEOFF_ACT", 0) # hold after takeoff
await drone.param.set_int_param(
"COM_OBL_ACT",
0) # 0: land if lost offboard signal; 1: hold if lost offboard signal
# Start parallel tasks
asyncio.ensure_future(print_altitude(drone))
asyncio.ensure_future(print_flight_mode(drone))
termination_task = asyncio.ensure_future(observe_is_in_air(drone))
# Execute the maneuvers
print("-- Arming")
try:
await drone.action.arm()
except Exception as e:
print(e)
#print("-- Taking off")
# await drone.action.takeoff()
# await asyncio.sleep(10) # stay in there for 10 seconds
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.action.disarm()
return
await asyncio.sleep(1)
print("-- Go up 0.5 m")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -0.5, 0.0))
await asyncio.sleep(1)
print(
"-- Go 1m North, 0m East, -0.5m Down (actually 0.5m high from ground) within local coordinate system, turn to face East"
)
await drone.offboard.set_position_ned(PositionNedYaw(1.0, 0.0, -0.5, 0.0))
await asyncio.sleep(1)
print(
"-- Back to start point: Go 0m North, 0m East, -0.5m Down (actually 0.5m high from ground) within local coordinate system, turn to face East"
)
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -0.5, 0.0))
await asyncio.sleep(1)
print(
"-- Go -1m North, 0m East, -0.5m Down (actually 0.5m high from ground) within local coordinate system, turn to face East"
)
await drone.offboard.set_position_ned(PositionNedYaw(-1.0, 0.0, -0.5, 0.0))
await asyncio.sleep(1)
print(
"-- Back to start point: Go 0m North, 0m East, -0.5m Down (actually 0.5m high from ground) within local coordinate system, turn to face East"
)
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -0.5, 0.0))
await asyncio.sleep(1)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
print("-- Landing")
await drone.action.land()
# Wait until the drone is landed (instead of returning after 'land' is sent)
await termination_task
async def print_flight_mode(yolo, helipad_edge, image_width, image_height):
drone = System()
await drone.connect(system_address=aa['address'])
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
# async for flight_mode in drone.telemetry.flight_mode():
# print("FlightMode:{}|||||||||".format(flight_mode))
# if str(flight_mode) == "HOLD":
# await asyncio.sleep(10)
# break
print("-- Arming")
await drone.action.arm()
await asyncio.sleep(4)
# await drone.action.set_takeoff_altitude(5.0)
# print("-- Taking off")
# await drone.action.takeoff()
# await asyncio.sleep(9)
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.action.disarm()
return
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -15.0, 0.0))
await asyncio.sleep(7)
with picamera.PiCamera() as camera:
total_x = 0
total_y = 0
camera.resolution = (640, 480)
camera.start_preview()
count = 0
results = []
for image in image_generator(camera):
print('image' + str(count) + '.jpg')
image.save('./video/test' + str(count) + '.jpg', 'jpeg')
# image = Image.open('./video/outputframe2.jpg')
result = detect_img(yolo, image)
result = calculate_distance(result, helipad_edge, image_width,
image_height)
print(result)
if result is not None:
results.append(result)
total_x += result['actual_delta_x']
total_y += -result['actual_delta_y']
print(result)
count += 1
# print("-- Setting initial setpoint")
# await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -10.0, 0.0))
# await asyncio.sleep(5)
height = 0.0
if count == 1:
height = -10.0
elif count == 2:
height = -5.0
elif count == 3:
height = 0.0
print(
"-- Go {}m North, {}m East, 0m Down within local coordinate system"
.format(-result['actual_delta_y'],
result['actual_delta_x']))
await drone.offboard.set_position_ned(
PositionNedYaw(total_y, total_x, height, 0.0))
await asyncio.sleep(8)
if count == 3:
print(count)
break
print('Saved to results_pos_{}.json'.format(
datetime.now().strftime("%a, %d %b %Y %H:%M:%S")))
with open(
'results_pos_{}.json'.format(
datetime.now().strftime("%a, %d %b %Y %H:%M:%S")),
'w') as f:
json.dump({'results': results}, f)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
await asyncio.sleep(3)
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
# print('--Landing')
# await drone.action.land()
# await asyncio.sleep(10)
print('--Landing')
await drone.action.land()
await asyncio.sleep(5)
await drone.action.disarm()