Beispiel #1
0
    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()
Beispiel #2
0
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()
Beispiel #3
0
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)
Beispiel #4
0
 def __init__(self):
     self._telemetry_client = TelemetryClient()
     self.diagnostic_info = ""