def OnAttitudeTimer(self, evt):
if not self.renderer:
return
if self.dragging:
return
t = time.time()
dt = t - self.attitude_timer_last
self.attitude_timer_last = t
angle = self.gyro.length()
angle *= dt
self.renderer.rotate(self.gyro, angle)
alpha = 0.8
self.filtered_mag = alpha * self.filtered_mag + (1 - alpha) * self.mag
self.renderer.set_mag(self.filtered_mag)
q = self.renderer.common_model_transform.quaternion
diff = q / self.renderer.last_render_quaternion
angle = quaternion_to_axis_angle(diff).length()
if angle < math.radians(.5):
# Save some CPU time
return
self.Refresh()
def OnAttitudeTimer(self, evt):
if not self.renderer:
return
if self.dragging:
return
t = time.time()
dt = t - self.attitude_timer_last
self.attitude_timer_last = t
angle = self.gyro.length()
angle *= dt
self.renderer.rotate(self.gyro, angle)
alpha = 0.8
self.filtered_mag = alpha * self.filtered_mag + (1 - alpha) * self.mag
self.renderer.set_mag(self.filtered_mag)
q = self.renderer.common_model_transform.quaternion
diff = q / self.renderer.last_render_quaternion
angle = quaternion_to_axis_angle(diff).length()
if angle < math.radians(.5):
# Save some CPU time
return
self.Refresh()
def SetAttitude(self, roll, pitch, yaw, timestamp):
if not self.renderer:
return
dt = 0xFFFFFFFF & (timestamp - self.attitude_timestamp)
dt *= 0.001
self.attitude_timestamp = timestamp
desired_quaternion = quaternion.Quaternion((roll, pitch, yaw))
desired_quaternion.normalize()
error = desired_quaternion / self.renderer.common_model_transform.quaternion
error.normalize()
self.gyro = quaternion_to_axis_angle(error) * (1.0 / dt)
def SetAttitude(self, roll, pitch, yaw, timestamp):
if not self.renderer:
return
dt = 0xFFFFFFFF & (timestamp - self.attitude_timestamp)
dt *= 0.001
self.attitude_timestamp = timestamp
desired_quaternion = quaternion.Quaternion((roll, pitch, yaw))
desired_quaternion.normalize()
error = desired_quaternion / self.renderer.common_model_transform.quaternion
error.normalize()
self.gyro = quaternion_to_axis_angle(error) * (1.0 / dt)
def OnActuationTimer(self, evt):
if not self.renderer.vehicle:
return
dt = evt.GetInterval() * .001
axis, speed = self.angvel
angle = speed * dt
self.renderer.vehicle.model.rotate(axis, angle)
if self.actuation_state == 'attitude':
diff = self.desired_quaternion / self.renderer.vehicle.model.quaternion
diff = quaternion_to_axis_angle(diff)
angle = diff.length()
if abs(angle) <= 0.001:
self.renderer.vehicle.model.quaternion = self.desired_quaternion
self.StopActuation()
if self.attitude_callback:
self.attitude_callback()
self.attitude_callback = None
else:
speed = angle / dt
self.angvel = Vector3(diff.x, diff.y, diff.z), speed * .3
self.Refresh()
def OnActuationTimer(self, evt):
if not self.renderer.vehicle:
return
dt = evt.GetInterval() * .001
axis, speed = self.angvel
angle = speed * dt
self.renderer.vehicle.model.rotate(axis, angle)
if self.actuation_state == 'attitude':
diff = self.desired_quaternion / self.renderer.vehicle.model.quaternion
diff = quaternion_to_axis_angle(diff)
angle = diff.length()
if abs(angle) <= 0.001:
self.renderer.vehicle.model.quaternion = self.desired_quaternion
self.StopActuation()
if self.attitude_callback:
self.attitude_callback()
self.attitude_callback = None
else:
speed = angle / dt
self.angvel = Vector3(diff.x, diff.y, diff.z), speed * .3
self.Refresh()