How to use the pymavlink.quaternion.Quaternion function in pymavlink

To help you get started, we’ve selected a few pymavlink examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # construct q from a quaternion
        q_test = Quaternion(q)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(q)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(q)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # # construct q from a euler angles
        q_test = Quaternion(euler)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(euler)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(euler)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # # construct q from dcm (Matrix3 instance)
        q_test = Quaternion(dcm)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(dcm)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(dcm)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
def _helper_test_constructor(self, q, euler, dcm):
        """
        Helper function for constructor test

        Calls constructor for the quaternion from q euler and dcm and checks
        if the resulting converions are equivalent to the arguments.
        The test for the euler angles is weak as the solution is not unique

        :param q: quaternion 4x1, [w, x, y, z]
        :param euler: Vector3(roll, pitch, yaw), needs to be equivalent to q
        :param q: Matrix3, needs to be equivalent to q
        """
        # construct q from a Quaternion
        quaternion_instance = Quaternion(q)
        q_test = Quaternion(quaternion_instance)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(quaternion_instance)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(quaternion_instance)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # construct q from a QuaternionBase
        quaternion_instance = QuaternionBase(q)
        q_test = Quaternion(quaternion_instance)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(quaternion_instance)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(quaternion_instance)
        q_euler = Quaternion(q_test.euler)
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
def test_conversion(self):
        """
        Tests forward and backward conversions
        """
        for q in self.quaternions:
            # quaternion -> euler -> quaternion
            q0 = q
            e = Quaternion(q.q).euler
            q1 = Quaternion(e)
            assert q0.close(q1)

            # quaternion -> dcm (Matrix3) -> quaternion
            q0 = q
            dcm = Quaternion(q.q).dcm
            q1 = Quaternion(dcm)
            assert q0.close(q1)
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
Calls constructor for the quaternion from q euler and dcm and checks
        if the resulting converions are equivalent to the arguments.
        The test for the euler angles is weak as the solution is not unique

        :param q: quaternion 4x1, [w, x, y, z]
        :param euler: Vector3(roll, pitch, yaw), needs to be equivalent to q
        :param q: Matrix3, needs to be equivalent to q
        """
        # construct q from a Quaternion
        quaternion_instance = Quaternion(q)
        q_test = Quaternion(quaternion_instance)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(quaternion_instance)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(quaternion_instance)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # construct q from a QuaternionBase
        quaternion_instance = QuaternionBase(q)
        q_test = Quaternion(quaternion_instance)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(quaternion_instance)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(quaternion_instance)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # construct q from a quaternion
github ArduPilot / ardupilot / Tools / autotest / arduplane.py View on Github external
target_roll_degrees = 0
                    if abs(r - target_roll_degrees) < tolerance:
                        state = state_done
                else:
                    raise ValueError("Unknown state %s" % str(state))

                m_nav = self.mav.messages['NAV_CONTROLLER_OUTPUT']
                self.progress("%s Roll: %f desired=%f set=%f" %
                              (state, r, m_nav.nav_roll, target_roll_degrees))

                time_boot_millis = 0 # FIXME
                target_system = 1 # FIXME
                target_component = 1 # FIXME
                type_mask = 0b10000001 ^ 0xFF # FIXME
                # attitude in radians:
                q = quaternion.Quaternion([math.radians(target_roll_degrees),
                                           0,
                                           0])
                roll_rate_radians = 0.5
                pitch_rate_radians = 0
                yaw_rate_radians = 0
                thrust = 1.0
                self.mav.mav.set_attitude_target_send(time_boot_millis,
                                                      target_system,
                                                      target_component,
                                                      type_mask,
                                                      q,
                                                      roll_rate_radians,
                                                      pitch_rate_radians,
                                                      yaw_rate_radians,
                                                      thrust)
        except Exception as e:
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
# construct q from a QuaternionBase
        quaternion_instance = QuaternionBase(q)
        q_test = Quaternion(quaternion_instance)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(quaternion_instance)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(quaternion_instance)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # construct q from a quaternion
        q_test = Quaternion(q)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(q)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(q)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # # construct q from a euler angles
        q_test = Quaternion(euler)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(euler)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(euler)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(quaternion_instance)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(quaternion_instance)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # construct q from a QuaternionBase
        quaternion_instance = QuaternionBase(q)
        q_test = Quaternion(quaternion_instance)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(quaternion_instance)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(quaternion_instance)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # construct q from a quaternion
        q_test = Quaternion(q)
        np.testing.assert_almost_equal(q_test.q, q)
        q_test = Quaternion(q)
        assert q_test.dcm.close(dcm)
        q_test = Quaternion(q)
        q_euler = Quaternion(q_test.euler)
        assert(np.allclose(q_test.euler, euler) or
               np.allclose(q_test.q, q_euler.q))

        # # construct q from a euler angles
        q_test = Quaternion(euler)
        np.testing.assert_almost_equal(q_test.q, q)
github ArduPilot / MAVProxy / MAVProxy / modules / mavproxy_vicon.py View on Github external
vel = (pos_ned - last_pos) * (1.0/dt)
            last_pos = pos_ned
            last_frame_num = frame_num

            filtered_vel = self.vel_filter.apply(vel)

            if self.vicon_settings.vision_rate > 0:
                dt = now - last_msg_time
                if dt < 1.0 / self.vicon_settings.vision_rate:
                    continue

            last_msg_time = now

            # get orientation in euler, converting to ArduPilot conventions
            quat = vicon.get_segment_global_quaternion(name, segname)
            q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
            euler = q.euler
            roll, pitch, yaw = euler[2], euler[1], euler[0]
            yaw += math.radians(self.vicon_settings.yaw_offset)
            yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))

            self.pos = pos_ned
            self.att = [math.degrees(roll), math.degrees(pitch), math.degrees(yaw)]
            self.frame_count += 1

            yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
            if yaw_cd == 0:
                # the yaw extension to GPS_INPUT uses 0 as no yaw support
                yaw_cd = 36000

            time_us = int(now * 1.0e6)
github ArduPilot / MAVProxy / MAVProxy / modules / mavproxy_magical / wxgeodesicgrid.py View on Github external
self.sphere.material.set_color(1.2 * self.base_color)
        self.sphere.material.specular_exponent = 4
        self.sphere.material.alpha = 1.0
        glPolygonMode(GL_FRONT_AND_BACK, GL_LINE)
        self.sphere.draw(self.program)

        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
        self.mag.draw(self.program)

        self.sphere.material.set_color(self.base_color)
        self.sphere.material.alpha = .4
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
        faces = [i for i in range(80) if self.visible[i]]
        self.sphere.draw(self.program, faces=faces, camera=self.camera)

        self.last_render_quaternion = quaternion.Quaternion(
                self.common_model_transform.quaternion)
github ArduPilot / MAVProxy / MAVProxy / modules / mavproxy_magical / wxgeodesicgrid.py View on Github external
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)