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
