def create_packet(self, data, cls=None, heartbeat=False): """ Create a packet in JSON format from some data Args: data: dict with data that should come in the packet """ now = time.time() localtime = time.localtime(now) milliseconds = '%03d' % int((now - int(now)) * 1000) timestamp = time.strftime('%d%m%Y%H%M%S', localtime) + milliseconds if heartbeat: data.update({ 'message_type': 'status', 'timestamp': timestamp, 'heartbeat': True }) else: data.update({ 'message_type': 'status', 'timestamp': timestamp, 'heartbeat': False }) return json.dumps(data, cls=cls)
def point(self, degrees, relative=True): """ This won't be used in this project, but might prove useful for future uses """ self.solo_lock.acquire() if self.fence_breach: raise StandardError("You are outside of the fence") if self.vehicle.mode != 'GUIDED': self.logger.error("DroneDirectError: 'point({0})' was not executed. \ Vehicle was not in GUIDED mode".format(degrees)) self.solo_lock.release() return # limit our update rate if (time.time() - self.last_send_point) < 1.0 / self.update_rate: self.solo_lock.release() return if relative: is_relative = 1 # yaw relative to direction of travel else: is_relative = 0 # yaw is an absolute angle if degrees != 0: direction = int(degrees / abs(degrees)) else: direction = 1 degrees = degrees % 360 # create the CONDITION_YAW command using command_long_encode() msg = self.vehicle.message_factory.command_long_encode( 0, 0, # target system, target component mavlink.MAV_CMD_CONDITION_YAW, # command 0, # confirmation degrees, # param 1, yaw in degrees 0, # param 2, yaw speed deg/s direction, # param 3, direction -1 ccw, 1 cw is_relative, # param 4, relative offset 1, absolute angle 0 0, 0, 0) # param 5 ~ 7 not used # send command to vehicle self.vehicle.send_mavlink(msg) self.vehicle.flush() self.last_send_point = time.time() self.solo_lock.release()
def set_attitude(roll_angle=0.0, pitch_angle=0.0, yaw_angle=None, yaw_rate=0.0, use_yaw_rate=False, thrust=0.5, duration=0): """ Note that from AC3.3 the message should be re-sent more often than every second, as an ATTITUDE_TARGET order has a timeout of 1s. In AC3.2.1 and earlier the specified attitude persists until it is canceled. The code below should work on either version. Sending the message multiple times is the recommended way. """ send_attitude_target(roll_angle, pitch_angle, yaw_angle, yaw_rate, False, thrust) start = time.time() while time.time() - start < duration: send_attitude_target(roll_angle, pitch_angle, yaw_angle, yaw_rate, False, thrust) time.sleep(0.1) # Reset attitude, or it will persist for 1s more due to the timeout send_attitude_target(0, 0, 0, 0, True, thrust)
def translate(self, x=0, y=0, z=0, wait_for_arrival=False): """ This won't be used in this project, but might prove useful for future uses """ self.solo_lock.acquire() if self.fence_breach: self.solo_lock.release() raise StandardError("You are outside of the fence") if self.vehicle.mode != 'GUIDED': self.logger.error("DroneDirectError: 'translate({0},{1},{2})' was not executed. \ Vehicle was not in GUIDED mode".format(x, y, z)) self.solo_lock.release() return # limit our update rate if (time.time() - self.last_send_translate) < 1.0 / self.update_rate: self.solo_lock.release() return yaw = self.vehicle.attitude.yaw # radians location = self.vehicle.location.global_relative_frame # latlon # rotate to earth-frame angles x_ef = y * math.cos(yaw) - x * math.sin(yaw) y_ef = y * math.sin(yaw) + x * math.cos(yaw) latlon_to_m = 1.113195e5 # converts lat/lon to meters lat = x_ef / latlon_to_m + location.lat lon = y_ef / latlon_to_m + location.lon alt = z + location.alt msg = self.vehicle.message_factory.set_position_target_global_int_encode( 0, # time_boot_ms (not used) 0, 0, # target system, target component mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame 0b0000111111111000, # type_mask (only speeds enabled) lat * 1e7, # lat_int - X Position in WGS84 frame in 1e7 * meters lon * 1e7, # lon_int - Y Position in WGS84 frame in 1e7 * meters alt, # alt - Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, # above terrain if GLOBAL_TERRAIN_ALT_INT 0, # X velocity in NED frame in m/s 0, # Y velocity in NED frame in m/s 0, # Z velocity in NED frame in m/s 0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink) 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) # send command to vehicle self.vehicle.send_mavlink(msg) self.vehicle.flush() self.last_send_translate = time.time() self.point(0) self.logger.info("translating...") if wait_for_arrival: while self.vehicle.mode == "GUIDED": veh_loc = self.vehicle.location.global_relative_frame diff_lat_m = (lat - veh_loc.lat) * latlon_to_m diff_lon_m = (lon - veh_loc.lon) * latlon_to_m diff_alt_m = alt - veh_loc.alt dist_xyz = math.sqrt(diff_lat_m**2 + diff_lon_m**2 + diff_alt_m**2) if dist_xyz < self.distance_threshold: self.logger.info("Arrived") self.solo_lock.release() return self.logger.error("DroneDirectError: 'translate({0},{1},{2})' was interrupted. \ Vehicle was switched out of GUIDED mode".format(x, y, z)) self.solo_lock.release()