Example #1
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.pos_node = getNode("/position", True)
        self.home_node = getNode("/task/home", True)
        self.circle_node = getNode("/task/circle", True)
        self.ap_node = getNode("/autopilot", True)
        self.targets_node = getNode("/autopilot/targets", True)
        self.nav_node = getNode("/navigation", True)
        self.tecs_node = getNode("/config/autopilot/TECS", True)

        self.t = 0.0
        self.direction = "left"
        self.radius_m = 100.0
        
        self.name = config_node.getString("name")
        if config_node.hasChild("direction"):
            # only override the default if a value is given
            self.direction = config_node.getString("direction")
        if config_node.hasChild("radius_m"):
            # only override the default if a value is given
            self.radius_m = config_node.getFloat("radius_m")
            
        # estimate the distance of the route (assuming loop_t is set correctly!)
        steps = 1000
        self.step = loop_t / float(steps)
        t1 = 0
        t2 = t1 + self.step
        self.loop_dist = 0.0
        while t2 <= loop_t + 0.5*self.step:
            self.loop_dist += distance_t2t(t1, t2)
            t1 = t2
            t2 += self.step
        print('route distance is approximately:', self.loop_dist)
Example #2
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.pos_node = getNode("/position", True)
        self.orient_node = getNode("/orientation", True)
        self.circle_node = getNode("/task/circle", True)
        self.ap_node = getNode("/autopilot", True)
        self.nav_node = getNode("/navigation", True)
        self.targets_node = getNode("/autopilot/targets", True)

        self.coord_path = ""
        self.direction = "left"
        self.radius_m = 100.0
        self.target_agl_ft = 0.0
        self.target_speed_kt = 0.0
        self.exit_agl_ft = 0.0
        self.exit_heading_deg = 0.0
        
        self.name = config_node.getString("name")
        self.nickname = config_node.getString("nickname")
        self.coord_path = config_node.getString("coord_path")
        if config_node.hasChild("direction"):
            # only override the default if a value is given
            self.direction = config_node.getString("direction")
        if config_node.hasChild("radius_m"):
            # only override the default if a value is given
            self.radius_m = config_node.getFloat("radius_m")
        self.target_agl_ft = config_node.getFloat("altitude_agl_ft")
        self.target_speed_kt = config_node.getFloat("speed_kt")

        # this needs to be done at the end if a coord_path is provided
        if self.coord_path != "":
            self.coord_node = getNode(self.coord_path, True)
        else:
            self.coord_node = None
Example #3
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.route_node = getNode('/task/route', True)
        self.ap_node = getNode('/autopilot', True)
        self.nav_node = getNode("/navigation", True)
        self.targets_node = getNode('/autopilot/targets', True)

        self.alt_agl_ft = 0.0
        self.speed_kt = 30.0

        self.saved_fcs_mode = ''
        self.saved_nav_mode = ''
        self.saved_agl_ft = 0.0
        self.saved_speed_kt = 0.0

        self.name = config_node.getString('name')
        self.nickname = config_node.getString('nickname')
        self.coord_path = config_node.getString('coord_path')
        self.alt_agl_ft = config_node.getFloat('altitude_agl_ft')
        self.speed_kt = config_node.getFloat('speed_kt')

        # load a route if included in config tree
        if control.route.build(config_node):
            control.route.swap()
        else:
            print('Detected an internal inconsistency in the route')
            print(' configuration.  See earlier errors for details.')
            quit()
Example #4
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.task_node = getNode("/task", True)
     self.ap_node = getNode("/autopilot", True)
     self.engine_node = getNode("/controls/engine", True)
     self.name = config_node.getString("name")
     self.nickname = config_node.getString("nickname")
Example #5
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.sensors_node = getNode("/sensors", True)
     self.gps_node = getNode("/sensors/gps", True)
     self.home_node = getNode("/task/home", True)
     self.task_node = getNode("/task", True)
     self.completed = False
     self.name = config_node.getString("name")
Example #6
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.pos_node = getNode("/position", True)
     self.home_node = getNode("/task/home", True)
     self.home_node.setBool("valid", False)
     self.startup_node = getNode("/task/startup", True)
     self.gps_node = getNode("/sensors/gps", True)
     self.name = config_node.getString("name")
Example #7
0
    def __init__(self, config_node):
        Task.__init__(self)

        self.name = config_node.getString("name")

        self.ap_node = getNode("/autopilot", True)
        self.locks_node = getNode("/autopilot/locks", True)

        self.last_master_switch = False
        self.last_fcs_mode = ''
