Ejemplo n.º 1
0
class Autopilot(object):
    def __init__(self):

        self.waypoints = [(37.614299, -122.357153), (37.602468, -122.398867),
                          (37.630683, -122.415518), (37.638364, -122.385477)]

        self.navigator = Navigator()

    def setup(self, configuration):
        self.updateCnt = 0

        self.target_heading = 100
        self.target_airspeed = 75.0
        self.target_altitude = 1000.0

        self.current_waypoint = 0

        self.altitude_hold_autopilot = AltitudeHoldAutopilot(
            settings.PITCH_P, settings.PITCH_I, settings.PITCH_D,
            settings.ELEVATOR_P, settings.ELEVATOR_I, settings.ELEVATOR_D,
            settings.dt)

        self.airspeed_hold_autopilot = AirspeedHoldAutopilot(
            settings.THROTTLE_P, settings.THROTTLE_I, settings.THROTTLE_D,
            settings.dt)

        self.heading_hold_autopilot = HeadingHoldAutopilot(
            settings.ROLL_P, settings.ROLL_I, settings.ROLL_D,
            settings.AILERON_P, settings.AILERON_I, settings.AILERON_D,
            settings.dt)

        self.altitude_hold_autopilot.set_target_altitude(self.target_altitude)
        self.heading_hold_autopilot.set_target_heading(self.target_heading)
        self.airspeed_hold_autopilot.set_target_airspeed(self.target_airspeed)

    def reset(self, configuration):
        #just call setup again for the moment
        self.setup(configuration)

    def run(self, aircraft):
        instrumentation = aircraft.getInstrumentation()

        current_airspeed = instrumentation.getAirspeed()
        throttle = self.airspeed_hold_autopilot.get_throttle(current_airspeed)

        current_altitude = instrumentation.getAltitude()
        current_pitch = instrumentation.getPitch()
        elevator = self.altitude_hold_autopilot.get_elevator(
            current_altitude, current_pitch)

        if self.updateCnt == 0:
            gps = aircraft.getGPS()

            waypoint = self.waypoints[self.current_waypoint]

            distance_to_point = self.navigator.distance(
                gps.getLatitude(), gps.getLongitude(), waypoint[0],
                waypoint[1])

            target_heading = self.navigator.bearing(gps.getLatitude(),
                                                    gps.getLongitude(),
                                                    waypoint[0], waypoint[1])

            self.heading_hold_autopilot.set_target_heading(target_heading)

            self.updateCnt = 20

            if distance_to_point < 0.2:
                self.current_waypoint += 1
                self.current_waypoint %= len(self.waypoints)
                print("Going to %f, %f" %
                      (self.waypoints[self.current_waypoint][0],
                       self.waypoints[self.current_waypoint][1]))

        current_heading = instrumentation.getHeading()
        current_roll = instrumentation.getRoll()
        aileron = self.heading_hold_autopilot.get_aileron(
            current_heading, current_roll)

        controls = aircraft.getControls()

        controls.setElevator(elevator)
        controls.setAileron(aileron)
        controls.setThrottle(throttle)
        controls.setRudder(0.0)

        self.updateCnt -= 1
Ejemplo n.º 2
0
class Autopilot(object):
    def __init__(self):
        
        self.waypoints = [(37.614299,-122.357153),
                          (37.602468,-122.398867),
                          (37.630683,-122.415518),
                          (37.638364,-122.385477)]
         
        self.navigator = Navigator()
    
    def setup(self, configuration):    
        self.updateCnt = 0
        
        self.target_heading = 100;
        self.target_airspeed = 75.0;
        self.target_altitude = 1000.0
        
        self.current_waypoint = 0
        
        self.altitude_hold_autopilot = AltitudeHoldAutopilot(settings.PITCH_P,
                                                            settings.PITCH_I,
                                                            settings.PITCH_D,
                                                            settings.ELEVATOR_P,
                                                            settings.ELEVATOR_I,
                                                            settings.ELEVATOR_D,
                                                            settings.dt)
        
        self.airspeed_hold_autopilot = AirspeedHoldAutopilot(settings.THROTTLE_P,
                                                            settings.THROTTLE_I,
                                                            settings.THROTTLE_D,
                                                            settings.dt)
        
        self.heading_hold_autopilot = HeadingHoldAutopilot(settings.ROLL_P,
                                                           settings.ROLL_I,
                                                           settings.ROLL_D,
                                                           settings.AILERON_P,
                                                           settings.AILERON_I,
                                                           settings.AILERON_D,
                                                           settings.dt)
        
        self.altitude_hold_autopilot.set_target_altitude(self.target_altitude)
        self.heading_hold_autopilot.set_target_heading(self.target_heading)
        self.airspeed_hold_autopilot.set_target_airspeed(self.target_airspeed)
        
    def reset(self, configuration):
        #just call setup again for the moment
        self.setup(configuration)
    
    def run(self, aircraft):
        instrumentation = aircraft.getInstrumentation()
        
        current_airspeed = instrumentation.getAirspeed()
        throttle = self.airspeed_hold_autopilot.get_throttle(current_airspeed)
        
        current_altitude = instrumentation.getAltitude()
        current_pitch = instrumentation.getPitch()
        elevator = self.altitude_hold_autopilot.get_elevator(current_altitude, current_pitch)
            
        if self.updateCnt == 0:
            gps = aircraft.getGPS()
            
            waypoint = self.waypoints[self.current_waypoint]
            
            distance_to_point = self.navigator.distance(gps.getLatitude(), gps.getLongitude(),
                                                        waypoint[0], waypoint[1])
    
            target_heading = self.navigator.bearing(gps.getLatitude(), gps.getLongitude(),
                                                    waypoint[0], waypoint[1])
    
            self.heading_hold_autopilot.set_target_heading(target_heading)
    
            self.updateCnt = 20
            
            if distance_to_point < 0.2:
                self.current_waypoint += 1
                self.current_waypoint %= len(self.waypoints)
                print("Going to %f, %f" % (self.waypoints[self.current_waypoint][0], self.waypoints[self.current_waypoint][1]))
    
        current_heading = instrumentation.getHeading()
        current_roll = instrumentation.getRoll()
        aileron = self.heading_hold_autopilot.get_aileron(current_heading, current_roll)
        
        controls = aircraft.getControls()
        
        controls.setElevator(elevator)
        controls.setAileron(aileron)
        controls.setThrottle(throttle)
        controls.setRudder(0.0)
        
        self.updateCnt -= 1