def try_rotation(rotation,
demanded_roll,
demanded_pitch,
chan1, chan2):
(r, p, y) = gimbal_model(chan1, chan2)
print("%s: %.1f %.1f %.1f" % (rotation,
degrees(r),
degrees(p),
degrees(y)))
for idx in range(90):
dcm_estimated = Matrix3()
dcm_estimated.from_euler(r,p,y)
droll = demanded_roll
if droll is None:
droll = r
else:
droll = radians(droll)
dpitch = radians(demanded_pitch)
dcm_demanded = Matrix3()
dcm_demanded.from_euler(droll, dpitch, y)
(chan1_change, chan2_change) = gimbal_controller(dcm_estimated,
dcm_demanded, chan1)
chan1 += chan1_change
chan2 += chan2_change
(r, p, y) = gimbal_model(chan1, chan2)
print("-> %.1f %.1f %.1f" % (degrees(r),
degrees(p),
degrees(y)))
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if not self.mpstate.map:
# don't draw if no map
return
if m.get_type() != 'GIMBAL_REPORT':
return
needed = ['ATTITUDE', 'GLOBAL_POSITION_INT']
for n in needed:
if not n in self.master.messages:
return
# clear the camera icon
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('GimbalView'))
gpi = self.master.messages['GLOBAL_POSITION_INT']
att = self.master.messages['ATTITUDE']
vehicle_dcm = Matrix3()
vehicle_dcm.from_euler(att.roll, att.pitch, att.yaw)
rotmat_copter_gimbal = Matrix3()
rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el, m.joint_az)
gimbal_dcm = vehicle_dcm * rotmat_copter_gimbal
lat = gpi.lat * 1.0e-7
lon = gpi.lon * 1.0e-7
alt = gpi.relative_alt * 1.0e-3
# ground plane
ground_plane = Plane()
# the position of the camera in the air, remembering its a right
# hand coordinate system, so +ve z is down
camera_point = Vector3(0, 0, -alt)
# get view point of camera when not rotated
view_point = Vector3(1, 0, 0)
# rotate view_point to form current view vector
rot_point = gimbal_dcm * view_point
# a line from the camera to the ground
line = Line(camera_point, rot_point)
# find the intersection with the ground
pt = line.plane_intersection(ground_plane, forward_only=True)
if pt is None:
# its pointing up into the sky
return None
(view_lat, view_lon) = mp_util.gps_offset(lat, lon, pt.y, pt.x)
icon = self.mpstate.map.icon('camera-small-red.png')
self.mpstate.map.add_object(mp_slipmap.SlipIcon('gimbalview',
(view_lat,view_lon),
icon, layer='GimbalView', rotation=0, follow=False))
def gimbal_model(chan1, chan2):
'''model the gimbal setup'''
zero_chan1 = ROTATIONS['level'][0]
zero_chan2 = ROTATIONS['level'][1]
dcm = Matrix3()
dcm1 = Matrix3()
dcm1.from_euler(0, radians(PITCH_SCALE) * (chan2 - zero_chan2), 0)
dcm2 = Matrix3()
dcm2.from_euler(0, 0, radians(YAW_SCALE)*(chan1-zero_chan1))
dcm *= dcm1 * dcm2
return dcm.to_euler()
def test_axisangle(self):
axis = Vector3(0, 1, 0)
angle = radians(45)
m1 = Matrix3()
m1.from_axis_angle(axis, angle)
#print(m1)
assert m1.close(Matrix3(Vector3(0.71, 0.00, 0.71),
Vector3(0.00, 1.00, 0.00),
Vector3(-0.71, 0.00, 0.71)),
tol=1e-2)
def test_constructor(self):
"""Test the constructor functionality"""
m1 = Matrix3(Vector3(1, 0, 0), Vector3(1, 5, 0), Vector3(1, 0, -7))
m2 = Matrix3()
assert str(
m1
) == 'Matrix3((1.00, 0.00, 0.00), (1.00, 5.00, 0.00), (1.00, 0.00, -7.00))'
assert str(
m2
) == 'Matrix3((1.00, 0.00, 0.00), (0.00, 1.00, 0.00), (0.00, 0.00, 1.00))'
def test_matrixops(self):
m1 = Matrix3(Vector3(1, 0, 0), Vector3(1, 5, 0), Vector3(1, 0, -7))
m1.normalize()
#print(m1)
assert m1.close(Matrix3(Vector3(0.2, -0.98, 0), Vector3(0.1, 1, 0),
Vector3(0, 0, 1)),
tol=1e-2)
np.testing.assert_almost_equal(m1.trace(), 2.19115332535)
m1.rotate(Vector3(0.2, -0.98, 0))
assert m1.close(Matrix3(Vector3(0.2, -0.98, 0), Vector3(0.1, 1, -0.3),
Vector3(0.98, 0.2, 1)),
tol=1e-2)
def __init__(self, name, roll, pitch, yaw):
self.name = name
self.roll = roll
self.pitch = pitch
self.yaw = yaw
self.r = Matrix3()
self.r.from_euler(self.roll, self.pitch, self.yaw)
def __init__(self):
self.home_latitude = 0
self.home_longitude = 0
self.home_altitude = 0
self.ground_level = 0
self.frame_height = 0.0
self.latitude = self.home_latitude
self.longitude = self.home_longitude
self.altitude = self.home_altitude
self.dcm = Matrix3()
# rotation rate in body frame
self.gyro = Vector3(0, 0, 0) # rad/s
self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
self.position = Vector3(0, 0, 0) # m North, East, Down
self.mass = 0.0
self.update_frequency = 50 # in Hz
self.gravity = 9.80665 # m/s/s
self.accelerometer = Vector3(0, 0, -self.gravity)
self.wind = util.Wind('0,0,0')
self.time_base = time.time()
self.time_now = self.time_base + 100 * 1.0e-6
self.gyro_noise = math.radians(0.1)
self.accel_noise = 0.3
def correct(mag, offsets, diag, offdiag):
'''correct a mag sample'''
diag.x = 1.0
mat = Matrix3(Vector3(diag.x, offdiag.x, offdiag.y),
Vector3(offdiag.x, diag.y, offdiag.z),
Vector3(offdiag.y, offdiag.z, diag.z))
mag = mag + offsets
mag = mat * mag
return mag
def test_constructor(self):
"""Test the constructor functionality"""
# Test the identity case
q = [1, 0, 0, 0]
euler = [0, 0, 0]
dcm = Matrix3()
self._helper_test_constructor(q, euler, dcm)
# test a case with rotations around all angles (values from matlab)
q = [0.774519052838329, 0.158493649053890, 0.591506350946110,
0.158493649053890]
euler = [np.radians(60), np.radians(60), np.radians(60)]
dcm = Matrix3(Vector3(0.25, -0.058012701892219, 0.966506350946110),
Vector3(0.433012701892219, 0.899519052838329,
-0.058012701892219),
Vector3(-0.866025403784439, 0.433012701892219, 0.25))
self._helper_test_constructor(q, euler, dcm)
def generate_rotations():
'''generate all 90 degree rotations'''
rotations = []
for yaw in [0, 90, 180, 270]:
for pitch in [0, 90, 180, 270]:
for roll in [0, 90, 180, 270]:
m = Matrix3()
m.from_euler(radians(roll), radians(pitch), radians(yaw))
if not in_rotations_list(rotations, m):
rotations.append(Rotation(roll, pitch, yaw, m))
return rotations
def add_errors(mag, gyr, last_mag, deltat, total_error, rotations):
for i in range(len(rotations)):
if not rotations[i].is_90_degrees():
continue
r = rotations[i].r
m = Matrix3()
m.rotate(gyr * deltat)
rmag1 = r * last_mag
rmag2 = r * mag
rmag3 = m.transposed() * rmag1
err = rmag3 - rmag2
total_error[i] += err.length()
def __init__(self):
self.latitude = 0
self.longitude = 0
self.altitude = 0
self.heading = 0
self.velocity = Vector3()
self.accel = Vector3()
self.gyro = Vector3()
self.attitude = Vector3()
self.airspeed = 0
self.dcm = Matrix3()
self.timestamp_us = 1
def test_euler(self):
'''check that from_euler() and to_euler() are consistent'''
m = Matrix3()
for r in range(-179, 179, 10):
for p in range(-89, 89, 10):
for y in range(-179, 179, 10):
m.from_euler(radians(r), radians(p), radians(y))
(r2, p2, y2) = m.to_euler()
v1 = Vector3(r, p, y)
v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
diff = v1 - v2
assert diff.length() < 1.0e-12
def test_two_vectors(self):
'''test the from_two_vectors() method'''
for i in range(100):
v1 = Vector3(1, 0.2, -3)
v2 = Vector3(random.uniform(-5, 5), random.uniform(-5, 5),
random.uniform(-5, 5))
m = Matrix3()
m.from_two_vectors(v1, v2)
v3 = m * v1
diff = v3.normalized() - v2.normalized()
(r, p, y) = m.to_euler()
assert diff.length() < 0.001
def test_maths(self):
m1 = Matrix3(Vector3(1, 0, 0), Vector3(1, 5, 0), Vector3(1, 0, -7))
m2 = Matrix3()
assert m1 + m2 == Matrix3(Vector3(2, 0, 0), Vector3(1, 6, 0),
Vector3(1, 0, -6))
assert m1 - m2 == Matrix3(Vector3(0, 0, 0), Vector3(1, 4, 0),
Vector3(1, 0, -8))
assert m1 * 3 == Matrix3(Vector3(3, 0, 0), Vector3(3, 15, 0),
Vector3(3, 0, -21))
assert m1 * m1 == Matrix3(Vector3(1, 0, 0), Vector3(6, 25, 0),
Vector3(-6, 0, 49))
assert m1 / 2.0 == Matrix3(Vector3(0.5, 0, 0), Vector3(0.5, 2.5, 0),
Vector3(0.5, 0, -3.5))
assert m1 / 0.5 == Matrix3(Vector3(2, 0, 0), Vector3(2, 10, 0),
Vector3(2, 0, -14))
assert m1.transposed() == Matrix3(Vector3(1, 1, 1), Vector3(0, 5, 0),
Vector3(0, 0, -7))
def expected_field(ATT, yaw):
'''return expected magnetic field for attitude'''
global earth_field
roll = ATT.Roll
pitch = ATT.Pitch
rot = Matrix3()
rot.from_euler(math.radians(roll), math.radians(pitch), math.radians(yaw))
field = rot.transposed() * earth_field
return field
def remove_offsets(MAG, BAT, c):
'''remove all corrections to get raw sensor data'''
correction_matrix = Matrix3(Vector3(c.diag.x, c.offdiag.x, c.offdiag.y),
Vector3(c.offdiag.x, c.diag.y, c.offdiag.z),
Vector3(c.offdiag.y, c.offdiag.z, c.diag.z))
correction_matrix = correction_matrix.invert()
field = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
field -= c.cmot * BAT.Curr
field = correction_matrix * field
field *= 1.0 / c.scaling
field -= Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ)
MAG.MagX = int(field.x)
MAG.MagY = int(field.y)
MAG.MagZ = int(field.z)
def remove_offsets(MAG, BAT, c):
'''remove all corrections to get raw sensor data'''
correction_matrix = Matrix3(Vector3(c.diag.x, c.offdiag.x, c.offdiag.y),
Vector3(c.offdiag.x, c.diag.y, c.offdiag.z),
Vector3(c.offdiag.y, c.offdiag.z, c.diag.z))
correction_matrix = correction_matrix.invert()
field = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
if BAT is not None and not math.isnan(BAT.Curr):
field -= c.cmot * BAT.Curr
field = correction_matrix * field
field *= 1.0 / c.scaling
field -= Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ)
if math.isnan(field.x) or math.isnan(field.y) or math.isnan(field.z):
return False
MAG.MagX = int(field.x)
MAG.MagY = int(field.y)
MAG.MagZ = int(field.z)
return True
def correct(MAG, BAT, c):
'''correct a mag sample, returning a Vector3'''
mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
# add the given offsets
mag += c.offsets
# multiply by scale factor
mag *= c.scaling
# apply elliptical corrections
mat = Matrix3(Vector3(c.diag.x, c.offdiag.x, c.offdiag.y),
Vector3(c.offdiag.x, c.diag.y, c.offdiag.z),
Vector3(c.offdiag.y, c.offdiag.z, c.diag.z))
mag = mat * mag
# apply compassmot corrections
mag += c.cmot * BAT.Curr
return mag
def to_rotation_matrix(self):
m = Matrix3()
yy = self.y**2
yz = self.y * self.z
xx = self.x**2
xy = self.x * self.y
xz = self.x * self.z
wx = self.w * self.x
wy = self.w * self.y
wz = self.w * self.z
zz = self.z**2
m.a.x = 1.0 - 2.0 * (yy + zz)
m.a.y = 2.0 * (xy - wz)
m.a.z = 2.0 * (xz + wy)
m.b.x = 2.0 * (xy + wz)
m.b.y = 1.0 - 2.0 * (xx + zz)
m.b.z = 2.0 * (yz - wx)
m.c.x = 2.0 * (xz - wy)
m.c.y = 2.0 * (yz + wx)
m.c.z = 1.0 - 2.0 * (xx + yy)
return m
def quat_to_dcm(q1, q2, q3, q4):
"""Convert quaternion to DCM."""
q3q3 = q3 * q3
q3q4 = q3 * q4
q2q2 = q2 * q2
q2q3 = q2 * q3
q2q4 = q2 * q4
q1q2 = q1 * q2
q1q3 = q1 * q3
q1q4 = q1 * q4
q4q4 = q4 * q4
m = Matrix3()
m.a.x = 1.0 - 2.0 * (q3q3 + q4q4)
m.a.y = 2.0 * (q2q3 - q1q4)
m.a.z = 2.0 * (q2q4 + q1q3)
m.b.x = 2.0 * (q2q3 + q1q4)
m.b.y = 1.0 - 2.0 * (q2q2 + q4q4)
m.b.z = 2.0 * (q3q4 - q1q2)
m.c.x = 2.0 * (q2q4 - q1q3)
m.c.y = 2.0 * (q3q4 + q1q2)
m.c.z = 1.0 - 2.0 * (q2q2 + q3q3)
return m
self.v0_c0 = self.index_to_attr[v0_c0]
self.v1_c1 = self.index_to_attr[v1_c1]
self.v2_c1 = self.index_to_attr[v2_c1]
self.v4_c4 = self.index_to_attr[v4_c4]
self.v0_c4 = self.index_to_attr[v0_c4]
_neighbor_umbrellas = (
_NeighborUmbrella((9, 8, 7, 12, 14), 1, 2, 0, 0, 2),
_NeighborUmbrella((1, 2, 4, 5, 3), 0, 0, 2, 2, 0),
_NeighborUmbrella((16, 15, 13, 18, 17), 2, 2, 0, 2, 1),
)
_inverses = (
Matrix3(Vector3(-0.309017, 0.500000, 0.190983),
Vector3(0.000000, 0.000000, -0.618034),
Vector3(-0.309017, -0.500000, 0.190983)),
Matrix3(Vector3(-0.190983, 0.309017, -0.500000),
Vector3(-0.500000, -0.190983, 0.309017),
Vector3(0.309017, -0.500000, -0.190983)),
Matrix3(Vector3(-0.618034, 0.000000, 0.000000),
Vector3(0.190983, -0.309017, -0.500000),
Vector3(0.190983, -0.309017, 0.500000)),
Matrix3(Vector3(-0.500000, 0.190983, -0.309017),
Vector3(0.000000, -0.618034, 0.000000),
Vector3(0.500000, 0.190983, -0.309017)),
Matrix3(Vector3(-0.190983, -0.309017, -0.500000),
Vector3(-0.190983, -0.309017, 0.500000),
Vector3(0.618034, 0.000000, 0.000000)),
Matrix3(Vector3(-0.309017, -0.500000, -0.190983),
Vector3(0.190983, 0.309017, -0.500000),
def optimise_attitude(conn, rotation, tolerance, timeout=25, quick=False):
'''optimise attitude using servo changes'''
expected_roll = ROTATIONS[rotation].roll
expected_pitch = ROTATIONS[rotation].pitch
chan1 = ROTATIONS[rotation].chan1
chan2 = ROTATIONS[rotation].chan2
attitude = wait_quiescent(conn.refmav)
time_start = time.time()
# we always do at least 2 tries. This means the attitude accuracy
# will tend to improve over time, while not adding excessive time
# per board
tries = 0
while time.time() < time_start+timeout:
#logger.info("============================= BEGIN ROTATIONS try=%s =================" % (tries))
dcm_estimated = Matrix3()
dcm_estimated.from_euler(attitude.roll, attitude.pitch, attitude.yaw)
droll = expected_roll
if droll is None:
droll = attitude.roll
else:
droll = radians(droll)
dpitch = radians(expected_pitch)
dcm_demanded = Matrix3()
dcm_demanded.from_euler(droll, dpitch, attitude.yaw)
(chan1_change, chan2_change) = gimbal_controller(dcm_estimated,
dcm_demanded, chan1)
(err_roll, err_pitch) = attitude_error(attitude, expected_roll, expected_pitch)
logger.info("optimise_attitude: %s err_roll=%.2f err_pitch=%.2f c1=%u c2=%u" % (rotation, err_roll, err_pitch, chan1, chan2))
if ((abs(err_roll)+abs(err_pitch) > 5*tolerance and
(abs(chan1_change)<1 and abs(chan2_change)<1))):
chan1_change += random.uniform(-20, 20)
chan2_change += random.uniform(-20, 20)
if (tries > 0 and (abs(err_roll)+abs(err_pitch) < tolerance or
(abs(chan1_change)<1 and abs(chan2_change)<1))):
print("roll=%.2f pitch=%.2f expected_roll=%s expected_pitch=%s" % (
degrees(attitude.roll),
degrees(attitude.pitch),
expected_roll, expected_pitch))
logger.info("%s converged %.2f %.2f tolerance %.1f" % (rotation, err_roll, err_pitch, tolerance))
# update optimised rotations to save on convergence time for the next board
ROTATIONS[rotation].chan1 = chan1
ROTATIONS[rotation].chan2 = chan2
return True
chan1 += chan1_change
chan2 += chan2_change
if chan1 < 700:
chan1 += 900
if chan2 < 700:
chan2 += 900
if chan1 > 2300:
chan1 -= 900
if chan2 > 2300:
chan2 -= 900
if chan1 < 700 or chan1 > 2300 or chan2 < 700 or chan2 > 2300:
logger.debug("servos out of range")
return False
util.set_servo(conn.refmav, YAW_CHANNEL, chan1)
util.set_servo(conn.refmav, PITCH_CHANNEL, chan2)
attitude = wait_quiescent(conn.refmav, quick=quick)
tries += 1
logger.error("timed out rotating to %s" % rotation)
return False
def toVec(magnitude, angle):
"""Converts a magnitude and angle (radians) to a vector in the xy plane."""
v = Vector3(magnitude, 0, 0)
m = Matrix3()
m.from_euler(0, 0, angle)
return m.transposed() * v
def __init__(self):
self.r = Matrix3()
self.vel = Vector3()
self.pos = Vector3()
