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()