Example #8
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.targets_node = getNode("/autopilot/targets", True)
     self.config_node = config_node
     self.engine_node = getNode("/controls/engine", True)
     self.glide_node = getNode("/task/glide", True)
     self.pos_node = getNode("/position", True)
     self.name = config_node.getString("name")
     self.running = False
     self.last_enable = True  # so we don't start out with a enable event
Example #9
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.imu_node = getNode("/sensors/imu", True)
     self.config_node = config_node
     self.excite_node = getNode("/task/excite", True)
     self.name = config_node.getString("name")
     self.index = 0
     self.start_time = 0.0
     self.type = ''
     self.running = False
     self.last_trigger = True # so we don't start out with a trigger event
Example #10
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.flight_node = getNode("/controls/flight", True)
        self.imu_node = getNode("/sensors/imu", True)

        self.name = config_node.getString("name")
        if config_node.hasChild("speed_secs"):
            self.speed_secs = float(config_node.getString("speed_secs"))
            if self.speed_secs < 1.0:
                self.speed_secs = 1.0
        else:
            self.speed_secs = 5
Example #11
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.status_node = getNode("/status", True)
     self.task_node = getNode("/task", True)
     self.remote_link_node = getNode("/comms/remote_link", True)
     self.remote_link_node.setString("link", "inactive")
     self.link_state = False
     self.push_task = ""
     self.name = config_node.getString("name")
     self.timeout_sec = config_node.getFloat("timeout_sec")
     if self.timeout_sec < 1.0:
         # set a sane default if none provided
         self.timetout_sec = 60.0
     self.action = config_node.getString("action")
Example #12
0
    def __init__(self, config_node):
        Task.__init__(self)

        self.switches = []

        self.name = config_node.getString("name")

        children = config_node.getChildren(expand=True)
        #print "switches task children:", children
        for child_name in children:
            if re.match("switch", child_name):
                switch = Switch(config_node.getChild(child_name))
                #print switch.__dict__
                self.switches.append(switch)
Example #13
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.act_node = getNode("/actuators", True)
        self.gps_node = getNode("/sensors/gps", True)
        self.task_node = getNode("/task", True)

        # initial defaults are locked down (this is kind of a big deal!)
        self.master_safety = True
        self.safety_mode = 'on_ground'
        self.airborne_latch = False
        self.act_node.setBool("throttle_safety", True)

        self.name = config_node.getString("name")
        if config_node.hasChild("safety_mode"):
            self.safety_mode = config_node.getString('safety_mode')
Example #14
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.pos_node = getNode("/position", True)
        self.home_node = getNode("/task/home", True)
        self.circle_node = getNode("/task/circle/active", True)
        self.ap_node = getNode("/autopilot", True)
        self.targets_node = getNode("/autopilot/targets", True)
        self.nav_node = getNode("/navigation", True)
        self.tecs_node = getNode("/config/autopilot/TECS", True)

        self.t = 0.0

        self.name = config_node.getString("name")
        self.function = self.rose
        if config_node.getString("function") == "simple":
            self.function = self.simple_func
        if config_node.getString("function") == "rose":
            self.function = self.rose
        if config_node.getString("function") == "lemniscate":
            self.function = self.lemniscate
        self.radius_m = 200.0
        if config_node.hasChild("radius_m"):
            self.radius_m = config_node.getFloat("radius_m")
        self.vertical_m = 20.0
        if config_node.hasChild("vertical_m"):
            self.vertical_m = config_node.getFloat("vertical_m")
        self.min_kt = self.tecs_node.getFloat("min_kt")
        if config_node.hasChild("min_kt"):
            self.min_kt = config_node.getFloat("min_kt")
        self.max_kt = self.tecs_node.getFloat("max_kt")
        if config_node.hasChild("max_kt"):
            self.max_kt = config_node.getFloat("max_kt")

        # estimate a dt that roughly approximates 1 meter (doesn't
        # have to be perfect)
        loop_t = math.pi * 2
        steps = 1000
        self.step = loop_t / float(steps)
        t1 = 0
        t2 = t1 + self.step
        self.loop_dist = 0.0
        while t2 <= loop_t + 0.5 * self.step:
            self.loop_dist += self.distance_t2t(t1, t2)
            t1 = t2
            t2 += self.step
        print("parametric: 2*pi distance is approximately:", self.loop_dist)
        self.para_dt = 1.0 / self.loop_dist
        print("parametric: dt (approx 1m):", self.para_dt)
