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)