def __init__(self, connection_string, baud_rate=115200):
print 'Connecting Clients....'
self.avoid_sys = AvoidanceSystem(self)
self.in_client = IncomingClient(self)
self.out_client = OutgoingClient(self)
self.telem_client = TelemetryClient(self)
self._commands = None
def dronekit_printer(text):
print text
self.out_client.send_messages('d' + text)
print 'Connecting Plane....'
self.vehicle = connect(connection_string,
baud=baud_rate,
rate=25,
status_printer=dronekit_printer,
heartbeat_timeout=300)
print 'Waiting until plane is ready....'
self.vehicle.wait_ready(timeout=300)
print 'Getting commands....'
self.update_commands()
self._bank_angle = deg_to_rad(self.vehicle.parameters['LIM_ROLL_CD'] /
100)
self._wp_radius = self.vehicle.parameters['WP_RADIUS'] / 100
self._prev_wp = 0
self.out_client.start()
self.telem_client.start()
class TelemetryDiagnostics:
DiagnosticChannelConnectionString = "*111#"
def __init__(self):
self._telemetry_client = TelemetryClient()
self.diagnostic_info = ""
def check_transmission(self):
self.diagnostic_info = ""
self._telemetry_client.disconnect()
retryLeft = 3
while ((not self._telemetry_client.online_status) and retryLeft > 0):
self._telemetry_client.connect(TelemetryDiagnostics.DiagnosticChannelConnectionString)
retryLeft -= 1
if not self._telemetry_client.online_status:
raise Exception("Unable to connect.")
self._telemetry_client.send(TelemetryClient.DIAGNOSTIC_MESSAGE)
self.diagnostic_info = self._telemetry_client.receive()
class Plane(object):
def __init__(self, connection_string, baud_rate=115200):
print 'Connecting Clients....'
self.avoid_sys = AvoidanceSystem(self)
self.in_client = IncomingClient(self)
self.out_client = OutgoingClient(self)
self.telem_client = TelemetryClient(self)
self._commands = None
def dronekit_printer(text):
print text
self.out_client.send_messages('d' + text)
print 'Connecting Plane....'
self.vehicle = connect(connection_string,
baud=baud_rate,
rate=25,
status_printer=dronekit_printer,
heartbeat_timeout=300)
print 'Waiting until plane is ready....'
self.vehicle.wait_ready(timeout=300)
print 'Getting commands....'
self.update_commands()
self._bank_angle = deg_to_rad(self.vehicle.parameters['LIM_ROLL_CD'] /
100)
self._wp_radius = self.vehicle.parameters['WP_RADIUS'] / 100
self._prev_wp = 0
self.out_client.start()
self.telem_client.start()
def close(self):
self.avoid_sys.close()
self.out_client.close()
self.telem_client.close()
self.in_client.close()
sleep(1)
self.vehicle.close()
@property
def airspeed(self):
return self.vehicle.airspeed
@property
def heading(self):
return deg_to_rad(self.vehicle.heading)
@property
def pitch(self):
return self.vehicle.attitude.pitch
@property
def loc(self):
return Location.from_dronekit_location(
self.vehicle.location.global_relative_frame)
@property
def turning_radius(self):
return self.airspeed**2 / ACCEL_GRAV / tan(self._bank_angle)
@property
def mode(self):
return self.vehicle.mode.name
@property
def commands(self):
return self._commands
def update_commands(self):
try:
commands = self.vehicle.commands
commands.download()
commands.wait_ready(timeout=60)
self._commands = commands
except:
print 'Failed to download commands... trying again.'
self.update_commands()
@property
def wp_radius(self):
return self._wp_radius
@property
def home_loc(self):
home = self.vehicle.home_location
while not home:
sleep(1)
home = self.vehicle.home_location
return Location.from_dronekit_location(home)
@property
def next_wp_number(self):
next_wp_number = self.commands.next
if not next_wp_number:
next_wp_number = self._prev_wp
else:
self._prev_wp = next_wp_number
if next_wp_number < len(self.commands) and next_wp_number:
next_wp = Location.from_command(self.commands[next_wp_number - 1])
if self.loc.get_distance(next_wp).get_magnitude() \
<= self.wp_radius and not next_wp_number >= \
len(self.commands) + 1:
next_wp_number += 1
return next_wp_number
@property
def next_wp(self):
next_wp_number = self.next_wp_number
try:
return Location.from_command(self.commands[next_wp_number - 1])
except:
return None
def go_loiter(self):
if not self.mode == 'LOITER':
self.vehcile.mode = VehicleMode('LOITER')
def go_auto(self):
if not self.mode == 'AUTO':
self.vehicle.mode = VehicleMode('AUTO')
self.avoid_sys.monitor_count = 0
def goto(self, wp):
self.vehicle.simple_goto(wp.to_dronekit_location())
def turn(self, angle):
d = 2 * self.turning_radius * sin(turn_angle / 2)
bearing_1 = self.heading + turn_angle / 2
bearing_2 = self.heading + turn_angle
dist_1 = Distance.from_magnitude(d, bearing_1)
dist_2 = Distance.from_magnitude(FAR_WP_DIST, bearing_2)
wp_dist = dist_1.add(dist_2)
wp = self.loc.get_location(wp_dist)
self.goto(wp)
self.avoid_sys.monitor_count = 0
def to_static(self):
return PlaneStatic(self)
def __init__(self):
self._telemetry_client = TelemetryClient()
self.diagnostic_info = ""
