def getCurrentPosition(self):
position = Coordinates()
totalPosition = Coordinates()
measurement = 0
numberOfMeasurements = 20
while (measurement < numberOfMeasurements):
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=self.remote_id)
if status == POZYX_SUCCESS:
totalPosition.x = totalPosition.x + position.x
totalPosition.y = totalPosition.y + position.y
totalPosition.z = totalPosition.z + position.z
measurement = measurement + 1
else:
"""self.printPublishErrorCode("positioning")"""
totalPosition.x = totalPosition.x / numberOfMeasurements
totalPosition.y = totalPosition.y / numberOfMeasurements
totalPosition.z = totalPosition.z / numberOfMeasurements
return totalPosition
def get_tag_velocity(self, position_now, time_now):
delt_pos = Coordinates()
#unit: mm
delt_pos.x = position_now.x - self.pos_last.x
delt_pos.y = position_now.y - self.pos_last.y
delt_pos.z = position_now.z - self.pos_last.z
delt_time = (time_now - self.pos_last_time) * 1000.0 #s-->ms
self.pos_last = position_now
self.pos_last_time = time_now
self.velocity.x = delt_pos.x / delt_time
#m/s
self.velocity.y = delt_pos.y / delt_time
self.velocity.z = delt_pos.z / delt_time
if self.debug == 2:
print("Tag velocity is X=" + str(self.velocity.x) + "m/s; Y=" +
str(self.velocity.y) + "m/s Z=" + str(self.velocity.z) +
"m/s")
def getCurrentPosition(self):
position = Coordinates()
totalPosition = Coordinates()
status = 0
while not status:
status = self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm, remote_id=self.remote_id)
self.totalPositionX = self.totalPositionX - self.measurementsX.popleft() + position.x
self.totalPositionY = self.totalPositionY - self.measurementsY.popleft() + position.y
self.totalPositionZ = self.totalPositionZ - self.measurementsZ.popleft() + position.z
self.measurementsX.append(position.x)
self.measurementsY.append(position.y)
self.measurementsZ.append(position.z)
totalPosition.x = self.totalPositionX / self.numberOfMeasurements
totalPosition.y = self.totalPositionY / self.numberOfMeasurements
totalPosition.z = self.totalPositionZ / self.numberOfMeasurements
return totalPosition
def convert_to_ned(self, vector):
ned_vector = Coordinates()
ned_vector.x = vector.x * self.cos_yaw - vector.y * self.sin_yaw
ned_vector.y = vector.x * self.sin_yaw + vector.y * self.cos_yaw
ned_vector.z = vector.z
return ned_vector