def test_norm(self):
# """Tests the norm functions"""
qa = [1, 2, 3, 4]
q = QuaternionBase(qa)
n = np.sqrt(np.dot(qa, qa))
qan = qa / n
self.assertAlmostEqual(n, QuaternionBase.norm_array(qa))
np.testing.assert_almost_equal(qan, QuaternionBase.normalize_array(qa))
np.testing.assert_almost_equal(n, q.norm)
q.normalize()
np.testing.assert_almost_equal(qan, q.q)
self.assertAlmostEqual(1, q.norm)
def process_msg(self):
# the actuator assumes ned control, so don't do any transformation
q = QuaternionBase(self._msg.q)
self.data['roll'] = q.euler[0]
self.data['pitch'] = q.euler[1]
self.data['yaw'] = q.euler[2]
self.data['thrust'] = self._msg.thrust
def set_yaw():
set_yaw.heading = 0
roll = 0
pitch = 0
yaw = txt_set_heading.get()
control_yaw = True
# catch letters etc
try:
yaw = float(txt_set_heading.get())
set_yaw.heading = yaw
except:
return
bitmask = (1<<6 | 1<<3) if control_yaw else 1<<6
master.mav.set_attitude_target_send(
0,
0, 0,
bitmask,
QuaternionBase([math.radians(roll), math.radians(pitch), math.radians(yaw)]), # -> attitude quaternion (w, x, y, z | zero-rotation is 1, 0, 0, 0)
0, #roll rate
0, #pitch rate
0, 0) # yaw rate, thrust
def test_conversion(self):
"""
Tests forward and backward conversions
"""
for q in self.quaternions:
# quaternion -> euler -> quaternion
q0 = q
e = QuaternionBase(q.q).euler
q1 = QuaternionBase(e)
assert q0.close(q1)
# quaternion -> dcm -> quaternion
q0 = q
dcm = QuaternionBase(q.q).dcm
q1 = QuaternionBase(dcm)
assert q0.close(q1)
def set_yaw(direction=1):
# direction -1 or +1 left or right
set_yaw.heading = 0
roll = 0
pitch = 0
if heading_ref_var.get()=="ABSOLUTE":
# catch letters etc
try:
yaw = abs(float(txt_set_heading.get()))
set_yaw.heading = yaw
except:
return
elif heading_ref_var.get()=="RELATIVE":
try:
offset_yaw = abs(float(txt_set_heading.get()))
except:
return
# check for heading greater than single revolution just set to 0
if abs(float(offset_yaw)) > 360:
offsetyaw=0
# get current heading
reading_gotten = False
while reading_gotten == False:
data = update()
#doubled up as there is some caching issue.
data = update()
if 'VFR_HUD' in data:
current_yaw = data['VFR_HUD']['heading']
reading_gotten = True
yaw = current_yaw + (direction)*offset_yaw
if yaw > 359:
yaw-=360
if yaw < 0:
yaw+=360
set_yaw.heading = yaw
else:
return
control_yaw = True
bitmask = (1<<6 | 1<<3) if control_yaw else 1<<6
master.mav.set_attitude_target_send(
0,
0, 0,
bitmask,
QuaternionBase([math.radians(roll), math.radians(pitch), math.radians(yaw)]), # -> attitude quaternion (w, x, y, z | zero-rotation is 1, 0, 0, 0)
0, #roll rate
0, #pitch rate
0, 0) # yaw rate, thrust
def test_transform(self):
"""Test transform"""
for q in self.quaternions:
q_inv = q.inversed
v = np.array([1, 2, 3])
v1 = q.transform(v)
v1_dcm = np.dot(q.dcm, v)
np.testing.assert_almost_equal(v1, v1_dcm)
# test versus slower solution using multiplication
v1_mul = q * QuaternionBase(np.hstack([0, v])) * q.inversed
np.testing.assert_almost_equal(v1, v1_mul[1:4])
v2 = q_inv.transform(v1)
np.testing.assert_almost_equal(v, v2)
def set_yaw():
roll = 0
pitch = 0
yaw = 0
control_yaw = True
print('SET YAW') # or EXIT')
#valid_command = False
#while (valid_command == False):
# command = raw_input("> ")
# if command == 'EXIT':
# return
# elif command == 'ROLL' or command == 'PITCH' or command == 'YAW'
# valid_command = True
# else:
# print('INVALID, EXIT or try again')
#if command == 'ROLL':
# print('How much ROLL')
# roll = float(raw_input("> "))
#elif command == 'PITCH':
# print('How much PITCH')
# pitch = float(raw_input("> "))
#elif command == 'YAW':
# print('How much YAW')
# yaw = float(raw_input("> "))
yaw = float(raw_input("> "))
bitmask = (1 << 6 | 1 << 3) if control_yaw else 1 << 6
master.mav.set_attitude_target_send(
0,
0,
0,
bitmask,
QuaternionBase(
[math.radians(roll),
math.radians(pitch),
math.radians(yaw)]
), # -> attitude quaternion (w, x, y, z | zero-rotation is 1, 0, 0, 0)
0, #roll rate
0, #pitch rate
0,
0) # yaw rate, thrust
def test_norm(self):
# """Tests the norm functions"""
qa = [1, 2, 3, 4]
q = QuaternionBase(qa)
n = np.sqrt(np.dot(qa, qa))
qan = qa / n
self.assertAlmostEqual(n, QuaternionBase.norm_array(qa))
np.testing.assert_almost_equal(qan, QuaternionBase.normalize_array(qa))
np.testing.assert_almost_equal(n, q.norm)
q.normalize()
np.testing.assert_almost_equal(qan, q.q)
self.assertAlmostEqual(1, q.norm)
def arm_rov():
X = 0
Y = 0
Z = 0
reading_gotten = False
while reading_gotten == False:
data = update()
#doubled up as there is some caching issue.
data = update()
if 'VFR_HUD' in data:
#Depth is now commanded relative and not absolute
#current_depth = data['VFR_HUD']['alt']
current_heading = data['VFR_HUD']['heading']
reading_gotten = True
master.mav.set_position_target_local_ned_send(
0, # timestamp
0, 0, # target system_id # target component id
mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED, #MAV_FRAME_BODY_OFFSET_NED, # offset to current position and heading, MAV_FRAME_LOCAL_OFFSET_NED for testing recording as it only gives position in NED
0b110111111000, # mask specifying use-only-x-y-z
X, Y, Z, # x y z
0, 0, 0, #vx vy vz
0, 0, 0, # afx afy afz
0, 0) # yaw # yawrate
# set attitude to current attitude too
roll = 0
pitch = 0
master.mav.set_attitude_target_send(
0,
0, 0,
(1<<6 | 1<<3),
QuaternionBase([math.radians(roll), math.radians(pitch), math.radians(current_heading)]), # -> attitude quaternion (w, x, y, z | zero-rotation is 1, 0, 0, 0)
0, #roll rate
0, #pitch rate
0, 0) # yaw rate, thrust
# cannot arm while logging data given the heartbeat wait
master.arducopter_arm()
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: [roll, pitch, yaw], needs to be equivalent to q
:param q: dcm 3x3, needs to be equivalent to q
"""
# construct q from a QuaternionBase
quaternion_instance = QuaternionBase(q)
q_test = QuaternionBase(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(quaternion_instance)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(quaternion_instance)
q_euler = QuaternionBase(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 = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(q)
q_euler = QuaternionBase(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 = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(euler)
q_euler = QuaternionBase(q_test.euler)
assert (np.allclose(q_test.euler, euler)
or np.allclose(q_test.q, q_euler.q))
# construct q from dcm
q_test = QuaternionBase(dcm)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(dcm)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(dcm)
q_euler = QuaternionBase(q_test.euler)
assert (np.allclose(q_test.euler, euler)
or np.allclose(q_test.q, q_euler.q))
def _all_quaternions(self):
"""Generate quaternions from all euler angles"""
return [QuaternionBase(e) for e in self._all_angles()]