def _transmitter_thread(self):
sensor_request_pk = CRTPPacket()
sensor_request_pk.port = CRTPPort.SENSORS
control_input_pk = CRTPPacket()
control_input_pk.port = CRTPPort.OFFBOARDCTRL
vicon_yaw = 0.0
if SE_LISTEN_TO_VICON:
use_vicon_yaw = 1
else:
use_vicon_yaw = 0
imu_lc = lcm.LCM()
while True:
#t0 = time.time()
sensor_request_pk.data = struct.pack('<fi', vicon_yaw,
use_vicon_yaw)
sensor_request_dataout = self._pk_to_dataout(sensor_request_pk)
datain = self._write_read_usb(sensor_request_dataout)
sensor_packet = self._datain_to_pk(datain)
if not sensor_packet:
continue
try:
imu_reading = struct.unpack('<7f', sensor_packet.data)
except:
continue
self._state_estimator.add_imu_reading(imu_reading)
# msg = crazyflie_imu_t()
# msg.omegax = imu_reading[0]
# msg.omegay = imu_reading[1]
# msg.omegaz = imu_reading[2]
# msg.alphax = imu_reading[3]
# msg.alphay = imu_reading[4]
# msg.alphaz = imu_reading[5]
# imu_lc.publish('crazyflie_imu', msg.encode())
self._control_input_updated_flag.clear()
xhat = self._state_estimator.get_xhat()
vicon_yaw = xhat[5]
if self._use_drake_controller:
# wait for Drake to give us the control input
self._control_input_updated_flag.wait(0.005)
control_input = self._controller.get_control_input(xhat=xhat)
if self._use_pos_control:
control_input_pk.data = struct.pack('<7f', *control_input)
else:
control_input_pk.data = struct.pack('<5fi', *control_input)
control_input_dataout = self._pk_to_dataout(control_input_pk)
self._write_usb(control_input_dataout)
if not (self._use_pos_control):
# TODO: position control could still update the state
# estimator about the last input sent
self._state_estimator.add_input(control_input[0:4])
def _transmitter_thread(self):
sensor_request_pk = CRTPPacket()
sensor_request_pk.port = CRTPPort.SENSORS
control_input_pk = CRTPPacket()
control_input_pk.port = CRTPPort.OFFBOARDCTRL
vicon_yaw = 0.0
if SE_LISTEN_TO_VICON:
use_vicon_yaw = 1
else:
use_vicon_yaw = 0
imu_lc = lcm.LCM()
while True:
#t0 = time.time()
sensor_request_pk.data = struct.pack('<fi',vicon_yaw,use_vicon_yaw)
sensor_request_dataout = self._pk_to_dataout(sensor_request_pk)
datain = self._write_read_usb(sensor_request_dataout)
sensor_packet = self._datain_to_pk(datain)
if not sensor_packet:
continue
try:
imu_reading = struct.unpack('<7f',sensor_packet.data)
except:
continue
self._state_estimator.add_imu_reading(imu_reading)
# msg = crazyflie_imu_t()
# msg.omegax = imu_reading[0]
# msg.omegay = imu_reading[1]
# msg.omegaz = imu_reading[2]
# msg.alphax = imu_reading[3]
# msg.alphay = imu_reading[4]
# msg.alphaz = imu_reading[5]
# imu_lc.publish('crazyflie_imu', msg.encode())
self._control_input_updated_flag.clear()
xhat = self._state_estimator.get_xhat()
vicon_yaw = xhat[5]
if self._use_drake_controller:
# wait for Drake to give us the control input
self._control_input_updated_flag.wait(0.005)
control_input = self._controller.get_control_input(xhat=xhat)
if self._use_pos_control:
control_input_pk.data = struct.pack('<7f',*control_input)
else:
control_input_pk.data = struct.pack('<5fi',*control_input)
control_input_dataout = self._pk_to_dataout(control_input_pk)
self._write_usb(control_input_dataout)
if not(self._use_pos_control):
# TODO: position control could still update the state
# estimator about the last input sent
self._state_estimator.add_input(control_input[0:4])
def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate,
pitch_rate, yaw_rate):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2,
cmd3, cmd4, roll_rate, pitch_rate, yaw_rate)
self._cf.send_packet(pk)
def send_setpoint(self,
roll,
pitch,
yaw,
thrust,
hover=False,
set_hover=False,
hover_change=0.0):
#print "roll: ", roll, "; pitch: ", roll, "; yaw: ", yaw, "; thrust: ", thrust
"""
Send a new control setpoint for roll/pitch/yaw/thust to the copter
The arguments roll/pitch/yaw/thrust is the new setpoints that should
be sent to the copter. Hover = True enabled hover mode for as long as it is true
"""
if self._x_mode:
roll = 0.707 * (roll - pitch)
pitch = 0.707 * (roll + pitch)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
if set_hover or not hover:
hover_change = 0.0
pk.data = struct.pack('<fffH??f', roll, -pitch, yaw, thrust, hover,
set_hover, hover_change)
self._cf.send_packet(pk)
def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch,
yaw):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2,
cmd3, cmd4, roll, pitch, yaw)
self._cf.send_packet(pk)
def send_setpoint(self, roll, pitch, yaw, thrust, hover=False, set_hover=False, hover_change = 0.0):
#print "roll: ", roll, "; pitch: ", roll, "; yaw: ", yaw, "; thrust: ", thrust
"""
Send a new control setpoint for roll/pitch/yaw/thust to the copter
The arguments roll/pitch/yaw/thrust is the new setpoints that should
be sent to the copter. Hover = True enabled hover mode for as long as it is true
"""
if self._x_mode:
roll = 0.707*(roll-pitch)
pitch = 0.707*(roll+pitch)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
if set_hover or not hover:
hover_change = 0.0
pk.data = struct.pack('<fffH??f', roll, -pitch, yaw, thrust, hover, set_hover, hover_change)
self._cf.send_packet(pk)
def send_lh_persist_data_packet(self, geo_list, calib_list):
"""
Send geometry and calibration data to persistent memory subsystem
"""
geo_list.sort()
calib_list.sort()
max_bs_nr = 15
if len(geo_list) > 0:
if geo_list[0] < 0 or geo_list[-1] > max_bs_nr:
raise Exception('Geometry BS list is not valid')
if len(calib_list) > 0:
if calib_list[0] < 0 or calib_list[-1] > max_bs_nr:
raise Exception('Calibration BS list is not valid')
mask_geo = 0
mask_calib = 0
for bs in geo_list:
mask_geo += 1 << bs
for bs in calib_list:
mask_calib += 1 << bs
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BHH', self.LH_PERSIST_DATA, mask_geo,
mask_calib)
self._cf.send_packet(pk)
return pk.data
def send_synchronisation(self):
if self.cf.state == State.CONNECTED:
pk = CRTPPacket()
pk.port = CRTPPort.SYNC
t = rospy.Time.now().to_nsec()
pk.data = struct.pack('<Q',t) # timestamp in m
self.cf.send_packet(pk)
def send_synchronisation(self):
if self.cf.state == State.CONNECTED:
pk = CRTPPacket()
pk.port = CRTPPort.SYNC
t = rospy.Time.now().to_nsec()
pk.data = struct.pack('<Q', t) # timestamp in m
self.cf.send_packet(pk)
def send_setpoint(self, roll, pitch, yaw, thrust):
"""
Send a new control setpoint for roll/pitch/yaw/thust to the copter
The arguments roll/pitch/yaw/trust is the new setpoints that should
be sent to the copter
"""
if self.pointerYaw is not None: #elif = else if
# roll = x, pitch = y, yaw = A, A >= 0
# x' = x*cos(A) - y*sin(A)
# y' = x*sin(A) + y*cos(A)
# A < 0
# x' = x*cos(A) + y*sin(A)
# y' = -x*sin(A) + y*cos(A)
currentYaw = self.pointerYaw
self._yaw = math.radians(currentYaw)
cosy = math.cos(self._yaw)
siny = math.sin(self._yaw)
#print "Roll: %3.3f -- Pitch: %3.3f -- Yaw: %3.3f" % (self._actualPoint["stabilizer.roll"], self._actualPoint["stabilizer.pitch"], currentYaw)
#print "Degree Yaw: %3.3f -- Radians Yaw: %3.3f" % (currentYaw, self._yaw)
roll1 = roll
#if self._yaw >= 0:
# roll = roll*cosy - pitch*siny
# pitch = roll1*siny + pitch*cosy
#else:
roll = roll * cosy + pitch * siny
pitch = pitch * cosy - roll1 * siny
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._cf.send_packet(pk)
def _serial_listener(self):
while self.is_connected:
data = self._serial.read(size=29)
if (data):
pk = CRTPPacket()
pk.port = CRTP_PORT_MICROROS
pk.data = data
self.link.send_packet(pk)
def send_stop_setpoint(self):
"""
Send STOP setpoing, stopping the motors and (potentially) falling.
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<B', TYPE_STOP)
self._cf.send_packet(pk)
def _connected(self, link_uri):
print('Connected to %s' % link_uri)
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', 3.1, 3.2, 3.3)
self._cf.send_packet(pk)
def assist_now(self):
"""Callback from assistnow button"""
# print "Hello World"
if (self.helper.cf.link != None):
pk = CRTPPacket()
pk.port = CRTPPort.ABLOCK
pk.data = "<S>\n"
self.helper.cf.send_packet(pk)
def send_bypass_setpoint(self, m1, m2, m3, m4):
"""
Control mode where the motor setpoints are sent directly (in form uint16)
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_BYPASS, m1, m2, m3, m4)
self._cf.send_packet(pk)
def send_stop_setpoint(self):
"""
Send STOP setpoing, stopping the motors and (potentially) falling.
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<B', TYPE_STOP)
self._cf.send_packet(pk)
def send_shoot(self, notused):
if notused == 0:
return
sock.sendto(struct.pack("<LBB", 1, 2, 1), (SOUND_IP, SOUND_PORT))
print("Sending Shoot!")
pk = CRTPPacket()
pk.port = CRTPPort.SHOOT
pk.data = struct.pack('<fffH', 1, 2, 3, 4) #"test"#struct.pack()
self._cf.send_packet(pk)
def send_keep_alive(self):
"""
Keep the drone with previous control command which is saved in the microcontroller
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.channel = self.KEEP_ALIVE_CH
self._cf.send_packet(pk)
def send_message(self, str, hdr):
"""
Send STOP setpoing, stopping the motors and (potentially) falling.
"""
for i in range(len(str) / 29 + 1):
pk = CRTPPacket(hdr, None)
pk.port = CRTPPort.CONSOLE
pk.data = struct.pack('<30s', str[29 * i:(30 * (i + 1)) - (i + 1)])
self._cf.send_packet(pk)
def _serial_listener(self):
while self.running and self.is_connected:
data = self._serial.read_until(size=30)
if (data):
pk = CRTPPacket()
pk.port = self._port
pk.data = data
pk.size = len(data)
self._cf.send_packet(pk)
def send_height(self, vx, vy, yaw, height):
if height > 2.0 or height < 0.0:
raise ValueError('Height must be between 0.0 and 2.0m')
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_POSHOLD
pk.channel = 1
pk.data = struct.pack('<Bffff', TYPE_POSHOLD, vx, vy, yaw, height)
self._cf.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._cf.send_packet(pk)
def send_angular_velocity_setpoint(self, rollrate, pitchrate, yawrate,
thrust):
"""
Angular rate mode controller
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BfffH', 8, rollrate, pitchrate, yawrate,
thrust)
self._cf.send_packet(pk)
def send_extpos(self, x, y, z):
"""
Send the current Crazyflie X, Y, Z position. This is going to be
forwarded to the Crazyflie's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.POSITION
pk.data = struct.pack('<fff', x, y, z)
self._cf.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._cf.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._cf.send_packet(pk)
def send_extpos(self, pos):
"""
Send the current Crazyflie X, Y, Z position. This is going to be
forwarded to the Crazyflie's position estimator.
"""crazyflie
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
self._cf.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 Crazflie).
Roll, pitch, yawrate are defined as rad, rad, rad/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_HOVER, vx, vy, yawrate, zdistance)
self._cf.send_packet(pk)
def send_extpos(self, pos):
"""
Send the current Crazyflie X, Y, Z position. This is going to be
forwarded to the Crazyflie'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._cf.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._cf.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._cf.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 Crazflie).
Roll, pitch, yawrate are defined as rad, rad, rad/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE,
roll, pitch, yawrate, zdistance)
self._cf.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 rad/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD,
vx, vy, vz, yawrate)
self._cf.send_packet(pk)
def send_extpose(self, pos, quat):
"""
Send the current Crazyflie pose (position [x, y, z] and
attitude quaternion [qx, qy, qz, qw]). This is going to be forwarded
to the Crazyflie'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._cf.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 Crazflie).
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._cf.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/s
yaw is in degrees/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_POSITION,
x, y, z, yaw)
self._cf.send_packet(pk)
def _forward_to_cf(ac_id, msg):
try:
data = self._transport.pack_pprz_msg(0,
msg) # sender_id 0 = GCS
for i in range(0, len(data), 30):
pk = CRTPPacket()
pk.port = CRTP_PORT_PPRZLINK
pk.data = data[i:(i + 30)]
self._driver.send_packet(pk)
if self.verbose:
print('Forward message', msg.name)
except:
if self.verbose:
print('Forward error for', ac_id)
def send_setpoint(self, roll, pitch, yaw, thrust):
"""
Send a new control setpoint for roll/pitch/yaw/thust to the copter
The arguments roll/pitch/yaw/trust is the new setpoints that should
be sent to the copter
"""
if self._x_mode:
roll = 0.707 * (roll - pitch)
pitch = 0.707 * (roll + pitch)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._cf.send_packet(pk)
def send_setpoint(self, roll, pitch, yaw, thrust, yaw_Opti):
"""
Send a new control setpoint for roll/pitch/yaw/thust to the copter
The arguments roll/pitch/yaw/trust is the new setpoints that should
be sent to the copter
"""
if self._x_mode:
roll = 0.707 * (roll - pitch)
pitch = 0.707 * (roll + pitch)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffHf', roll, -pitch, yaw, thrust, yaw_Opti)
self._cf.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 crazyflie firmware that prevent
#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 = cflib.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._cf.send_packet(pk)
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._cf.send_packet(pk)
def send_full_state_setpoint(self, r, v, a, rpy, rate):
'''
Sending full state setpoint
'''
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
ri = self.toMilli(r)
vi = self.toMilli(v)
ai = self.toMilli(a)
rpyi = self.toMilli(rpy)
ratei = self.toMilli(rate)
pk.data = struct.pack('<BHHHHHHHHHHHHHHH', TYPE_FULL_STATE, ri[0],
ri[1], ri[2], vi[0], vi[1], vi[2], ai[0], ai[1],
ai[2], rpyi[0], rpyi[1], rpyi[2], ratei[0],
ratei[1], ratei[2])
self._cf.send_packet(pk)
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
id = numpy.uint8(0);
numberpack = numpy.uint16(self.increment_packet_id());
rssi = numpy.uint8(0)
x = numpy.uint16(0)
y = numpy.uint16(0)
z = numpy.uint16(0)
pk.data = struct.pack('<BHBHHHfffH', id, numberpack, rssi, x, y, z, roll, -pitch, yaw, thrust)
self._cf.send_packet(pk)
def outgoing(self, p):
time.sleep(100.0 / 1000.0)
pk = CRTPPacket()
pk.port = CRTPPort.ABLOCK
pk.data = p
self.cf.send_packet(pk)
def send_setpoint(self, roll, pitch, yaw, thrust):
"""
Send a new control setpoint for roll/pitch/yaw/thust to the copter
The arguments roll/pitch/yaw/trust is the new setpoints that should
be sent to the copter
"""
accZ = 0
if self._actualGravity is not None:
accZ = self._actualGravity["acc.zw"]
#if accZ < -0.05:
#thrust = 65000*4/5
if thrust > 65000*4/5:
thrust = 65000*4/5
'''
if roll != 0 or pitch != 0 or yaw != 0 or thrust != 0:
self._oldThrust = 0
if self._actualPoint is not None:
self._oldThrust = self._actualPoint['stabilizer.thrust']
if self._x_mode:
roll = 0.707 * (roll - pitch)
pitch = 0.707 * (roll + pitch)
elif self._carefree_mode and self._actualPoint is not None:#elif = else if
# roll = x, pitch = y, yaw = A, A >= 0
# x' = x*cos(A) - y*sin(A)
# y' = x*sin(A) + y*cos(A)
# A < 0
# x' = x*cos(A) + y*sin(A)
# y' = -x*sin(A) + y*cos(A)
currentYaw = self._actualPoint["stabilizer.yaw"]
self._yaw = math.radians(currentYaw)
cosy = math.cos(self._yaw)
siny = math.sin(self._yaw)
print "Roll: %3.3f -- Pitch: %3.3f -- Yaw: %3.3f" % (self._actualPoint["stabilizer.roll"], self._actualPoint["stabilizer.pitch"], currentYaw)
print "Degree Yaw: %3.3f -- Radians Yaw: %3.3f" % (currentYaw, self._yaw)
roll1 = roll
#if self._yaw >= 0:
# roll = roll*cosy - pitch*siny
# pitch = roll1*siny + pitch*cosy
#else:
roll = roll*cosy + pitch*siny
pitch = pitch*cosy - roll1*siny
elif self._position_mode and self._actualPoint is not None:
roll = 0
pitch = 0
yaw = self._actualPoint['stabilizer.yaw']
#if self._hold_mode and self._actualPoint is not None: #e non premo nessun tasto sul jaystick
#if self._hold_mode and self._oldThrust != 0:
##########################
## STABILIZE THE COPTER ##
##########################
#if roll == 0 and pitch == 0 and self._actualPoint is not None:
# accX = self._actualPoint['acc.x']
# accY = self._actualPoint['acc.y']
# accZ = self._actualPoint['acc.z']
# rollAcc = math.atan2(accX, accZ)*180/math.pi
# pitchAcc = math.atan2(accY, accZ)*180/math.pi
# roll -= self._actualPoint['stabilizer.roll'] + rollAcc
# pitch -= self._actualPoint['stabilizer.pitch'] + pitchAcc
#if self._actualPoint is not None:
#if (roll == 0 and math.fabs(self._actualPoint['stabilizer.roll']) <= self._delta) or (pitch == 0 and math.fabs(self._actualPoint['stabilizer.pitch']) <= self._delta):
# result = self.ComplementaryFilter(roll, pitch)
# roll = result[0]
# pitch = result[1]
#if roll == 0 and math.fabs(self._actualPoint['stabilizer.roll']) <= self._delta:
#roll = math.atan2(self._actualPoint['acc.x'], self._actualPoint['acc.z'])
#-self._actualPoint['stabilizer.roll']/2
#if pitch == 0 and math.fabs(self._actualPoint['stabilizer.pitch']) <= self._delta:
#pitch = math.atan2(self._actualPoint['acc.y'], self._actualPoint['acc.z'])
#-self._actualPoint['stabilizer.pitch']/2
#if yaw == 0 and math.fabs(self._actualPoint['stabilizer.yaw']) <= self._delta:
# yaw = -self._actualPoint['stabilizer.yaw']/2
#print "Roll: %3.3f -- Pitch: %3.3f -- Yaw: %3.3f" % (roll, pitch, yaw)
'''
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._cf.send_packet(pk)
def _send_packet(self, data):
pk = CRTPPacket()
pk.port = CRTPPort.SETPOINT_HL
pk.data = data
self._cf.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._cf.send_packet(pk)