Example #15
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.task_node = getNode("/task", True)
        self.preflight_node = getNode("/task/preflight", True)
        self.ap_node = getNode("/autopilot", True)
        self.targets_node = getNode("/autopilot/targets", True)
        self.imu_node = getNode("/sensors/imu", True)
        self.flight_node = getNode("/controls/flight", True)
        #self.saved_fcs_mode = ""
        self.timer = 0.0
        self.duration_sec = 60.0
        self.name = config_node.getString("name")

        # copy to /task/preflight
        if config_node.hasChild("duration_sec"):
            self.duration_sec = config_node.getFloat("duration_sec")
        self.preflight_node.setFloat("duration_sec", self.duration_sec)
Example #16
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.imu_node = getNode("/sensors/imu", True)
     self.chirp_node = getNode("/task/chirp", True)
     self.signal_node = getNode("/controls/signal", True)
     self.name = config_node.getString("name")
     self.nickname = config_node.getString("nickname")
     self.start_time = 0.0
     self.k = 0.0
     # rad/sec is hz*2*pi
     if config_node.hasChild("freq_start_rad_sec"):
         self.freq_start = float(
             config_node.getString("freq_start_rad_sec"))
         if self.freq_start < 0.1: self.freq_start = 0.1
         if self.freq_start > 100.0: self.freq_start = 100.0
     else:
         self.freq_start = 6.2830
     self.chirp_node.setFloat("freq_start_rad_sec", self.freq_start)
     if config_node.hasChild("freq_end_rad_sec"):
         self.freq_end = float(config_node.getString("freq_end_rad_sec"))
         if self.freq_end < 0.1: self.freq_end = 0.1
         if self.freq_end > 100.0: self.freq_end = 100.0
     else:
         self.freq_end = 62.83
     self.chirp_node.setFloat("freq_end_rad_sec", self.freq_end)
     if config_node.hasChild("duration_sec"):
         self.dur_sec = float(config_node.getString("duration_sec"))
         if self.dur_sec < 1: self.dur_sec = 1
         if self.dur_sec > 100: self.dur_sec = 100
     else:
         self.dur_sec = 20
     self.chirp_node.setFloat("duration_sec", self.dur_sec)
     if config_node.hasChild("amplitude"):
         self.amplitude = float(config_node.getString("amplitude"))
         if self.amplitude < 0.001: self.amplitude = 0.001
         if self.amplitude > 1: self.amplitude = 1
     else:
         self.amplitude = 0.1
     self.chirp_node.setFloat("amplitude", self.amplitude)
     if config_node.hasChild("inject"):
         self.inject = config_node.getString("inject")
     else:
         self.inject = "aileron"
     self.signal_node.setString("inject", self.inject)
     self.last_trigger = True  # so we don't start out with a trigger event
     self.running = False
Example #17
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.pos_node = getNode("/position", True)
     self.vel_node = getNode("/velocity", True)
     self.task_node = getNode("/task", True)
     self.status_node = getNode("/status", True)
     self.is_airborne = False
     self.off_alt_agl_ft = 0.0
     self.off_airspeed_kt = 0.0
     self.on_alt_agl_ft = 0.0
     self.on_airspeed_kt = 0.0
     self.name = config_node.getString("name")
     self.off_alt_agl_ft = config_node.getFloat("off_alt_agl_ft")
     self.off_airspeed_kt = config_node.getFloat("off_airspeed_kt")
     self.on_alt_agl_ft = config_node.getFloat("on_alt_agl_ft")
     self.on_airspeed_kt = config_node.getFloat("on_airspeed_kt")
     self.flight_accum = 0.0
     self.flight_start = 0.0
Example #18
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.name = config_node.getString("name")
     self.imu_node = getNode("/sensors/imu", True)
     self.config_imu_node = getNode("/config/drivers/Aura4/imu", True)
     self.task_calib_node = getNode("/task/calibrate", True)
     self.state = 0
     self.armed = False
     self.samples = []
     self.p_filt = LowPass(time_factor=0.1)
     self.q_filt = LowPass(time_factor=0.1)
     self.r_filt = LowPass(time_factor=0.1)
     self.ax_filt = LowPass(time_factor=0.2)
     self.ay_filt = LowPass(time_factor=0.2)
     self.az_filt = LowPass(time_factor=0.2)
     self.F = 1.0  # fitted/output intensity
     self.b = np.zeros([3, 1])
     self.A_1 = np.eye(3)
