def core_monitor(self): log_info("starting core state monitor") rc_timeout = 1.0 return_when_signal_lost = False self.mon_data = MonData() last_valid = time() while True: self.core.mon_read(self.mon_data) self.kalman_lat, self.kalman_lon = gps_add_meters( (self.core.params.start_lat, self.core.params.start_lon), self.setpoints[0:2] ) if self.mon_data.signal_valid: last_valid = time() else: if time() - rc_timeout < last_valid and return_when_signal_lost: if not self.icarus_takeover: self.icarus_takeover = True log_err("invalid RC signal, disabling mission interface")
def pilot_monitor(self): log_info('starting pilot state monitor') rc_timeout = 1.0 return_when_signal_lost = False self.mon_data = MonData() last_valid = time() while True: self.pilot.mon_read(self.mon_data) self.kalman_lat, self.kalman_lon = gps_add_meters( (self.pilot.params.start_lat, self.pilot.params.start_lon), self.setpoints[0:2]) if self.mon_data.signal_valid: last_valid = time() else: if time() - rc_timeout < last_valid and return_when_signal_lost: if not self.icarus_takeover: self.icarus_takeover = True log_err( 'invalid RC signal, disabling mission interface')
def run(self): # shortcut identifiers: arg = self.icarus.arg move_data = arg.move_data pilot = self.icarus.pilot params = pilot.params fsm = self.icarus.fsm prev_setp_rel = self.icarus.setpoints start_gps = (params.start_lat, params.start_lon) prev_setp_gps = gps_add_meters(start_gps, prev_setp_rel[0 : 2]) # calculate target x, y, z and move coord = [None, None, None] # x, y, z setpoints if arg.glob: # set global lat, lon postion: glob_sp = [None, None, None] for i in xrange(3): name = 'p%d' % i if move_data.HasField(name): glob_sp[i] = getattr(move_data, name) print 'p0, p1, p2 = ', glob_sp if arg.rel: print 'glob, rel' # interpret lat, lon, alt as relative # covert previous x and y setpoints to rad, using start_lat, start_lon: gps = list(prev_setp_gps) for i in range(0, 2): if glob_sp[i] != None: gps[i] += glob_sp[i] # convert from wsg84 to relative: coord[0 : 2] = gps_meters_offset(start_gps, gps) # add z value: coord[2] = prev_setp_rel[2] if glob_sp[2] != None: coord[2] += glob_sp[2] else: print 'glob, abs' # interpret lat, lon, alt as absolute for i in range(0, 2): if glob_sp[i] == None: glob_sp[i] = prev_setp_gps[i] print start_gps, glob_sp[0 : 2] coord[0 : 2] = gps_meters_offset(start_gps, glob_sp[0 : 2]) if glob_sp[2] != None: coord[2] = glob_sp[2] - params.start_alt else: coord[2] = prev_setp_rel[2] else: # local position update: for i in xrange(3): name = 'p%d' % i if move_data.HasField(name): if arg.rel: print 'local, rel' # relative local coordinate: coord[i] = prev_setp_rel[i] + getattr(move_data, name) else: print 'local, abs' # absolute local coordinate: coord[i] = getattr(move_data, name) else: coord[i] = prev_setp_rel[i] print 'coord output:', coord self.icarus.setpoints = coord # set position pilot.set_ctrl_param(POS_E, coord[0]) pilot.set_ctrl_param(POS_N, coord[1]) """ # did the altitude change?: if coord[2] != prev_setp_rel[2]: # set up linear z interpolation between start and destination points: dist = hypot(prev_setp_rel[0] - coord[0], prev_setp_rel[1] - coord[1]) z_interp = LinearInterpolation(0.0, start_z, dist, coord[2]) # update z setpoint linearly between starting position and destination: target_dist = hypot(pilot.mon[5], pilot.mon[6]) while target_dist > self.LAT_STAB_EPSILON: sleep(1) if self.canceled: pilot.set_ctrl_param(POS_N, pilot.mon[0]) pilot.set_ctrl_param(POS_E, pilot.mon[1]) pilot.set_ctrl_param(POS_U, pilot.mon[2]) self.stabilize() return # not going into hovering state z = z_interp(dist - target_dist) # check elevation map: srtm_z = 1000.0 #_srtm_elev_map.lookup(lat, lon) - params.start_alt if z < srtm_alt + self.SRTM_SAFETY_ALT: z = srtm_alt + self.SRTM_SAFETY_ALT pilot.set_ctrl_param(POS_Z, z) """ self.stabilize() if not self.canceled: fsm.handle('done')
def run(self): # shortcut identifiers: arg = self.icarus.arg move_data = arg.move_data pilot = self.icarus.pilot params = pilot.params fsm = self.icarus.fsm prev_setp_rel = self.icarus.setpoints start_gps = (params.start_lat, params.start_lon) prev_setp_gps = gps_add_meters(start_gps, prev_setp_rel[0:2]) # calculate target x, y, z and move coord = [None, None, None] # x, y, z setpoints if arg.glob: # set global lat, lon postion: glob_sp = [None, None, None] for i in xrange(3): name = 'p%d' % i if move_data.HasField(name): glob_sp[i] = getattr(move_data, name) print 'p0, p1, p2 = ', glob_sp if arg.rel: print 'glob, rel' # interpret lat, lon, alt as relative # covert previous x and y setpoints to rad, using start_lat, start_lon: gps = list(prev_setp_gps) for i in range(0, 2): if glob_sp[i] != None: gps[i] += glob_sp[i] # convert from wsg84 to relative: coord[0:2] = gps_meters_offset(start_gps, gps) # add z value: coord[2] = prev_setp_rel[2] if glob_sp[2] != None: coord[2] += glob_sp[2] else: print 'glob, abs' # interpret lat, lon, alt as absolute for i in range(0, 2): if glob_sp[i] == None: glob_sp[i] = prev_setp_gps[i] print start_gps, glob_sp[0:2] coord[0:2] = gps_meters_offset(start_gps, glob_sp[0:2]) if glob_sp[2] != None: coord[2] = glob_sp[2] - params.start_alt else: coord[2] = prev_setp_rel[2] else: # local position update: for i in xrange(3): name = 'p%d' % i if move_data.HasField(name): if arg.rel: print 'local, rel' # relative local coordinate: coord[i] = prev_setp_rel[i] + getattr(move_data, name) else: print 'local, abs' # absolute local coordinate: coord[i] = getattr(move_data, name) else: coord[i] = prev_setp_rel[i] print 'coord output:', coord self.icarus.setpoints = coord # set position pilot.set_ctrl_param(POS_E, coord[0]) pilot.set_ctrl_param(POS_N, coord[1]) """ # did the altitude change?: if coord[2] != prev_setp_rel[2]: # set up linear z interpolation between start and destination points: dist = hypot(prev_setp_rel[0] - coord[0], prev_setp_rel[1] - coord[1]) z_interp = LinearInterpolation(0.0, start_z, dist, coord[2]) # update z setpoint linearly between starting position and destination: target_dist = hypot(pilot.mon[5], pilot.mon[6]) while target_dist > self.LAT_STAB_EPSILON: sleep(1) if self.canceled: pilot.set_ctrl_param(POS_N, pilot.mon[0]) pilot.set_ctrl_param(POS_E, pilot.mon[1]) pilot.set_ctrl_param(POS_U, pilot.mon[2]) self.stabilize() return # not going into hovering state z = z_interp(dist - target_dist) # check elevation map: srtm_z = 1000.0 #_srtm_elev_map.lookup(lat, lon) - params.start_alt if z < srtm_alt + self.SRTM_SAFETY_ALT: z = srtm_alt + self.SRTM_SAFETY_ALT pilot.set_ctrl_param(POS_Z, z) """ self.stabilize() if not self.canceled: fsm.handle('done')