def gimbal_controller(dcm_estimated, dcm_demanded, chan1):
'''return the delta to chan1 and chan2'''
global PITCH_SCALE, YAW_SCALE
quat_est = Quaternion(dcm_estimated)
quat_dem = Quaternion(dcm_demanded)
quatErr = quat_division(quat_dem, quat_est)
if quatErr[0] >= 0.0:
scaler = 2.0
else:
scaler = -2.0
bf_rates = scaler * Vector3(quatErr[1], quatErr[2], quatErr[3])
yaw_joint_rad = radians((chan1 - ROTATIONS['level'].chan1) * YAW_SCALE)
chan1_change = degrees(bf_rates.z) * YAW_SCALE
chan2_change = degrees(bf_rates.x * sin(yaw_joint_rad) +
bf_rates.y * cos(yaw_joint_rad)) / PITCH_SCALE
return (chan1_change, chan2_change)
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 magfit(logfile):
'''find best magnetometer rotation fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
# generate 90 degree rotations
rotations = generate_rotations()
print("Generated %u rotations" % len(rotations))
count = 0
total_error = [0] * len(rotations)
attitude = None
gps = None
# now gather all the data
while True:
m = mlog.recv_match()
if m is None:
break
if m.get_type() == "ATTITUDE":
attitude = m
if m.get_type() == "GPS_RAW_INT":
gps = m
if m.get_type() == "RAW_IMU":
mag = Vector3(m.xmag, m.ymag, m.zmag)
if attitude is not None and gps is not None and gps.vel > args.min_speed * 100 and gps.fix_type >= 3:
add_errors(mag, attitude, total_error, rotations)
count += 1
best_i = 0
best_err = total_error[0]
for i in range(len(rotations)):
r = rotations[i]
print("(%u,%u,%u) err=%.2f" %
(r.roll, r.pitch, r.yaw, total_error[i] / count))
if total_error[i] < best_err:
best_i = i
best_err = total_error[i]
r = rotations[best_i]
print("Best rotation (%u,%u,%u) err=%.2f" %
(r.roll, r.pitch, r.yaw, best_err / count))
def fit_WWW():
from scipy import optimize
c = copy.copy(old_corrections)
p = [c.offsets.x, c.offsets.y, c.offsets.z, c.scaling]
if args.elliptical:
p.extend([
c.diag.x, c.diag.y, c.diag.z, c.offdiag.x, c.offdiag.y, c.offdiag.z
])
if args.cmot:
p.extend([c.cmot.x, c.cmot.y, c.cmot.z])
ofs = args.max_offset
bounds = [(-ofs, ofs), (-ofs, ofs), (-ofs, ofs),
(args.min_scale, args.max_scale)]
if args.elliptical:
for i in range(3):
bounds.append((args.min_diag, args.max_diag))
for i in range(3):
bounds.append((args.min_offdiag, args.max_offdiag))
if args.cmot:
for i in range(3):
bounds.append((-args.max_cmot, args.max_cmot))
p = optimize.fmin_slsqp(wmm_error, p, bounds=bounds)
p = list(p)
c.offsets = Vector3(p.pop(0), p.pop(0), p.pop(0))
c.scaling = p.pop(0)
if args.elliptical:
c.diag = Vector3(p.pop(0), p.pop(0), p.pop(0))
c.offdiag = Vector3(p.pop(0), p.pop(0), p.pop(0))
else:
c.diag = Vector3(1.0, 1.0, 1.0)
c.offdiag = Vector3(0.0, 0.0, 0.0)
if args.cmot:
c.cmot = Vector3(p.pop(0), p.pop(0), p.pop(0))
else:
c.cmot = Vector3(0.0, 0.0, 0.0)
return c
def mag_error(p, data):
cx, cy, cz, r = p
errout = 0
# cruse bounds mechanism
if r < 0.3:
errout = (0.3 - r) * 100
r = 0.3
if r > 2.0:
errout = (r - 2.0) * 100
r = 2.0
corr = Vector3(cx, cy, cz)
(baseline_field, baseline_throttle) = data[0]
ret = []
for d in data:
(mag, throttle) = d
cmag = mag + corr * math.pow(throttle, r)
err = (cmag - baseline_field).length()
err += errout
ret.append(err)
return ret
def get_vicon_pose(self, object_name, segment_name):
# get position in mm. Coordinates are in NED
vicon_pos = self.vicon.get_segment_global_translation(
object_name, segment_name)
if vicon_pos is None:
# Object is not in view
return None, None, None, None
vicon_quat = Quaternion(
self.vicon.get_segment_global_quaternion(object_name,
segment_name))
pos_ned = Vector3(vicon_pos * 0.001)
euler = vicon_quat.euler
roll, pitch, yaw = euler[0], euler[1], euler[2]
yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))
return pos_ned, roll, pitch, yaw
def update_arrows(self):
m = self.q.to_rotation_matrix().transposed()
sl = 1.1 if self.reference else 1.0
for i in self.xarrows:
i.axis = vpy_vec(m * Vector3(sl, 0, 0))
i.up = vpy_vec(m * Vector3(0, 1, 0))
for i in self.yarrows:
i.axis = vpy_vec(m * Vector3(0, sl, 0))
i.up = vpy_vec(m * Vector3(1, 0, 0))
for i in self.zarrows:
i.axis = vpy_vec(m * Vector3(0, 0, sl))
i.up = vpy_vec(m * Vector3(1, 0, 0))
for i in self.labels:
i.xoffset = scene.width * 0.07
i.yoffset = scene.width * 0.1
def velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, shift=0):
'''return summed velocity error'''
sum = 0
count = 0
for i in range(0, len(vel)-1):
dv = vel[i+1] - vel[i]
da = Vector3()
for idx in range(1+accel_indexes[i]-shift, 1+accel_indexes[i+1]-shift):
da += gaccel[idx]
dt1 = timestamps[i+1] - timestamps[i]
dt2 = (accel_indexes[i+1] - accel_indexes[i]) * imu_dt
da *= imu_dt
da *= dt1/dt2
#print(accel_indexes[i+1] - accel_indexes[i])
ex = abs(dv.x - da.x)
ey = abs(dv.y - da.y)
sum += 0.5*(ex+ey)
count += 1
if count == 0:
return None
return sum/count
def CalcRotationVector(self, dx, dy):
angle = math.degrees(math.atan2(dy, dx))
# Make angle discrete on multiples of 45 degrees
angle = angle + 22.5 - (angle + 22.5) % 45
if angle % 90 == 45:
x, y = 1, 1
elif (angle / 90) % 2 == 0:
x, y = 1, 0
else:
x, y = 0, 1
if abs(angle) > 90:
x *= -1
if angle < 0:
y *= -1
# NED coordinates from the camera point of view
dx, dy, dz = 0, x, y
# Get rotation axis by rotating the moving vector -90 degrees on x axis
self.rotation_vector = Vector3(dx, dz, -dy)
def add_arrows(self, arrowpos=Vector3(0, 0, 0), labeltext=None):
if labeltext is not None:
self.labels.append(label(pos=vpy_vec(arrowpos), text=labeltext))
sw = .005 if self.reference else .05
self.xarrows.append(
arrow(pos=vpy_vec(arrowpos),
color=color.red,
opacity=1,
shaftwidth=sw))
self.yarrows.append(
arrow(pos=vpy_vec(arrowpos),
color=color.green,
opacity=1,
shaftwidth=sw))
self.zarrows.append(
arrow(pos=vpy_vec(arrowpos),
color=color.blue,
opacity=1,
shaftwidth=sw))
self.update_arrows()
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 magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
data = []
throttle = 0
# now gather all the data
while True:
m = mlog.recv_match(condition=args.condition)
if m is None:
break
if m.get_type() == "MAG":
mag = Vector3(m.MagX, m.MagY, m.MagZ)
data.append((mag, throttle))
#if m.get_type() == "RCOU":
# throttle = math.sqrt(m.C1/500.0)
if m.get_type() == "CTUN":
throttle = m.ThO
print("Extracted %u data points" % len(data))
(cmot, r) = fit_data(data)
print("Fit : %s r: %s" % (cmot, r))
x = range(len(data))
errors = mag_error((cmot.x, cmot.y, cmot.z, r), data)
import matplotlib.pyplot as plt
plt.plot(x, errors, 'bo-')
x1, x2, y1, y2 = plt.axis()
plt.axis((x1, x2, 0, y2))
plt.ylabel('Error')
plt.xlabel('Sample')
plt.show()
def OnActuationTimer(self, evt):
if not self.renderer.vehicle:
return
dt = evt.GetInterval() * .001
axis, speed = self.angvel
angle = speed * dt
self.renderer.vehicle.model.rotate(axis, angle)
if self.actuation_state == 'attitude':
diff = self.desired_quaternion / self.renderer.vehicle.model.quaternion
diff = quaternion_to_axis_angle(diff)
angle = diff.length()
if abs(angle) <= 0.001:
self.renderer.vehicle.model.quaternion = self.desired_quaternion
self.StopActuation()
if self.attitude_callback:
self.attitude_callback()
self.attitude_callback = None
else:
speed = angle / dt
self.angvel = Vector3(diff.x, diff.y, diff.z), speed * .3
self.Refresh()
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)
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
data = []
last_t = 0
offsets = Vector3(0,0,0)
# now gather all the data
while True:
m = mlog.recv_match(condition=args.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())
if m.get_type() == "MAG" and not args.mag2:
offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
mag = Vector3(m.MagX,m.MagY,m.MagZ)
data.append(mag - offsets + noise())
if m.get_type() == "MAG2" and args.mag2:
offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
mag = Vector3(m.MagX,m.MagY,m.MagZ)
data.append(mag - offsets + noise())
print("Extracted %u data points" % len(data))
print("Current offsets: %s" % offsets)
orig_data = data
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)))
if args.plot:
plot_data(orig_data, data)
def SetMag(self, x, y, z):
self.mag = Vector3(x, y, z)
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=args.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=args.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 noise():
'''a noise vector'''
from random import gauss
v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
v.normalize()
return v * args.noise
def magfit(logfile):
'''find best magnetometer rotation fit to a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
last_mag = None
last_usec = 0
count = 0
total_error = [0] * len(rotations)
AHRS_ORIENTATION = 0
COMPASS_ORIENT = 0
COMPASS_EXTERNAL = 0
last_gyr = None
# now gather all the data
while True:
m = mlog.recv_match()
if m is None:
break
if m.get_type() in ["PARAM_VALUE", "PARM"]:
if m.get_type() == "PARM":
name = str(m.Name)
value = int(m.Value)
else:
name = str(m.param_id)
value = int(m.param_value)
if name == "AHRS_ORIENTATION":
AHRS_ORIENTATION = value
if name == 'COMPASS_ORIENT':
COMPASS_ORIENT = value
if name == 'COMPASS_EXTERNAL':
COMPASS_EXTERNAL = value
if m.get_type() in ["RAW_IMU", "MAG", "IMU"]:
if m.get_type() == "RAW_IMU":
mag = Vector3(m.xmag, m.ymag, m.zmag)
gyr = Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001
usec = m.time_usec
elif m.get_type() == "IMU":
last_gyr = Vector3(m.GyrX, m.GyrY, m.GyrZ)
continue
elif last_gyr is not None:
mag = Vector3(m.MagX, m.MagY, m.MagZ)
gyr = last_gyr
usec = m.TimeMS * 1000
mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT,
COMPASS_EXTERNAL)
if last_mag is not None and gyr.length() > radians(
args.min_rotation):
add_errors(mag, gyr, last_mag, (usec - last_usec) * 1.0e-6,
total_error, rotations)
count += 1
last_mag = mag
last_usec = usec
best_i = 0
best_err = total_error[0]
for i in range(len(rotations)):
r = rotations[i]
if not r.is_90_degrees():
continue
if args.verbose:
print("%s err=%.2f" % (r, total_error[i] / count))
if total_error[i] < best_err:
best_i = i
best_err = total_error[i]
r = rotations[best_i]
print(
"Current rotation is AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=%u"
% (rotations[AHRS_ORIENTATION], rotations[COMPASS_ORIENT],
COMPASS_EXTERNAL))
print("Best rotation is %s err=%.2f from %u points" %
(r, best_err / count, count))
print(
"Please set AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=1" %
(rotations[AHRS_ORIENTATION], r))
def add_noise(self, throttle):
"""Add noise based on throttle level (from 0..1)."""
self.gyro += Vector3(random.gauss(0, 1), random.gauss(0, 1),
random.gauss(0, 1)) * throttle * self.gyro_noise
self.accel_body += Vector3(random.gauss(0, 1), random.gauss(
0, 1), random.gauss(0, 1)) * throttle * self.accel_noise
def __init__(self):
self.offsets = Vector3(0.0, 0.0, 0.0)
self.diag = Vector3(1.0, 1.0, 1.0)
self.offdiag = Vector3(0.0, 0.0, 0.0)
self.cmot = Vector3(0.0, 0.0, 0.0)
self.scaling = 1.0
def magfit(logfile):
'''find best magnetometer offset fit to a log file'''
print("Processing log %s" % logfile)
mlog = mavutil.mavlink_connection(logfile)
global earth_field, declination
global data
data = []
ATT = None
BAT = None
mag_msg = 'MAG%s' % mag_idx
count = 0
parameters = {}
# get parameters
while True:
msg = mlog.recv_match(type=['PARM'])
if msg is None:
break
parameters[msg.Name] = msg.Value
mlog.rewind()
# extract MAG data
while True:
msg = mlog.recv_match(
type=['GPS', mag_msg, 'ATT', 'CTUN', 'BARO', 'BAT'],
condition=args.condition)
if msg is None:
break
if msg.get_type() == 'GPS' and msg.Status >= 3 and earth_field is None:
earth_field = mavextra.expected_earth_field(msg)
(declination, inclination,
intensity) = mavextra.get_mag_field_ef(msg.Lat, msg.Lng)
print("Earth field: %s strength %.0f declination %.1f degrees" %
(earth_field, earth_field.length(), declination))
if msg.get_type() == 'ATT':
ATT = msg
if msg.get_type() == 'BAT':
BAT = msg
if msg.get_type() == mag_msg and ATT is not None:
if count % args.reduce == 0:
data.append((msg, ATT, BAT))
count += 1
old_corrections.offsets = Vector3(
parameters.get('COMPASS_OFS%s_X' % mag_idx, 0.0),
parameters.get('COMPASS_OFS%s_Y' % mag_idx, 0.0),
parameters.get('COMPASS_OFS%s_Z' % mag_idx, 0.0))
old_corrections.diag = Vector3(
parameters.get('COMPASS_DIA%s_X' % mag_idx, 1.0),
parameters.get('COMPASS_DIA%s_Y' % mag_idx, 1.0),
parameters.get('COMPASS_DIA%s_Z' % mag_idx, 1.0))
old_corrections.offdiag = Vector3(
parameters.get('COMPASS_ODI%s_X' % mag_idx, 0.0),
parameters.get('COMPASS_ODI%s_Y' % mag_idx, 0.0),
parameters.get('COMPASS_ODI%s_Z' % mag_idx, 0.0))
if parameters.get('COMPASS_MOTCT', 0) == 2:
# only support current based corrections for now
old_corrections.cmot = Vector3(
parameters.get('COMPASS_MOT%s_X' % mag_idx, 0.0),
parameters.get('COMPASS_MOT%s_Y' % mag_idx, 0.0),
parameters.get('COMPASS_MOT%s_Z' % mag_idx, 0.0))
old_corrections.scaling = parameters.get('COMPASS_SCALE%s' % mag_idx, None)
if old_corrections.scaling is None or old_corrections.scaling < 0.1:
force_scale = False
old_corrections.scaling = 1.0
else:
force_scale = True
# remove existing corrections
data2 = []
for (MAG, ATT, BAT) in data:
if remove_offsets(MAG, BAT, old_corrections):
data2.append((MAG, ATT, BAT))
data = data2
print("Extracted %u points" % len(data))
print("Current: %s diag: %s offdiag: %s cmot: %s scale: %.2f" %
(old_corrections.offsets, old_corrections.diag,
old_corrections.offdiag, old_corrections.cmot,
old_corrections.scaling))
# do fit
c = fit_WWW()
# normalise diagonals to scale factor
if force_scale:
avgdiag = (c.diag.x + c.diag.y + c.diag.z) / 3.0
calc_scale = c.scaling
c.scaling *= avgdiag
if c.scaling > args.max_scale:
c.scaling = args.max_scale
if c.scaling < args.min_scale:
c.scaling = args.min_scale
scale_change = c.scaling / calc_scale
c.diag *= 1.0 / scale_change
c.offdiag *= 1.0 / scale_change
print("New: %s diag: %s offdiag: %s cmot: %s scale: %.2f" %
(c.offsets, c.diag, c.offdiag, c.cmot, c.scaling))
x = []
corrected = {}
corrected['Yaw'] = []
expected1 = {}
expected2 = {}
uncorrected = {}
uncorrected['Yaw'] = []
yaw_change1 = []
yaw_change2 = []
for i in range(len(data)):
(MAG, ATT, BAT) = data[i]
yaw1 = get_yaw(ATT, MAG, BAT, c)
corrected['Yaw'].append(yaw1)
ef1 = expected_field(ATT, yaw1)
cf = correct(MAG, BAT, c)
yaw2 = get_yaw(ATT, MAG, BAT, old_corrections)
ef2 = expected_field(ATT, yaw2)
uncorrected['Yaw'].append(yaw2)
uf = correct(MAG, BAT, old_corrections)
yaw_change1.append(mavextra.wrap_180(yaw1 - yaw2))
yaw_change2.append(mavextra.wrap_180(yaw1 - ATT.Yaw))
for axis in ['x', 'y', 'z']:
if not axis in corrected:
corrected[axis] = []
uncorrected[axis] = []
expected1[axis] = []
expected2[axis] = []
corrected[axis].append(getattr(cf, axis))
uncorrected[axis].append(getattr(uf, axis))
expected1[axis].append(getattr(ef1, axis))
expected2[axis].append(getattr(ef2, axis))
x.append(i)
c.show_parms()
fig, axs = pyplot.subplots(3, 1, sharex=True)
for axis in ['x', 'y', 'z']:
axs[0].plot(numpy.array(x),
numpy.array(uncorrected[axis]),
label='Uncorrected %s' % axis.upper())
axs[0].plot(numpy.array(x),
numpy.array(expected2[axis]),
label='Expected %s' % axis.upper())
axs[0].legend(loc='upper left')
axs[0].set_title('Original')
axs[0].set_ylabel('Field (mGauss)')
axs[1].plot(numpy.array(x),
numpy.array(corrected[axis]),
label='Corrected %s' % axis.upper())
axs[1].plot(numpy.array(x),
numpy.array(expected1[axis]),
label='Expected %s' % axis.upper())
axs[1].legend(loc='upper left')
axs[1].set_title('Corrected')
axs[1].set_ylabel('Field (mGauss)')
# show change in yaw estimate from old corrections to new
axs[2].plot(numpy.array(x),
numpy.array(yaw_change1),
label='Mag Yaw Change')
axs[2].plot(numpy.array(x),
numpy.array(yaw_change2),
label='ATT Yaw Change')
axs[2].set_title('Yaw Change (degrees)')
axs[2].legend(loc='upper left')
pyplot.show()
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 gps_lag(logfile):
'''work out gps velocity lag times for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
timestamps = []
vel = []
gaccel = []
accel_indexes = []
ATT = None
IMU = None
dtsum = 0
dtcount = 0
while True:
m = mlog.recv_match(type=['GPS','IMU','ATT'])
if m is None:
break
t = m.get_type()
if t == 'GPS' and m.Status >= 3 and m.Spd>args.minspeed:
v = Vector3(m.Spd*cos(radians(m.GCrs)), m.Spd*sin(radians(m.GCrs)), m.VZ)
vel.append(v)
timestamps.append(m._timestamp)
accel_indexes.append(max(len(gaccel)-1,0))
elif t == 'ATT':
ATT = m
elif t == 'IMU':
if ATT is not None:
gaccel.append(earth_accel_df(m, ATT))
if IMU is not None:
dt = m._timestamp - IMU._timestamp
dtsum += dt
dtcount += 1
IMU = m
imu_dt = dtsum / dtcount
print("Loaded %u samples imu_dt=%.3f" % (len(vel), imu_dt))
besti = -1
besterr = 0
delays = []
errors = []
for i in range(0,100):
err = velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, shift=i)
if err is None:
break
errors.append(err)
delays.append(i*imu_dt)
if besti == -1 or err < besterr:
besti = i
besterr = err
print("Best %u (%.3fs) %f" % (besti, besti*imu_dt, besterr))
if args.plot:
import matplotlib.pyplot as plt
plt.plot(delays, errors, 'bo-')
x1,x2,y1,y2 = plt.axis()
plt.axis((x1,x2,0,y2))
plt.ylabel('Error')
plt.xlabel('Delay(s)')
plt.show()
def SetEuler(self, roll, pitch, yaw, callback=None):
self.desired_quaternion = Quaternion((roll, pitch, yaw))
self.attitude_callback = callback
self.actuation_state = 'attitude'
self.angvel = Vector3(1, 0, 0), 0
self.StartActuation()
def IMUfit(logfile):
'''find IMU calibration parameters from a log file'''
print("Processing log %s" % logfile)
mlog = mavutil.mavlink_connection(logfile)
data = IMUData()
c = Coefficients()
orientation = 0
stop_capture = [False] * 3
if args.tclr:
messages = ['PARM', 'TCLR']
else:
messages = ['PARM', 'IMU']
while True:
msg = mlog.recv_match(type=messages)
if msg is None:
break
if msg.get_type() == 'PARM':
# build up the old coefficients so we can remove the impact of
# existing coefficients from the data
m = re.match("^INS_TCAL(\d)_ENABLE$", msg.Name)
if m:
imu = int(m.group(1)) - 1
if stop_capture[imu]:
continue
if msg.Value == 1 and c.enable[imu] == 2:
print("TCAL[%u] enabled" % imu)
stop_capture[imu] = True
continue
if msg.Value == 0 and c.enable[imu] == 1:
print("TCAL[%u] disabled" % imu)
stop_capture[imu] = True
continue
c.set_enable(imu, msg.Value)
m = re.match("^INS_TCAL(\d)_(ACC|GYR)([1-3])_([XYZ])$", msg.Name)
if m:
imu = int(m.group(1)) - 1
stype = m.group(2)
p = int(m.group(3))
axis = m.group(4)
if stop_capture[imu]:
continue
if stype == 'ACC':
c.set_acoeff(imu, axis, p, msg.Value / SCALE_FACTOR)
if stype == 'GYR':
c.set_gcoeff(imu, axis, p, msg.Value / SCALE_FACTOR)
m = re.match("^INS_TCAL(\d)_TMIN$", msg.Name)
if m:
imu = int(m.group(1)) - 1
if stop_capture[imu]:
continue
c.set_tmin(imu, msg.Value)
m = re.match("^INS_TCAL(\d)_TMAX", msg.Name)
if m:
imu = int(m.group(1)) - 1
if stop_capture[imu]:
continue
c.set_tmax(imu, msg.Value)
m = re.match("^INS_GYR(\d)_CALTEMP", msg.Name)
if m:
imu = int(m.group(1)) - 1
if stop_capture[imu]:
continue
c.set_gyro_tcal(imu, msg.Value)
m = re.match("^INS_ACC(\d)_CALTEMP", msg.Name)
if m:
imu = int(m.group(1)) - 1
if stop_capture[imu]:
continue
c.set_accel_tcal(imu, msg.Value)
m = re.match("^INS_(ACC|GYR)(\d?)OFFS_([XYZ])$", msg.Name)
if m:
stype = m.group(1)
if m.group(2) == "":
imu = 0
else:
imu = int(m.group(2)) - 1
axis = m.group(3)
if stop_capture[imu]:
continue
if stype == 'ACC':
c.set_aoffset(imu, axis, msg.Value)
if stype == 'GYR':
c.set_goffset(imu, axis, msg.Value)
if msg.Name == 'AHRS_ORIENTATION':
orientation = int(msg.Value)
print("Using orientation %d" % orientation)
if msg.get_type() == 'TCLR' and args.tclr:
imu = msg.I
T = msg.Temp
if msg.SType == 0:
# accel
acc = Vector3(msg.X, msg.Y, msg.Z)
time = msg.TimeUS * 1.0e-6
data.add_accel(imu, T, time, acc)
elif msg.SType == 1:
# gyro
gyr = Vector3(msg.X, msg.Y, msg.Z)
time = msg.TimeUS * 1.0e-6
data.add_gyro(imu, T, time, gyr)
if msg.get_type() == 'IMU' and not args.tclr:
imu = msg.I
if stop_capture[imu]:
continue
T = msg.T
acc = Vector3(msg.AccX, msg.AccY, msg.AccZ)
gyr = Vector3(msg.GyrX, msg.GyrY, msg.GyrZ)
# invert the board orientation rotation. Corrections are in sensor frame
if orientation != 0:
acc = acc.rotate_by_inverse_id(orientation)
gyr = gyr.rotate_by_inverse_id(orientation)
if acc is None or gyr is None:
print("Invalid AHRS_ORIENTATION %u" % orientation)
sys.exit(1)
if c.enable[imu] == 1:
acc -= c.correction_accel(imu, T)
gyr -= c.correction_gyro(imu, T)
time = msg.TimeUS * 1.0e-6
data.add_accel(imu, T, time, acc)
data.add_gyro(imu, T, time, gyr)
if len(data.IMUs()) == 0:
print("No data found")
sys.exit(1)
print("Loaded %u accel and %u gyro samples" %
(len(data.accel[0]['T']), len(data.gyro[0]['T'])))
if not args.tclr:
# apply moving average filter with 2s width
data.Filter(2)
clog = c
c = Coefficients()
calfile = open(args.outfile, "w")
for imu in data.IMUs():
tmin = np.amin(data.accel[imu]['T'])
tmax = np.amax(data.accel[imu]['T'])
c.set_tmin(imu, tmin)
c.set_tmax(imu, tmax)
for axis in AXES:
if args.online:
fit = OnlineIMUfit()
trel = data.accel[imu]['T'] - TEMP_REF
ofs = data.accel_at_temp(imu, axis, clog.atcal[imu])
c.set_accel_poly(
imu, axis,
fit.polyfit(trel, data.accel[imu][axis] - ofs, POLY_ORDER))
trel = data.gyro[imu]['T'] - TEMP_REF
c.set_gyro_poly(
imu, axis,
fit.polyfit(trel, data.gyro[imu][axis], POLY_ORDER))
else:
trel = data.accel[imu]['T'] - TEMP_REF
if imu in clog.atcal:
ofs = data.accel_at_temp(imu, axis, clog.atcal[imu])
else:
ofs = np.mean(data.accel[imu][axis])
c.set_accel_poly(
imu, axis,
np.polyfit(trel, data.accel[imu][axis] - ofs, POLY_ORDER))
trel = data.gyro[imu]['T'] - TEMP_REF
c.set_gyro_poly(
imu, axis,
np.polyfit(trel, data.gyro[imu][axis], POLY_ORDER))
params = c.param_string(imu)
print(params)
calfile.write(params)
calfile.close()
print("Calibration written to %s" % args.outfile)
if args.no_graph:
return
fig, axs = pyplot.subplots(len(data.IMUs()), 1, sharex=True)
num_imus = len(data.IMUs())
if num_imus == 1:
axs = [axs]
for imu in data.IMUs():
scale = math.degrees(1)
for axis in AXES:
axs[imu].plot(data.gyro[imu]['time'],
data.gyro[imu][axis] * scale,
label='Uncorrected %s' % axis)
for axis in AXES:
poly = np.poly1d(c.gcoef[imu][axis])
trel = data.gyro[imu]['T'] - TEMP_REF
correction = poly(trel)
axs[imu].plot(data.gyro[imu]['time'],
(data.gyro[imu][axis] - correction) * scale,
label='Corrected %s' % axis)
if args.log_parm:
for axis in AXES:
if clog.enable[imu] == 0.0:
print("IMU[%u] disabled in log parms" % imu)
continue
poly = np.poly1d(clog.gcoef[imu][axis])
correction = poly(data.gyro[imu]['T'] - TEMP_REF) - poly(
clog.gtcal[imu] - TEMP_REF) + clog.gofs[imu][axis]
axs[imu].plot(data.gyro[imu]['time'],
(data.gyro[imu][axis] - correction) * scale,
label='Corrected %s (log parm)' % axis)
ax2 = axs[imu].twinx()
ax2.plot(data.gyro[imu]['time'],
data.gyro[imu]['T'],
label='Temperature(C)',
color='black')
ax2.legend(loc='upper right')
axs[imu].legend(loc='upper left')
axs[imu].set_title('IMU[%u] Gyro (deg/s)' % imu)
fig, axs = pyplot.subplots(num_imus, 1, sharex=True)
if num_imus == 1:
axs = [axs]
for imu in data.IMUs():
for axis in AXES:
ofs = data.accel_at_temp(imu, axis, clog.atcal.get(imu, TEMP_REF))
axs[imu].plot(data.accel[imu]['time'],
data.accel[imu][axis] - ofs,
label='Uncorrected %s' % axis)
for axis in AXES:
poly = np.poly1d(c.acoef[imu][axis])
trel = data.accel[imu]['T'] - TEMP_REF
correction = poly(trel)
ofs = data.accel_at_temp(imu, axis, clog.atcal.get(imu, TEMP_REF))
axs[imu].plot(data.accel[imu]['time'],
(data.accel[imu][axis] - ofs) - correction,
label='Corrected %s' % axis)
if args.log_parm:
for axis in AXES:
if clog.enable[imu] == 0.0:
print("IMU[%u] disabled in log parms" % imu)
continue
poly = np.poly1d(clog.acoef[imu][axis])
ofs = data.accel_at_temp(imu, axis, clog.atcal[imu])
correction = poly(data.accel[imu]['T'] -
TEMP_REF) - poly(clog.atcal[imu] - TEMP_REF)
axs[imu].plot(data.accel[imu]['time'],
(data.accel[imu][axis] - ofs) - correction,
label='Corrected %s (log parm)' % axis)
ax2 = axs[imu].twinx()
ax2.plot(data.accel[imu]['time'],
data.accel[imu]['T'],
label='Temperature(C)',
color='black')
ax2.legend(loc='upper right')
axs[imu].legend(loc='upper left')
axs[imu].set_title('IMU[%u] Accel (m/s^2)' % imu)
pyplot.show()
def vector_to_AP(vec):
'''convert pybullet vector tuple to ArduPilot Vector3'''
return Vector3(vec[0], -vec[1], -vec[2])
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
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/AP_GeodesicGrid.h
and
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/AP_GeodesicGrid.cpp
as reference for defining the geodesic sections and implementing almost all
functions. Those files should be consulted for implementation details.
'''
import math
from pymavlink.rotmat import Matrix3, Vector3
# The golden number below was obtained from scipy.constants.golden. Let's use
# the literal value here as this is the only place that would require scipy
# module.
g = 1.618033988749895
_first_half = (
(Vector3(-g, 1, 0), Vector3(-1, 0, -g), Vector3(-g, -1, 0)),
(Vector3(-1, 0, -g), Vector3(-g, -1, 0), Vector3(0, -g, -1)),
(Vector3(-g, -1, 0), Vector3(0, -g, -1), Vector3(0, -g, 1)),
(Vector3(-1, 0, -g), Vector3(0, -g, -1), Vector3(1, 0, -g)),
(Vector3(0, -g, -1), Vector3(0, -g, 1), Vector3(g, -1, 0)),
(Vector3(0, -g, -1), Vector3(1, 0, -g), Vector3(g, -1, 0)),
(Vector3(g, -1, 0), Vector3(1, 0, -g), Vector3(g, 1, 0)),
(Vector3(1, 0, -g), Vector3(g, 1, 0), Vector3(0, g, -1)),
(Vector3(1, 0, -g), Vector3(0, g, -1), Vector3(-1, 0, -g)),
(Vector3(0, g, -1), Vector3(-g, 1, 0), Vector3(-1, 0, -g)),
)
_second_half = tuple((-a, -b, -c) for a, b, c in _first_half)
triangles = _first_half + _second_half
radius = math.sqrt(1 + g**2)