Example #19
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.pos_node = getNode("/position", True)
        self.orient_node = getNode("/orientation", True)
        self.circle_active_node = getNode("/task/circle/active", True)
        self.circle_standby_node = getNode("/task/circle/standby", True)
        self.nav_node = getNode("/navigation", True)
        self.targets_node = getNode("/autopilot/targets", True)

        self.direction = "left"
        self.radius_m = 100.0

        self.name = config_node.getString("name")
        if config_node.hasChild("direction"):
            # only override the default if a value is given
            self.direction = config_node.getString("direction")
        if config_node.hasChild("radius_m"):
            # only override the default if a value is given
            self.radius_m = config_node.getFloat("radius_m")
Example #20
0
    def __init__(self, config_node):
        Task.__init__(self)

        self.ap_node = getNode("/autopilot", True)
        self.task_node = getNode("/task", True)
        self.pos_node = getNode("/position", True)
        self.vel_node = getNode("/velocity", True)
        self.orient_node = getNode("/orientation", True)
        self.targets_node = getNode("/autopilot/targets", True)
        self.imu_node = getNode("/sensors/imu", True)
        self.flight_node = getNode("/controls/flight", True)
        self.engine_node = getNode("/controls/engine", True)

        self.complete_agl_ft = 150.0
        self.mission_agl_ft = 300.0
        self.target_speed_kt = 25.0
        self.roll_gain = 0.5
        self.roll_limit = 5.0
        self.rudder_enable = False
        self.rudder_gain = 1.0
        self.rudder_max = 1.0
        self.control_limit = 1.0
        self.flaps = 0.0

        self.last_ap_master = False
        self.relhdg = 0.0

        self.name = config_node.getString("name")
        self.nickname = config_node.getString("nickname")
        self.completion_agl_ft = config_node.getFloat("completion_agl_ft")
        self.mission_agl_ft = config_node.getFloat("mission_agl_ft")
        self.target_speed_kt = config_node.getFloat("speed_kt")
        self.roll_gain = config_node.getFloat("roll_gain")
        self.roll_limit = config_node.getFloat("roll_limit")
        self.rudder_enable = config_node.getBool("rudder_enable")
        self.rudder_gain = config_node.getFloat("rudder_gain")
        self.rudder_max = config_node.getFloat("rudder_max")
        self.flaps = config_node.getFloat("flaps")
        self.target_pitch_deg = None
        if config_node.hasChild("target_pitch_deg"):
            self.target_pitch_deg = config_node.getFloat("target_pitch_deg")
Example #21
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.imu_node = getNode("/sensors/imu", True)
     self.pos_node = getNode("/position", True)
     self.orient_node = getNode("/orientation", True)
     self.flight_node = getNode("/controls/flight", True)
     self.task_node = getNode("/task", True)
     self.comms_node = getNode( '/comms', True)
     self.name = config_node.getString("name")
     self.nickname = config_node.getString("nickname")
     self.start_time = 0.0
     self.max_attitude = 20.0
     if config_node.hasChild("trigger"):
         self.trigger_name = config_node.getString("trigger")
     else:
         self.trigger_name = "gear"
     if config_node.hasChild("forward_fov_deg"):
         self.forward_fov_deg = config_node.getFloat("forward_fov_deg")
         if self.forward_fov_deg < 10:  self.forward_fov_deg = 10
         if self.forward_fov_deg > 170: self.forward_fov_deg = 170
     else:
         self.forward_fov_deg = 60
     self.fov2_tan = math.tan(self.forward_fov_deg*0.5 * d2r)
     if config_node.hasChild("lateral_fov_deg"):
         self.lateral_fov_deg = config_node.getFloat("lateral_fov_deg")
         if self.lateral_fov_deg < 10:  self.lateral_fov_deg = 10
         if self.lateral_fov_deg > 170: self.lateral_fov_deg = 170
     else:
         self.lateral_fov_deg = 60
     if config_node.hasChild("overlap"):
         self.overlap = config_node.getFloat("overlap")
         if self.overlap < 0:  self.overlap = 0
         if self.overlap > 1: self.overlap = 1
     else:
         self.overlap = 0.7
     self.min_interval = 0.5
     self.max_interval = 10.0
     self.trigger_state = False
     self.trigger_time = 0.0
     self.last_lat = 0.0
     self.last_lon = 0.0
