def printOrientationAcceleration(self):
orientation = EulerAngles()
acceleration = Acceleration()
self.pozyx.getEulerAngles_deg(orientation, self.remote_id)
self.pozyx.getAcceleration_mg(acceleration, self.remote_id)
print("Orientation: %s, acceleration: %s" %
(str(orientation), str(acceleration)))
def GetEulerAngles(self, tagId):
result = EulerAngles()
status = self.pozyx.getEulerAngles_deg(result, tagId)
self.CheckStatus(status)
res = dict()
res['heading'] = result.heading
res['roll'] = result.roll
res['pitch'] = result.pitch
return res
def getOrientation(self):
# reads euler angles (yaw, roll, pitch) and exports the results
orientation = EulerAngles()
status = self.serial.getEulerAngles_deg(orientation)
if status == POZYX_SUCCESS:
# print("POZYX data:", orientation)
return orientation
else:
print("Sensor data not found")
return None
def loop(self):
"""Gets new IMU sensor data"""
angles = EulerAngles()
coordinates = Coordinates()
if self.remote_id is not None or self.pozyx.checkForFlag(
POZYX_INT_MASK_IMU, 0.01) == POZYX_SUCCESS:
status = self.pozyx.getEulerAngles_deg(angles, self.remote_id)
status &= self.pozyx.doPositioning(coordinates,
self.dimension,
self.height,
self.algorithm,
remote_id=self.remote_id)
if status == POZYX_SUCCESS:
self.publishSensorData(angles, coordinates)
def loop(self):
"""Performs positioning and prints the results."""
for i, tag in enumerate(self.tags):
position = Coordinates()
angles = EulerAngles()
status = self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm, remote_id=tag)
if status == POZYX_SUCCESS:
#BUFFERx.pop(0)
#BUFFERx.append(int(position.x))
#BUFFERy.pop(0)
#BUFFERy.append(int(position.y))
#tempy = AverageMinMax(BUFFERy)
# tempx = AverageMinMax(BUFFERx)
# BUFFERx[1] = BUFFERx[0]
# BUFFERx[0] = position.x
# BUFFERy[1] = BUFFERy[0]
# BUFFERy[0] = position.y
# tempx = Exponentialfilter(BUFFERx)
# tempy = Exponentialfilter(BUFFERy)
theta= -2*pi/3
position.x = cos(theta)*position.x - sin(theta)*position.y
position.y = sin(theta)*position.x + cos(theta)*position.y
uwb_pos = Twist()
uwb_pos.linear.x = int(position.x) / 1000.
uwb_pos.linear.y = int(position.y) / 1000.
uwb_pos.linear.z = int(position.z) / 1000.
self.printPublishPosition(position, tag)
self.uwb_target_pub.publish(uwb_pos)
return False
else:
self.printPublishErrorCode("positioning", tag)
return True
def loop(self):
"""Performs positioning and prints the results."""
for tag_id in self.tag_ids:
position = Coordinates()
orientation = EulerAngles()
acceleration = Acceleration()
try:
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=tag_id)
status &= self.pozyx.getEulerAngles_deg(orientation, tag_id)
status &= self.pozyx.getAcceleration_mg(acceleration, tag_id)
if status == POZYX_SUCCESS:
#self.printPublishPosition(position, tag_id)
self.printPublishPositionWithOrientation(
position, orientation, acceleration, tag_id)
else:
self.printPublishErrorCode("positioning", tag_id)
except struct.error as err:
print(err)
#import pypozyx
import sys, time
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, POZYX_SUCCESS, SingleRegister, EulerAngles, Acceleration, UWBSettings
#pozyx = PozyxLib() # PozyxSerial has PozyxLib's functions, just for generality
CURSOR_UP_ONE = '\x1b[1A'
ERASE_LINE = '\x1b[2K'
is_cursor_up = False
#print(pypozyx.get_first_pozyx_serial_port())
pozyx = PozyxSerial(get_first_pozyx_serial_port())
who_am_i = SingleRegister()
# get the data, passing along the container
status = pozyx.getWhoAmI(who_am_i)
acceleration = Acceleration()
euler_angles = EulerAngles()
uwb_settings = UWBSettings()
# check the status to see if the read was successful. Handling failure is covered later.
if status == POZYX_SUCCESS:
# print the container. Note how a SingleRegister will print as a hex string by default.
print('Who Am I: {}'.format(who_am_i)) # will print '0x43'
while True:
# initalize the Pozyx as above
# initialize the data container
# and repeat
# initialize the data container
# get the data, passing along the container
if is_cursor_up:
sys.stdout.write(CURSOR_UP_ONE)
def getHeading(self):
orientation = EulerAngles()
self.pozyx.getEulerAngles_deg(orientation, self.remote_id)
return orientation.heading
def mockedOrientation(cls):
return EulerAngles(random.randint(0, 30), random.randint(0, 30), random.randint(0, 30))
def __init__(self):
self.pozyx = PozyxSerial(get_first_pozyx_serial_port())
self.direct = EulerAngles()
self.position = Coordinates()