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)