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
def __init__(self, roll, pitch, yaw, timestamp): self.dcm = Matrix3() self.dcm.from_euler(radians(roll), radians(pitch), radians(yaw)) self.gyro = Vector3() self.accel = Vector3() self.timestamp = timestamp (self.roll, self.pitch, self.yaw) = self.dcm.to_euler()
def pixel_position(xpos, ypos, height, pitch, roll, yaw, C): ''' find the offset on the ground in meters of a pixel in a ground image given height above the ground in meters, and pitch/roll/yaw in degrees, the lens and image parameters The xpos,ypos is from the top-left of the image The height is in meters The yaw is from grid north. Positive yaw is clockwise The roll is from horiznotal. Positive roll is down on the right The pitch is from horiznotal. Positive pitch is up in the front lens is in mm sensorwidth is in mm xresolution and yresolution is in pixels return result is a tuple, with meters east and north of current GPS position This is only correct for small values of pitch/roll ''' from rotmat import Vector3, Matrix3, Plane, Line from math import radians # get pixel sizes in meters, this assumes we are pointing straight down with square pixels pw = pixel_width(height, xresolution=C.xresolution, lens=C.lens, sensorwidth=C.sensorwidth) # 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, -height) # get position on ground relative to camera assuming camera is pointing straight down ground_point = Vector3(-pw * (ypos - (C.yresolution / 2)), pw * (xpos - (C.xresolution / 2)), height) # form a rotation matrix from our current attitude r = Matrix3() r.from_euler(radians(roll), radians(pitch), radians(yaw)) # rotate ground_point to form vector in ground frame rot_point = r * ground_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 return (pt.y, pt.x)
def _euler_to_dcm(self, euler): """ Create DCM (Matrix3) from euler angles :param euler: array [roll, pitch, yaw] in rad :returns: Matrix3 """ assert (len(euler) == 3) m = Matrix3() m.from_euler(*euler) return m
def _dcm_array_to_matrix3(self, dcm): """ Converts dcm array into Matrix3 :param dcm: 3x3 dcm array :returns: Matrix3 """ assert (dcm.shape == (3, 3)) a = Vector3(dcm[0][0], dcm[0][1], dcm[0][2]) b = Vector3(dcm[1][0], dcm[1][1], dcm[1][2]) c = Vector3(dcm[2][0], dcm[2][1], dcm[2][2]) return Matrix3(a, b, c)
def expected_mag(RAW_IMU, ATTITUDE, inclination, declination): '''return expected mag vector''' m_body = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag) field_strength = m_body.length() m = rotation(ATTITUDE) r = Matrix3() r.from_euler(0, -radians(inclination), radians(declination)) m_earth = r * Vector3(field_strength, 0, 0) return m.transposed() * m_earth
def mag_rotation(RAW_IMU, inclination, declination): '''return an attitude rotation matrix that is consistent with the current mag vector''' m_body = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag) m_earth = Vector3(m_body.length(), 0, 0) r = Matrix3() r.from_euler(0, -radians(inclination), radians(declination)) m_earth = r * m_earth r.from_two_vectors(m_earth, m_body) return r
def __init__(self, roll, pitch, yaw): self.dcm = Matrix3() self.dcm2 = Matrix3() self.dcm.from_euler(radians(roll), radians(pitch), radians(yaw)) self.dcm2.from_euler(radians(roll), radians(pitch), radians(yaw)) self.mag = Vector3() self.gyro = Vector3() self.accel = Vector3() self.gps = None self.rate = 50.0 self.kp = 0.2 self.kp_yaw = 0.3 self.omega_P = Vector3() self.omega_P_yaw = Vector3() self.omega_I = Vector3() # (-0.00199045287445, -0.00653007719666, -0.00714212376624) self.omega_I_sum = Vector3() self.omega_I_sum_time = 0 self.omega = Vector3() self.ra_sum = Vector3() self.last_delta_angle = Vector3() self.last_velocity = Vector3() (self.roll, self.pitch, self.yaw) = self.dcm.to_euler() (self.roll2, self.pitch2, self.yaw2) = self.dcm2.to_euler()
def accel(self, velocity, deltat=None): '''return current wind acceleration in ground frame. The velocity is a Vector3 of the current velocity of the aircraft in earth frame, m/s''' from math import radians (speed, direction) = self.current(deltat=deltat) # wind vector v = Vector3(speed, 0, 0) m = Matrix3() m.from_euler(0, 0, radians(direction)) wind = m.transposed() * v # relative wind vector relwind = wind - velocity # add in cross-section effect a = relwind * self.cross_section return a
def quat_to_dcm(q1, q2, q3, q4): 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
def rotation(ATTITUDE): '''return the current DCM rotation matrix''' r = Matrix3() r.from_euler(ATTITUDE.roll, ATTITUDE.pitch, ATTITUDE.yaw) return r
def update(self): '''update the gimbal state''' # calculate delta time in seconds now = time.time() delta_t = now - self.last_update_t self.last_update_t = now if self.dcm is None: # start with copter rotation matrix self.dcm = self.vehicle.dcm.copy() # take a copy of the demanded rates to bypass the limiter function for testing demRateRaw = self.demanded_angular_rate # 1) Rotate the copters rotation rates into the gimbals frame of reference # copterAngRate_G = transpose(DCMgimbal)*DCMcopter*copterAngRate copterAngRate_G = self.dcm.transposed( ) * self.vehicle.dcm * self.vehicle.gyro #print("copterAngRate_G ", copterAngRate_G) # 2) Subtract the copters body rates to obtain a copter relative rotational # rate vector (X,Y,Z) in gimbal sensor frame # relativeGimbalRate(X,Y,Z) = gimbalRateDemand - copterAngRate_G relativeGimbalRate = self.demanded_angular_rate - copterAngRate_G #print("relativeGimbalRate ", relativeGimbalRate) # calculate joint angles (euler312 order) # calculate copter -> gimbal rotation matrix rotmat_copter_gimbal = self.dcm.transposed() * self.vehicle.dcm self.joint_angles = Vector3( *rotmat_copter_gimbal.transposed().to_euler312()) #print("joint_angles ", self.joint_angles) # 4) For each of the three joints, calculate upper and lower rate limits # from the corresponding angle limits and current joint angles # # upperRatelimit = (jointAngle - lowerAngleLimit) * travelLimitGain # lowerRatelimit = (jointAngle - upperAngleLimit) * travelLimitGain # # travelLimitGain is equal to the inverse of the bump stop time constant and # should be set to something like 20 initially. If set too high it can cause # the rates to 'ring' when they the limiter is in force, particularly given # we are using a first order numerical integration. upperRatelimit = -(self.joint_angles - self.upper_joint_limits) * self.travelLimitGain lowerRatelimit = -(self.joint_angles - self.lower_joint_limits) * self.travelLimitGain # 5) Calculate the gimbal joint rates (roll, elevation, azimuth) # # gimbalJointRates(roll, elev, azimuth) = Matrix*relativeGimbalRate(X,Y,Z) # # where matrix = # # +- -+ # | cos(elevAngle), 0, sin(elevAngle) | # | | # | sin(elevAngle) tan(rollAngle), 1, -cos(elevAngle) tan(rollAngle) | # | | # | sin(elevAngle) cos(elevAngle) | # | - --------------, 0, -------------- | # | cos(rollAngle) cos(rollAngle) | # +- -+ rollAngle = self.joint_angles.x elevAngle = self.joint_angles.y matrix = Matrix3( Vector3(cos(elevAngle), 0, sin(elevAngle)), Vector3( sin(elevAngle) * tan(rollAngle), 1, -cos(elevAngle) * tan(rollAngle)), Vector3(-sin(elevAngle) / cos(rollAngle), 0, cos(elevAngle) / cos(rollAngle))) gimbalJointRates = matrix * relativeGimbalRate #print("lowerRatelimit ", lowerRatelimit) #print("upperRatelimit ", upperRatelimit) #print("gimbalJointRates ", gimbalJointRates) # 6) Apply the rate limits from 4) gimbalJointRates.x = constrain(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x) gimbalJointRates.y = constrain(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y) gimbalJointRates.z = constrain(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z) # 7) Convert the modified gimbal joint rates to body rates (still copter # relative) # relativeGimbalRate(X,Y,Z) = Matrix * gimbalJointRates(roll, elev, azimuth) # # where Matrix = # # +- -+ # | cos(elevAngle), 0, -cos(rollAngle) sin(elevAngle) | # | | # | 0, 1, sin(rollAngle) | # | | # | sin(elevAngle), 0, cos(elevAngle) cos(rollAngle) | # +- -+ matrix = Matrix3( Vector3(cos(elevAngle), 0, -cos(rollAngle) * sin(elevAngle)), Vector3(0, 1, sin(rollAngle)), Vector3(sin(elevAngle), 0, cos(elevAngle) * cos(rollAngle))) relativeGimbalRate = matrix * gimbalJointRates # 8) Add to the result from step 1) to obtain the demanded gimbal body rates # in an inertial frame of reference # demandedGimbalRatesInertial(X,Y,Z) = relativeGimbalRate(X,Y,Z) + copterAngRate_G demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G #if now - self.last_print_t >= 1.0: # print("demandedGimbalRatesInertial ", demandedGimbalRatesInertial) # for the moment we will set gyros equal to demanded_angular_rate self.gimbal_angular_rate = demRateRaw #demandedGimbalRatesInertial + self.true_gyro_bias - self.supplied_gyro_bias # update rotation of the gimbal self.dcm.rotate(self.gimbal_angular_rate * delta_t) self.dcm.normalize() # calculate copter -> gimbal rotation matrix rotmat_copter_gimbal = self.dcm.transposed() * self.vehicle.dcm # calculate joint angles (euler312 order) self.joint_angles = Vector3( *rotmat_copter_gimbal.transposed().to_euler312()) # update observed gyro self.gyro = self.gimbal_angular_rate + self.true_gyro_bias # update delta_angle (integrate) self.delta_angle += self.gyro * delta_t # calculate accel in gimbal body frame copter_accel_earth = self.vehicle.dcm * self.vehicle.accel_body accel = self.dcm.transposed() * copter_accel_earth # integrate velocity self.delta_velocity += accel * delta_t # see if we should do a report if now - self.last_report_t >= 1.0 / self.reporting_rate: self.send_report() self.last_report_t = now if now - self.last_print_t >= 1.0: self.last_print_t = now # calculate joint angles (euler312 order) Euler312 = Vector3(*self.dcm.to_euler312()) print("Euler Angles 312 %6.1f %6.1f %6.1f" % (degrees( Euler312.z), degrees(Euler312.x), degrees(Euler312.y)))
def toVec(magnitude, angle): v = Vector3(magnitude, 0, 0) m = Matrix3() m.from_euler(0, 0, angle) return m.transposed() * v
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 rotation2(AHRS2): '''return the current DCM rotation matrix''' r = Matrix3() r.from_euler(AHRS2.roll, AHRS2.pitch, AHRS2.yaw) return r
def rotation_df(ATT): '''return the current DCM rotation matrix''' r = Matrix3() r.from_euler(radians(ATT.Roll), radians(ATT.Pitch), radians(ATT.Yaw)) return r