def fit_data(data):
from scipy import optimize
p0 = [
0.0,
0.0,
0.0, # offsets
500.0, # radius
1.0,
1.0,
1.0, # diagonals
0.0,
0.0,
0.0 # offdiagonals
]
if args.radius is not None:
p0[3] = args.radius
p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data))
if not ier in [1, 2, 3, 4]:
print(p1)
raise RuntimeError("Unable to find solution: %u" % ier)
if args.radius is not None:
r = args.radius
else:
r = p1[3]
return (Vector3(p1[0], p1[1],
p1[2]), r, Vector3(p1[4], p1[5],
p1[6]), Vector3(p1[7], p1[8], p1[9]))
def __init__(self, integrate_separately=True):
self.alpha = Vector3(0, 0, 0)
self.beta = Vector3(0, 0, 0)
self.last_alpha = Vector3(0, 0, 0)
self.last_delta_alpha = Vector3(0, 0, 0)
self.last_sample = Vector3(0, 0, 0)
self.integrate_separately = integrate_separately
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 set_mag(self, mag):
if not mag.length():
return
m = self.common_model_transform.apply(mag).normalized()
axis = Vector3(0, 0, 1) % m
angle = math.acos(Vector3(0, 0, 1) * m)
self.mag.model.set_rotation(axis, angle)
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 fit_data(data):
from scipy import optimize
p0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data))
if not ier in [1, 2, 3, 4]:
raise RuntimeError("Unable to find solution")
return (Vector3(p1[0], p1[1], p1[2]), Vector3(p1[3], p1[4], p1[5]), p1[6])
def __init__(self):
# Base must be orthonormal
self.base = (Vector3(1.0, 0.0,
0.0), Vector3(0.0, 1.0,
0.0), Vector3(0.0, 0.0, 1.0))
self.position = Vector3(0.0, 0.0, 0.0)
self.position_transform = Transform()
self.base_transform = Transform()
def imu_cb(self, msg):
self.gyro = Vector3(msg.angular_velocity.x, -msg.angular_velocity.y,
-msg.angular_velocity.z)
self.accel_body = Vector3(msg.linear_acceleration.x,
-msg.linear_acceleration.y,
-msg.linear_acceleration.z)
self.dcm = quat_to_dcm(msg.orientation.w, msg.orientation.x,
-msg.orientation.y, -msg.orientation.z)
self.have_new_imu = True
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"""
v1 = Vector3(1, 0.2, -3)
v2 = Vector3([1, 0.2, -3])
v3 = Vector3([1, 0.3, -3])
assert v1 == v2
assert v1 != v3
assert str(v1) == "Vector3(1.00, 0.20, -3.00)"
def sphere_error(p, data):
x,y,z,mx,my,mz,r = p
ofs = Vector3(x,y,z)
motor_ofs = Vector3(mx,my,mz)
ret = []
for d in data:
(mag,motor) = d
err = r - radius((mag,motor), ofs, motor_ofs)
ret.append(err)
return ret
def mtl_to_material(mtl):
if mtl.name not in material_map:
m = Material(
ambient=Vector3(*mtl.Ka),
diffuse=Vector3(*mtl.Kd),
specular=Vector3(*mtl.Ks),
specular_exponent=mtl.Ns,
)
material_map[mtl.name] = m
return material_map[mtl.name]
def update_airspeed_estimate(self, m):
'''update airspeed estimate for helicopters'''
if self.cuav_settings.wind_speed <= 0:
return
from pymavlink.rotmat import Vector3
wind = Vector3(self.cuav_settings.wind_speed*math.cos(math.radians(self.cuav_settings.wind_direction)),
self.cuav_settings.wind_speed*math.sin(math.radians(self.cuav_settings.wind_direction)), 0)
ground = Vector3(m.vx*0.01, m.vy*0.01, 0)
airspeed = ground + wind
self.console.set_status('AirspeedEstimate', 'AirspeedEstimate: %u m/s' % airspeed.length(), row=8)
def quaternion_to_axis_angle(q):
from pymavlink.rotmat import Vector3
a, b, c, d = q.q
n = math.sqrt(b**2 + c**2 + d**2)
if not n:
return Vector3(0, 0, 0)
angle = 2 * math.acos(a)
if angle > math.pi:
angle = angle - 2 * math.pi
return Vector3(angle * b / n, angle * c / n, angle * d / n)
def gyro_integrate(conn):
'''test gyros by integrating while rotating to the given rotations'''
conn.ref.send('set streamrate -1\n')
conn.test.send('set streamrate -1\n')
util.param_set(conn.ref, 'SR0_RAW_SENS', 20)
util.param_set(conn.test, 'SR0_RAW_SENS', 20)
logger.info("Starting gyro integration")
wait_quiescent(conn.refmav)
conn.discard_messages()
util.set_servo(conn.refmav, YAW_CHANNEL, ROTATIONS['level'].chan1+200)
util.set_servo(conn.refmav, PITCH_CHANNEL, ROTATIONS['level'].chan2+200)
start_time = time.time()
ref_tstart = None
test_tstart = [None]*3
ref_sum = Vector3()
test_sum = [Vector3(), Vector3(), Vector3()]
msgs = { 'RAW_IMU' : 0, 'SCALED_IMU2' : 1, 'SCALED_IMU3' : 2 }
while time.time() < start_time+20:
imu = conn.refmav.recv_match(type='RAW_IMU', blocking=False)
if imu is not None:
gyro = util.gyro_vector(imu)
tnow = imu.time_usec*1.0e-6
if ref_tstart is not None:
deltat = tnow - ref_tstart
ref_sum += gyro * deltat
ref_tstart = tnow
if time.time() - start_time > 2 and gyro.length() < GYRO_TOLERANCE:
break
imu = conn.testmav.recv_match(type=msgs.keys(), blocking=False)
if imu is not None:
idx = msgs[imu.get_type()]
gyro = util.gyro_vector(imu)
if imu.get_type().startswith("SCALED_IMU"):
tnow = imu.time_boot_ms*1.0e-3
else:
tnow = imu.time_usec*1.0e-6
if test_tstart[idx] is not None:
deltat = tnow - test_tstart[idx]
test_sum[idx] += gyro * deltat
test_tstart[idx] = tnow
logger.debug("Gyro ref sums: %s" % ref_sum)
logger.debug("Gyro test sum1: %s" % test_sum[0])
logger.debug("Gyro test sum2: %s" % test_sum[1])
logger.debug("Gyro test sum3: %s" % test_sum[2])
for idx in range(3):
err = test_sum[idx] - ref_sum
if abs(err.x) > GYRO_SUM_TOLERANCE:
util.failure("X gyro %u error: %.1f" % (idx, err.x))
if abs(err.y) > GYRO_SUM_TOLERANCE:
util.failure("Y gyro %u error: %.1f" % (idx, err.y))
if abs(err.z) > GYRO_SUM_TOLERANCE:
util.failure("Z gyro %u error: %.1f" % (idx, err.z))
def __init__(self,
ambient=Vector3(.5, .5, .5),
diffuse=Vector3(.5, .5, .5),
specular=Vector3(.5, .5, .5),
specular_exponent=32.0,
alpha=1.0):
self.ambient = ambient
self.diffuse = diffuse
self.specular = specular
self.specular_exponent = specular_exponent
self.alpha = alpha
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 __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_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_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 magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
data = []
last_t = 0
offsets = Vector3(0, 0, 0)
# now gather all the data
while True:
m = mlog.recv_match(condition=opts.condition)
if m is None:
break
if m.get_type() == "SENSOR_OFFSETS":
# update current offsets
offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
if m.get_type() == "RAW_IMU":
mag = Vector3(m.xmag, m.ymag, m.zmag)
# add data point after subtracting the current offsets
data.append(mag - offsets + noise())
print("Extracted %u data points" % len(data))
print("Current offsets: %s" % offsets)
data = select_data(data)
# remove initial outliers
data.sort(lambda a, b: radius_cmp(a, b, offsets))
data = data[len(data) / 16:-len(data) / 16]
# do an initial fit
(offsets, field_strength) = fit_data(data)
for count in range(3):
# sort the data by the radius
data.sort(lambda a, b: radius_cmp(a, b, offsets))
print("Fit %u : %s field_strength=%6.1f to %6.1f" %
(count, offsets, radius(data[0],
offsets), radius(data[-1], offsets)))
# discard outliers, keep the middle 3/4
data = data[len(data) / 8:-len(data) / 8]
# fit again
(offsets, field_strength) = fit_data(data)
print("Final : %s field_strength=%6.1f to %6.1f" %
(offsets, radius(data[0], offsets), radius(data[-1], offsets)))
def __init__(self,
position=Vector3(0.0, 0.0, 1.0),
ambient=Vector3(1.0, 1.0, 1.0),
diffuse=Vector3(1.0, 1.0, 1.0),
specular=Vector3(1.0, 1.0, 1.0),
att_linear=0.0,
att_quad=0.0):
self.position = position
self.ambient = ambient
self.diffuse = diffuse
self.specular = specular
self.att_linear = att_linear
self.att_quad = att_quad
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 sphere_error(p, data):
x,y,z,r,dx,dy,dz,odx,ody,odz = p
if args.radius is not None:
r = args.radius
ofs = Vector3(x,y,z)
diag = Vector3(dx, dy, dz)
offdiag = Vector3(odx, ody, odz)
ret = []
for d in data:
mag = correct(d, ofs, diag, offdiag)
err = r - mag.length()
ret.append(err)
return ret
def __init__(self, background):
glClearColor(*background)
self.program = opengl.Program()
self.program.compile_and_link()
self.program.use_light(
opengl.Light(
position=Vector3(-2.0, 0.0, -1.0),
ambient=Vector3(0.8, 0.8, 0.8),
diffuse=Vector3(0.5, 0.5, 0.5),
specular=Vector3(0.25, 0.25, 0.25),
att_linear=0.000,
att_quad=0.000,
))
self.camera = opengl.Camera()
# Adjust camera to show NED coordinates properly
self.camera.base = (
Vector3(0, 1, 0),
Vector3(0, 0, -1),
Vector3(-1, 0, 0),
)
self.camera.position = Vector3(-100.0, 0, 0)
near = -self.camera.position.x - 1.0
far = near + 2.0
self.projection = opengl.Orthographic(near=near, far=far)
self.program.use_projection(self.projection)
def SetAngvel(self, axis, speed):
if not isinstance(axis, Vector3):
axis = Vector3(*axis)
axis = self.renderer.vehicle.model.apply(axis)
self.angvel = axis, speed
self.actuation_state = 'angvel'
self.StartActuation()
def __init__(self, background):
super(Renderer, self).__init__(background)
glEnable(GL_DEPTH_TEST)
glEnable(GL_MULTISAMPLE)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
self.visible = [False for _ in range(len(geodesic_grid.sections))]
self.common_model_transform = opengl.Transform()
self.last_render_quaternion = quaternion.Quaternion(
self.common_model_transform.quaternion)
vertices = [(p.x, p.y, p.z) for t in geodesic_grid.sections for p in t]
self.sphere = opengl.Object(vertices, enable_alpha=True)
self.sphere.local.scale(0.46)
self.sphere.model = self.common_model_transform
self.base_color = Vector3(0.08, 0.68, 0.706667)
self.vehicle = None
path = os.path.join(magical.datapath, 'arrow.obj')
obj = wv.ObjParser(filename=path).parse()
self.mag = opengl.WavefrontObject(obj)
self.mag.local.scale(.88)
def to_euler(self):
roll = (atan2(2.0 * (self.w * self.x + self.y * self.z),
1 - 2.0 * (self.x * self.x + self.y * self.y)))
pitch = asin(2.0 * (self.w * self.y - self.z * self.x))
yaw = atan2(2.0 * (self.w * self.z + self.x * self.y),
1 - 2.0 * (self.y * self.y + self.z * self.z))
return Vector3(roll, pitch, yaw)
def drag(self, velocity, deltat=None):
"""Return current wind force in Earth frame. The velocity parameter is
a Vector3 of the current velocity of the aircraft in earth frame, m/s ."""
from math import radians
# (m/s, degrees) : wind vector as a magnitude and angle.
(speed, direction) = self.current(deltat=deltat)
# speed = self.speed
# direction = self.direction
# Get the wind vector.
w = toVec(speed, radians(direction))
obj_speed = velocity.length()
# Compute the angle between the object vector and wind vector by taking
# the dot product and dividing by the magnitudes.
d = w.length() * obj_speed
if d == 0:
alpha = 0
else:
alpha = acos((w * velocity) / d)
# Get the relative wind speed and angle from the object. Note that the
# relative wind speed includes the velocity of the object; i.e., there
# is a headwind equivalent to the object's speed even if there is no
# absolute wind.
(rel_speed, beta) = apparent_wind(speed, obj_speed, alpha)
# Return the vector of the relative wind, relative to the coordinate
# system.
relWindVec = toVec(rel_speed, beta + atan2(velocity.y, velocity.x))
# Combine them to get the acceleration vector.
return Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x)), acc(relWindVec.y, drag_force(self, relWindVec.y)), 0)
