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 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()