Example #22
0
 def __init__(self, config_node):
     Task.__init__(self)
     self.name = config_node.getString("name")
     self.imu_node = getNode("/sensors/imu", True)
     self.config_imu_node = getNode("/config/drivers/Aura4/imu", True)
     self.task_node = getNode("/task", True)
     self.state = 0
     self.ax_slow = LowPass(time_factor=1.0) 
     self.ax_fast = LowPass(time_factor=0.2) 
     self.ay_slow = LowPass(time_factor=1.0) 
     self.ay_fast = LowPass(time_factor=0.2) 
     self.az_slow = LowPass(time_factor=1.0) 
     self.az_fast = LowPass(time_factor=0.2)
     self.armed = False
     self.ref = [ [  0,  0, -g ],
                  [  0,  0,  g ],
                  [ -g,  0,  0 ],
                  [  g,  0,  0 ],
                  [  0, -g,  0 ],
                  [  0,  g,  0 ] ]
     self.meas = list(self.ref) # copy
     self.checked = {}
Example #23
0
    def __init__(self, config_node):
        Task.__init__(self)
        self.task_node = getNode("/task", True)
        self.home_node = getNode("/task/home", True)
        self.land_node = getNode("/task/land", True)
        self.circle_node = getNode("/task/circle", True)
        self.route_node = getNode("/task/route", True)
        self.ap_node = getNode("/autopilot", True)
        self.nav_node = getNode("/navigation", True)
        self.pos_node = getNode("/position", True)
        self.vel_node = getNode("/velocity", True)
        self.orient_node = getNode("/orientation", True)
        self.flight_node = getNode("/controls/flight", True)
        self.engine_node = getNode("/controls/engine", True)
        self.imu_node = getNode("/sensors/imu", True)
        self.targets_node = getNode("/autopilot/targets", True)
        self.pilot_node = getNode("/sensors/pilot_input", True)

        # get task configuration parameters
        self.name = config_node.getString("name")
        self.nickname = config_node.getString("nickname")
        self.lateral_offset_m = config_node.getFloat("lateral_offset_m")
        self.glideslope_deg = config_node.getFloat("glideslope_deg")
        if self.glideslope_deg < 0.01:
            self.glideslope_deg = 6.0
        self.turn_radius_m = config_node.getFloat("turn_radius_m")
        if self.turn_radius_m < 1.0:
            self.turn_radius_m = 75.0
        self.direction = config_node.getString("direction")
        if self.direction == "":
            self.direction = "left"
        self.extend_final_leg_m = config_node.getFloat("extend_final_leg_m")
        self.alt_bias_ft = config_node.getFloat("alt_bias_ft")
        self.approach_speed_kt = config_node.getFloat("approach_speed_kt")
        if self.approach_speed_kt < 0.1:
            self.approach_speed_kt = 25.0
        self.flare_pitch_deg = config_node.getFloat("flare_pitch_deg")
        self.flare_seconds = config_node.getFloat("flare_seconds")
        if self.flare_seconds < 0.1:
            self.flare_seconds = 5.0
        if config_node.hasChild("flaps"):
            self.flaps = config_node.getFloat("flaps")
        else:
            self.flaps = 0.0

        # copy to /task/land
        self.land_node.setFloat("lateral_offset_m", self.lateral_offset_m)
        self.land_node.setFloat("glideslope_deg", self.glideslope_deg)
        self.land_node.setFloat("turn_radius_m", self.turn_radius_m)
        self.land_node.setString("direction", self.direction)
        self.land_node.setFloat("extend_final_leg_m", self.extend_final_leg_m)
        self.land_node.setFloat("altitude_bias_ft", self.alt_bias_ft)
        self.land_node.setFloat("approach_speed_kt", self.approach_speed_kt)
        self.land_node.setFloat("flare_pitch_deg", self.flare_pitch_deg)
        self.land_node.setFloat("flare_seconds", self.flare_seconds)

        self.side = -1.0
        self.flare = False
        self.flare_start_time = 0.0
        self.approach_throttle = 0.0
        self.approach_pitch = 0.0
        self.flare_pitch_range = 0.0
        self.final_heading_deg = 0.0
        self.final_leg_m = 0.0
        self.dist_rem_m = 0.0
        self.circle_capture = False
        self.gs_capture = False