def send_stop_setpoint(self):
"""
Send STOP setpoint, stopping the motors and (potentially) falling.
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<B', TYPE_STOP)
self._ed.send_packet(pk)
def send_emergency_reset(self):
"""
Send emergency reset
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<B', self.EMERGENCY_RESET)
self._ed.send_packet(pk)
def send_emergency_stop_watchdog(self):
"""
Send emergency stop watchdog
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<B', self.EMERGENCY_STOP_WATCHDOG)
self._ed.send_packet(pk)
def send_short_lpp_packet(self, dest_id, data):
"""
Send ultra-wide-band LPP packet to dest_id
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data
self._ed.send_packet(pk)
def send_extpos(self, pos):
"""
Send the current Espdrone X, Y, Z position. This is going to be
forwarded to the Espdrone's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
self._ed.send_packet(pk)
def send_velocity_world_setpoint(self, vx, vy, vz, yawrate):
"""
Send Velocity in the world frame of reference setpoint.
vx, vy, vz are in m/s
yawrate is in degrees/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD, vx, vy, vz,
yawrate)
self._ed.send_packet(pk)
def send_position_setpoint(self, x, y, z, yaw):
"""
Control mode where the position is sent as absolute x,y,z coordinate in
meter and the yaw is the absolute orientation.
x and y are in m
yaw is in degrees
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_POSITION, x, y, z, yaw)
self._ed.send_packet(pk)
def send_hover_setpoint(self, vx, vy, yawrate, zdistance):
"""
Control mode where the height is send as an absolute setpoint (intended
to be the distance to the surface under the Espdrone).
vx and vy are in m/s
yawrate is in degrees/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_HOVER, vx, vy, yawrate, zdistance)
self._ed.send_packet(pk)
def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance):
"""
Control mode where the height is send as an absolute setpoint (intended
to be the distance to the surface under the Espdrone).
Roll, pitch, yawrate are defined as degrees, degrees, degrees/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE, roll, pitch, yawrate,
zdistance)
self._ed.send_packet(pk)
def send_extpose(self, pos, quat):
"""
Send the current Espdrone pose (position [x, y, z] and
attitude quaternion [qx, qy, qz, qw]). This is going to be forwarded
to the Espdrone's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<Bfffffff', self.EXT_POSE, pos[0], pos[1],
pos[2], quat[0], quat[1], quat[2], quat[3])
self._ed.send_packet(pk)
def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, quat_x,
quat_y, quat_z, quat_w, rate_roll, rate_pitch,
rate_yaw):
"""
Full state control!
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack(
'<Bhhhhhhhhhihhh', TYPE_POSITION, x * 1000, y * 1000, z * 1000,
vx * 1000, vy * 1000, vz * 1000, ax * 1000, ay * 1000, az * 1000,
Localization.quatcompress([quat_x, quat_y, quat_z, quat_w]),
rate_roll * 1000, rate_pitch * 1000, rate_yaw * 1000)
self._ed.send_packet(pk)
def reset_to_bootloader1(self, cpu_id):
""" Reset to the bootloader
The parameter cpuid shall correspond to the device to reset.
Return true if the reset has been done and the contact with the
bootloader is established.
"""
# Send an echo request and wait for the answer
# Mainly aim to bypass a bug of the espdrone firmware that prevents
# reset before normal CRTP communication
pk = CRTPPacket()
pk.port = CRTPPort.LINKCTRL
pk.data = (1, 2, 3) + cpu_id
self.link.send_packet(pk)
pk = None
while True:
pk = self.link.receive_packet(2)
if not pk:
return False
if pk.port == CRTPPort.LINKCTRL:
break
# Send the reset to bootloader request
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (0xFF, 0xFE) + cpu_id
self.link.send_packet(pk)
# Wait to ack the reset ...
pk = None
while True:
pk = self.link.receive_packet(2)
if not pk:
return False
if pk.port == 0xFF and tuple(pk.data) == (0xFF, 0xFE) + cpu_id:
pk.data = (0xFF, 0xF0) + cpu_id
self.link.send_packet(pk)
break
time.sleep(0.1)
self.link.close()
self.link = edlib.crtp.get_link_driver(self.clink_address)
# time.sleep(0.1)
return self._update_info()
def send_setpoint(self, roll, pitch, yaw, thrust):
"""
Send a new control setpoint for roll/pitch/yaw/thrust to the copter
The arguments roll/pitch/yaw/trust is the new setpoints that should
be sent to the copter
"""
if thrust > 0xFFFF or thrust < 0:
raise ValueError('Thrust must be between 0 and 0xFFFF')
if self._x_mode:
roll, pitch = 0.707 * (roll - pitch), 0.707 * (roll + pitch)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._ed.send_packet(pk)
def _send_packet(self, data):
pk = CRTPPacket()
pk.port = CRTPPort.SETPOINT_HL
pk.data = data
self._ed.send_packet(pk)
def _forward(self, data):
pk = CRTPPacket()
pk.port = CRTP_PORT_MAVLINK # CRTPPort.COMMANDER
pk.data = data # struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._ed.send_packet(pk)
