def __init__(self, frame="generic"):
Aircraft.__init__(self)
self.sim = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sim.connect(('127.0.0.1', 9002))
self.sim.setblocking(0)
self.accel_body = Vector3(0, 0, -self.gravity)
self.frame = frame
self.last_timestamp = 0
if self.frame.find("heli") != -1:
self.decode_servos = self.decode_servos_heli
else:
self.decode_servos = self.decode_servos_fixed_wing
def transform(self, v3):
"""
Calculates the vector transformed by this quaternion
:param v3: Vector3 to be transformed
:returns: transformed vector
"""
if isinstance(v3, Vector3):
t = super(Quaternion, self).transform([v3.x, v3.y, v3.z])
return Vector3(t[0], t[1], t[2])
elif len(v3) == 3:
return super(Quaternion, self).transform(v3)
else:
raise TypeError("param v3 is not a vector type")
def send_report(self):
'''send a report to the vehicle control code over MAVLink'''
from pymavlink import mavutil
if self.connection is None:
try:
self.connection = mavutil.mavlink_connection(self.mavlink_url,
retries=0)
except Exception as e:
return
print("Gimbal connected to %s" % self.mavlink_url)
# check for incoming control messages
while True:
m = self.connection.recv_match(
type=['GIMBAL_CONTROL', 'HEARTBEAT'], blocking=False)
if m is None:
break
if m.get_type() == 'GIMBAL_CONTROL':
self.demanded_angular_rate = Vector3(m.demanded_rate_x,
m.demanded_rate_y,
m.demanded_rate_z)
self.seen_gimbal_control = True
if m.get_type() == 'HEARTBEAT' and not self.seen_heartbeat:
self.seen_heartbeat = True
self.vehicle_component_id = m.get_srcComponent()
self.connection.mav.srcSystem = m.get_srcSystem()
self.connection.mav.srcComponent = mavutil.mavlink.MAV_COMP_ID_GIMBAL
print("Gimbal using srcSystem %u" %
self.connection.mav.srcSystem)
if self.seen_heartbeat:
now = time.time()
if now - self.last_heartbeat_t >= 1:
self.connection.mav.heartbeat_send(
mavutil.mavlink.MAV_TYPE_GIMBAL,
mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
self.last_heartbeat_t = now
if self.seen_heartbeat:
delta_time = now - self.last_report_t
self.last_report_t = now
self.connection.mav.gimbal_report_send(
self.connection.mav.srcSystem,
mavutil.mavlink.MAV_COMP_ID_GIMBAL, delta_time,
self.delta_angle.x, self.delta_angle.y, self.delta_angle.z,
self.delta_velocity.x, self.delta_velocity.y,
self.delta_velocity.z, self.joint_angles.x,
self.joint_angles.y, self.joint_angles.z)
self.delta_velocity.zero()
self.delta_angle.zero()
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
# open the log file
mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
data = []
mag = None
offsets = Vector3(0, 0, 0)
# now gather all the data
while True:
# get the next MAVLink message in the log
m = mlog.recv_match(condition=opts.condition)
if m is None:
break
if m.get_type() == "SENSOR_OFFSETS":
# update offsets that were used during this flight
offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
if m.get_type() == "RAW_IMU" and offsets != None:
# extract one mag vector, removing the offsets that were
# used during that flight to get the raw sensor values
mag = Vector3(m.xmag, m.ymag, m.zmag) - offsets
data.append(mag)
print("Extracted %u data points" % len(data))
print("Current offsets: %s" % offsets)
# run the fitting algorithm
ofs = offsets
ofs = Vector3(0, 0, 0)
for r in range(opts.repeat):
ofs = find_offsets(data, ofs)
print('Loop %u offsets %s' % (r, ofs))
sys.stdout.flush()
print("New offsets: %s" % ofs)
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.8 # m/s/s
self.accelerometer = Vector3(0, 0, -self.gravity)
self.wind = util.Wind('0,0,0')
def EarthRatesToBodyRates(dcm, earth_rates):
'''convert the angular velocities from earth frame to
body frame. Thanks to James Goppert for the formula
all inputs and outputs are in radians
returns a gyro vector in body frame, in rad/s
'''
from math import sin, cos
(phi, theta, psi) = dcm.to_euler()
phiDot = earth_rates.x
thetaDot = earth_rates.y
psiDot = earth_rates.z
p = phiDot - psiDot * sin(theta)
q = cos(phi) * thetaDot + sin(phi) * psiDot * cos(theta)
r = cos(phi) * psiDot * cos(theta) - sin(phi) * thetaDot
return Vector3(p, q, r)
def update_position(self, delta_time):
'''update lat/lon/alt from position'''
radius_of_earth = 6378100.0 # in meters
dlat = math.degrees(math.atan(self.position.x / radius_of_earth))
self.latitude = self.home_latitude + dlat
lon_scale = math.cos(math.radians(self.latitude))
dlon = math.degrees(math.atan(
self.position.y / radius_of_earth)) / lon_scale
self.longitude = self.home_longitude + dlon
self.altitude = self.home_altitude - self.position.z
velocity_body = self.dcm.transposed() * self.velocity
# force the acceleration to mostly be from gravity. We should be using 100% accel_body,
# but right now that flies very badly as the AHRS system can't do centripetal correction
# for multicopters. This is a compromise until we get that sorted out
accel_true = self.accel_body
accel_fake = self.dcm.transposed() * Vector3(0, 0, -self.gravity)
self.accelerometer = (accel_true * 0.5) + (accel_fake * 0.5)
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 BodyRatesToEarthRates(dcm, gyro):
'''convert the angular velocities from body frame to
earth frame.
all inputs and outputs are in radians/s
returns a earth rate vector
'''
from math import sin, cos, tan, fabs
p = gyro.x
q = gyro.y
r = gyro.z
(phi, theta, psi) = dcm.to_euler()
phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi))
thetaDot = q * cos(phi) - r * sin(phi)
if fabs(cos(theta)) < 1.0e-20:
theta += 1.0e-10
psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta)
return Vector3(phiDot, thetaDot, psiDot)
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
# open the log file
flog = open(filename, mode='r')
data = []
data_no_motors = []
mag = None
offsets = None
# now gather all the data
for line in flog:
if not line.startswith('COMPASS,'):
continue
line = line.rstrip()
line = line.replace(' ', '')
a = line.split(',')
ofs = Vector3(float(a[4]), float(a[5]), float(a[6]))
if offsets is None:
initial_offsets = ofs
offsets = ofs
motor_ofs = Vector3(float(a[7]), float(a[8]), float(a[9]))
mag = Vector3(float(a[1]), float(a[2]), float(a[3]))
mag = mag - offsets
data.append(mag)
data_no_motors.append(mag - motor_ofs)
print("Extracted %u data points" % len(data))
print("Current offsets: %s" % initial_offsets)
# run the fitting algorithm
ofs = initial_offsets
for r in range(opts.repeat):
ofs = find_offsets(data, ofs)
plot_corrected_field('plot.dat', data, ofs)
plot_corrected_field('initial.dat', data, initial_offsets)
plot_corrected_field('zero.dat', data, Vector3(0,0,0))
plot_corrected_field('hand.dat', data, Vector3(-25,-8,-2))
plot_corrected_field('zero-no-motors.dat', data_no_motors, Vector3(0,0,0))
print('Loop %u offsets %s' % (r, ofs))
sys.stdout.flush()
print("New offsets: %s" % ofs)
def drag(self, velocity, deltat=None, testing=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 )
def gps_velocity_old(GPS_RAW_INT):
'''return GPS velocity vector'''
return Vector3(
GPS_RAW_INT.vel * 0.01 * cos(radians(GPS_RAW_INT.cog * 0.01)),
GPS_RAW_INT.vel * 0.01 * sin(radians(GPS_RAW_INT.cog * 0.01)), 0)
def earth_accel(RAW_IMU, ATTITUDE):
'''return earth frame acceleration vector'''
r = rotation(ATTITUDE)
accel = Vector3(RAW_IMU.xacc, RAW_IMU.yacc, RAW_IMU.zacc) * 9.81 * 0.001
return r * accel
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 gps_velocity(GLOBAL_POSITION_INT):
'''return GPS velocity vector'''
return Vector3(GLOBAL_POSITION_INT.vx, GLOBAL_POSITION_INT.vy,
GLOBAL_POSITION_INT.vz) * 0.01
def update(self, servos):
for i in range(0, len(self.motors)):
servo = servos[self.motors[i].servo - 1]
if servo <= 0.0:
self.motor_speed[i] = 0
else:
self.motor_speed[i] = servo
m = self.motor_speed
# how much time has passed?
t = self.time_now
delta_time = t - self.last_time
self.last_time = t
# rotational acceleration, in rad/s/s, in body frame
rot_accel = Vector3(0, 0, 0)
thrust = 0.0
for i in range(len(self.motors)):
rot_accel.x += -radians(5000.0) * math.sin(
radians(self.motors[i].angle)) * m[i]
rot_accel.y += radians(5000.0) * math.cos(
radians(self.motors[i].angle)) * m[i]
if self.motors[i].clockwise:
rot_accel.z -= m[i] * radians(400.0)
else:
rot_accel.z += m[i] * radians(400.0)
thrust += m[i] * self.thrust_scale # newtons
# rotational air resistance
rot_accel.x -= self.gyro.x * radians(
5000.0) / self.terminal_rotation_rate
rot_accel.y -= self.gyro.y * radians(
5000.0) / self.terminal_rotation_rate
rot_accel.z -= self.gyro.z * radians(
400.0) / self.terminal_rotation_rate
# update rotational rates in body frame
self.gyro += rot_accel * delta_time
# update attitude
self.dcm.rotate(self.gyro * delta_time)
self.dcm.normalize()
# air resistance
air_resistance = -self.velocity * (self.gravity /
self.terminal_velocity)
accel_body = Vector3(0, 0, -thrust / self.mass)
accel_earth = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity)
accel_earth += air_resistance
# add in some wind (turn force into accel by dividing by mass).
# NOTE: disable this drag correction until we work out
# why it is blowing up
# accel_earth += self.wind.drag(self.velocity) / self.mass
# if we're on the ground, then our vertical acceleration is limited
# to zero. This effectively adds the force of the ground on the aircraft
if self.on_ground() and accel_earth.z > 0:
accel_earth.z = 0
# work out acceleration as seen by the accelerometers. It sees the kinematic
# acceleration (ie. real movement), plus gravity
self.accel_body = self.dcm.transposed() * (
accel_earth + Vector3(0, 0, -self.gravity))
# new velocity vector
self.velocity += accel_earth * delta_time
# new position vector
old_position = self.position.copy()
self.position += self.velocity * delta_time
# constrain height to the ground
if self.on_ground():
if not self.on_ground(old_position):
print("Hit ground at %f m/s" % (self.velocity.z))
self.velocity = Vector3(0, 0, 0)
# zero roll/pitch, but keep yaw
(r, p, y) = self.dcm.to_euler()
self.dcm.from_euler(0, 0, y)
self.position = Vector3(
self.position.x, self.position.y,
-(self.ground_level + self.frame_height - self.home_altitude))
# update lat/lon/altitude
self.update_position(delta_time)
def __init__(self, vehicle):
# vehicle is the vehicle (usually a multicopter) that the gimbal is attached to
self.vehicle = vehicle
# URL of SITL 2nd telem port
self.mavlink_url = 'tcp:127.0.0.1:5762'
# rotation matrix (gimbal body -> earth)
self.dcm = None
# time of last update
self.last_update_t = time.time()
# true angular rate of gimbal in body frame (rad/s)
self.gimbal_angular_rate = Vector3()
# observed angular rate (including biases)
self.gyro = Vector3()
# joint angles, in radians. in yaw/roll/pitch order. Relative to fwd.
# So 0,0,0 points forward.
# Pi/2,0,0 means pointing right
# 0, Pi/2, 0 means pointing fwd, but rolled 90 degrees to right
# 0, 0, -Pi/2, means pointing down
self.joint_angles = Vector3()
# physical constraints on joint angles in (roll, pitch, azimuth) order
self.lower_joint_limits = Vector3(radians(-40), radians(-135),
radians(-7.5))
self.upper_joint_limits = Vector3(radians(40), radians(45),
radians(7.5))
self.travelLimitGain = 20
# true gyro bias
self.true_gyro_bias = Vector3()
##################################
# reporting variables. gimbal pushes these to vehicle code over MAVLink at approx 100Hz
# reporting rate in Hz
self.reporting_rate = 100
# integral of gyro vector over last time interval. In radians
self.delta_angle = Vector3()
# integral of accel vector over last time interval. In m/s
self.delta_velocity = Vector3()
# we also push joint_angles
##################################
# control variables from the vehicle
# angular rate in rad/s. In body frame of gimbal
self.demanded_angular_rate = Vector3(radians(0), radians(0),
radians(0))
# gyro bias provided by EKF on vehicle. In rad/s.
# Should be subtracted from the gyro readings to get true body rotatation rates
self.supplied_gyro_bias = Vector3()
###################################
# communication variables
self.connection = None
self.last_report_t = time.time()
self.last_heartbeat_t = time.time()
self.seen_heartbeat = False
self.seen_gimbal_control = False
self.last_print_t = time.time()
self.vehicle_component_id = None
self.last_report_t = time.time()
#!/usr/bin/env python
# simple test of wind generation code
import util, time, random
from rotmat import Vector3
wind = util.Wind('7,90,0.1')
t0 = time.time()
velocity = Vector3(0, 0, 0)
t = 0
deltat = 0.01
while t < 60:
print("%.4f %f" % (t, wind.drag(velocity, deltat=deltat).length()))
t += deltat
def toVec(magnitude, angle):
v = Vector3(magnitude, 0, 0)
m = Matrix3()
m.from_euler(0, 0, angle)
return m.transposed() * v
def gps_velocity_df(GPS):
'''return GPS velocity vector'''
vx = GPS.Spd * cos(radians(GPS.GCrs))
vy = GPS.Spd * sin(radians(GPS.GCrs))
return Vector3(vx, vy, GPS.VZ)
def earth_accel_df(IMU, ATT):
'''return earth frame acceleration vector from df log'''
r = rotation_df(ATT)
accel = Vector3(IMU.AccX, IMU.AccY, IMU.AccZ)
return r * accel
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)
motor_ofs = Vector3(0, 0, 0)
motor = 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() == "PARAM_VALUE" and m.param_id == "RC3_MIN":
rc3_min = float(m.param_value)
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() == "SERVO_OUTPUT_RAW":
motor_pwm = m.servo1_raw + m.servo2_raw + m.servo3_raw + m.servo4_raw
motor_pwm *= 0.25
rc3_min = mlog.param('RC3_MIN', 1100)
rc3_max = mlog.param('RC3_MAX', 1900)
motor = (motor_pwm - rc3_min) / (rc3_max - rc3_min)
if motor > 1.0:
motor = 1.0
if motor < 0.0:
motor = 0.0
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(), motor))
print("Extracted %u data points" % len(data))
print("Current offsets: %s" % offsets)
data = select_data(data)
# do an initial fit with all data
(offsets, motor_ofs, 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, motor_ofs))
print("Fit %u : %s %s field_strength=%6.1f to %6.1f" %
(count, offsets, motor_ofs, radius(data[0], offsets, motor_ofs),
radius(data[-1], offsets, motor_ofs)))
# discard outliers, keep the middle 3/4
data = data[len(data) / 8:-len(data) / 8]
# fit again
(offsets, motor_ofs, field_strength) = fit_data(data)
print("Final : %s %s field_strength=%6.1f to %6.1f" %
(offsets, motor_ofs, radius(data[0], offsets, motor_ofs),
radius(data[-1], offsets, motor_ofs)))
print "mavgraph.py '%s' 'mag_field(RAW_IMU)' 'mag_field_motors(RAW_IMU,SENSOR_OFFSETS,(%f,%f,%f),SERVO_OUTPUT_RAW,(%f,%f,%f))'" % (
filename, offsets.x, offsets.y, offsets.z, motor_ofs.x, motor_ofs.y,
motor_ofs.z)
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 earth_gyro(RAW_IMU, ATTITUDE):
'''return earth frame gyro vector'''
r = rotation(ATTITUDE)
accel = Vector3(degrees(RAW_IMU.xgyro), degrees(RAW_IMU.ygyro),
degrees(RAW_IMU.zgyro)) * 0.001
return r * accel
def gps_velocity_body(GPS_RAW_INT, ATTITUDE):
'''return GPS velocity vector in body frame'''
r = rotation(ATTITUDE)
return r.transposed() * Vector3(GPS_RAW_INT.vel*0.01*cos(radians(GPS_RAW_INT.cog*0.01)),
GPS_RAW_INT.vel*0.01*sin(radians(GPS_RAW_INT.cog*0.01)),
-tan(ATTITUDE.pitch)*GPS_RAW_INT.vel*0.01)
def update(self, servos):
# how much time has passed?
t = time.time()
delta_time = t - self.last_time
self.last_time = t
# get swash proportions (these are from 0 to 1)
swash1 = servos[0]
swash2 = servos[1]
swash3 = servos[2]
tail_rotor = servos[3]
rsc = servos[7]
# get total thrust from 0 to 1
thrust = (rsc / self.rsc_setpoint) * (swash1 + swash2 + swash3) / 3.0
# very simplistic mapping to body euler rates
roll_rate = swash1 - swash2
pitch_rate = (swash1 + swash2) / 2.0 - swash3
yaw_rate = tail_rotor - 0.5
#print(swash1, swash2, swash3, roll_rate, pitch_rate, yaw_rate, thrust, rsc)
roll_rate *= rsc / self.rsc_setpoint
pitch_rate *= rsc / self.rsc_setpoint
yaw_rate *= rsc / self.rsc_setpoint
rot_accel = Vector3(roll_rate * self.roll_rate_max,
pitch_rate * self.pitch_rate_max,
yaw_rate * self.yaw_rate_max)
# rotational air resistance
rot_accel.x -= self.gyro.x * radians(
5000.0) / self.terminal_rotation_rate
rot_accel.y -= self.gyro.y * radians(
5000.0) / self.terminal_rotation_rate
rot_accel.z -= self.gyro.z * radians(
5000.0) / self.terminal_rotation_rate
# update rotational rates in body frame
self.gyro += rot_accel * delta_time
# update attitude
self.dcm.rotate(self.gyro * delta_time)
self.dcm.normalize()
# air resistance
air_resistance = -self.velocity * (self.gravity /
self.terminal_velocity)
thrust *= self.thrust_scale
accel_body = Vector3(0, 0, -thrust / self.mass)
accel_earth = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity)
accel_earth += air_resistance
# if we're on the ground, then our vertical acceleration is limited
# to zero. This effectively adds the force of the ground on the aircraft
if self.on_ground() and accel_earth.z > 0:
accel_earth.z = 0
# work out acceleration as seen by the accelerometers. It sees the kinematic
# acceleration (ie. real movement), plus gravity
self.accel_body = self.dcm.transposed() * (
accel_earth + Vector3(0, 0, -self.gravity))
# new velocity vector
self.velocity += accel_earth * delta_time
# new position vector
old_position = self.position.copy()
self.position += self.velocity * delta_time
# constrain height to the ground
if self.on_ground():
if not self.on_ground(old_position):
print("Hit ground at %f m/s" % (self.velocity.z))
self.velocity = Vector3(0, 0, 0)
# zero roll/pitch, but keep yaw
(r, p, y) = self.dcm.to_euler()
self.dcm.from_euler(0, 0, y)
self.position = Vector3(
self.position.x, self.position.y,
-(self.ground_level + self.frame_height - self.home_altitude))
# update lat/lon/altitude
self.update_position()
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 update(self, servos):
# if in skid steering mode the steering and throttle values are used for motor1 and motor2
if self.skid_steering:
motor1 = 2 * (servos[0] - 0.5)
motor2 = 2 * (servos[2] - 0.5)
steering = motor1 - motor2
throttle = 0.5 * (motor1 + motor2)
else:
steering = 2 * (servos[0] - 0.5)
throttle = 2 * (servos[2] - 0.5)
# how much time has passed?
t = self.time_now
delta_time = t - self.last_time
self.last_time = t
# speed in m/s in body frame
velocity_body = self.dcm.transposed() * self.velocity
# speed along x axis, +ve is forward
speed = velocity_body.x
# yaw rate in degrees/s
yaw_rate = self.yaw_rate(steering, speed)
# target speed with current throttle
target_speed = throttle * self.max_speed
# linear acceleration in m/s/s - very crude model
accel = self.max_accel * (target_speed - speed) / self.max_speed
# print('speed=%f throttle=%f steering=%f yaw_rate=%f accel=%f' % (speed, state.throttle, state.steering, yaw_rate, accel))
self.gyro = Vector3(0, 0, radians(yaw_rate))
# update attitude
self.dcm.rotate(self.gyro * delta_time)
self.dcm.normalize()
# accel in body frame due to motor
accel_body = Vector3(accel, 0, 0)
# add in accel due to direction change
accel_body.y += radians(yaw_rate) * speed
# now in earth frame
accel_earth = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity)
# if we're on the ground, then our vertical acceleration is limited
# to zero. This effectively adds the force of the ground on the aircraft
accel_earth.z = 0
# work out acceleration as seen by the accelerometers. It sees the kinematic
# acceleration (ie. real movement), plus gravity
self.accel_body = self.dcm.transposed() * (
accel_earth + Vector3(0, 0, -self.gravity))
# new velocity vector
self.velocity += accel_earth * delta_time
# new position vector
old_position = self.position.copy()
self.position += self.velocity * delta_time
# update lat/lon/altitude
self.update_position()
def noise():
'''a noise vector'''
from random import gauss
v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
v.normalize()
return v * opts.noise
def pos_cb(self, msg):
self.velocity = Vector3(msg.vx, msg.vy, msg.vz)
self.position = Vector3(msg.x, msg.y, msg.z)
self.have_new_pos = True