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()