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()
if args.lat != 0 and args.lon != 0:
earth_field = mavextra.expected_earth_field_lat_lon(args.lat, args.lon)
(declination, inclination,
intensity) = mavextra.get_mag_field_ef(args.lat, args.lon)
print("Earth field: %s strength %.0f declination %.1f degrees" %
(earth_field, earth_field.length(), declination))
# 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))
if old_corrections.diag == Vector3(0, 0, 0):
old_corrections.diag = Vector3(1, 1, 1)
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))
if len(data) == 0:
return
# 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 magfit(mlog, timestamp_in_range):
'''find best magnetometer offset fit to a log file'''
global earth_field, declination
global data
data = []
ATT = None
BAT = None
mag_msg = margs['Magnetometer']
global mag_idx
if mag_msg[-1].isdigit():
mag_instance = None
mag_idx = mag_msg[-1]
elif mag_msg.endswith('[0]'):
mag_instance = 0
mag_idx = ''
mag_msg = 'MAG'
elif mag_msg.endswith(']'):
mag_instance = int(mag_msg[-2])
mag_idx = str(mag_instance + 1)
mag_msg = 'MAG'
else:
mag_instance = None
mag_idx = ''
count = 0
parameters = {}
# get parameters
mlog.rewind()
while True:
msg = mlog.recv_match(type=['PARM'])
if msg is None:
break
parameters[msg.Name] = msg.Value
mlog.rewind()
lat = margs['Lattitude']
lon = margs['Longitude']
if lat != 0 and lon != 0:
earth_field = mavextra.expected_earth_field_lat_lon(lat, lon)
(declination, inclination,
intensity) = mavextra.get_mag_field_ef(lat, lon)
print("Earth field: %s strength %.0f declination %.1f degrees" %
(earth_field, earth_field.length(), declination))
ATT_NAME = margs['Attitude']
print("Attitude source %s" % ATT_NAME)
# extract MAG data
while True:
msg = mlog.recv_match(type=['GPS', mag_msg, ATT_NAME, 'BAT'])
if msg is None:
break
in_range = timestamp_in_range(msg._timestamp)
if in_range < 0:
continue
if in_range > 0:
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_NAME:
if getattr(msg, 'C', 0) != 0:
# use core zero for EKF attitude
continue
ATT = msg
if msg.get_type() == 'BAT':
BAT = msg
if msg.get_type() == mag_msg and ATT is not None:
if mag_instance is not None:
if getattr(msg, 'I', 0) != mag_instance:
continue
if count % margs['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))
if old_corrections.diag == Vector3(0, 0, 0):
old_corrections.diag = Vector3(1, 1, 1)
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))
if len(data) == 0:
return
# 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
min_scale = margs['ScaleMin']
max_scale = margs['ScaleMax']
if c.scaling > max_scale:
c.scaling = max_scale
if c.scaling < min_scale:
c.scaling = 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(block=False)
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, new_param_format
global data
data = []
ATT = None
BAT = None
if args.mag == 1:
mag_msg = 'MAG'
else:
mag_msg = 'MAG%s' % args.mag
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()
if args.lat != 0 and args.lon != 0:
earth_field = mavextra.expected_earth_field_lat_lon(args.lat, args.lon)
(declination, inclination,
intensity) = mavextra.get_mag_field_ef(args.lat, args.lon)
print("Earth field: %s strength %.0f declination %.1f degrees" %
(earth_field, earth_field.length(), declination))
if args.att_source is not None:
ATT_NAME = args.att_source
if parameters['AHRS_EKF_TYPE'] == 2:
ATT_NAME = 'NKF1'
elif parameters['AHRS_EKF_TYPE'] == 3:
ATT_NAME = 'XKF1'
else:
ATT_NAME = 'ATT'
print("Attitude source %s" % ATT_NAME)
# extract MAG data
while True:
msg = mlog.recv_match(
type=['GPS', mag_msg, ATT_NAME, '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_NAME:
ATT = msg
# remove IMU sensor to body frame trim offsets to get back to the IMU sensor frame used by the EKFs
ATT.Roll = ATT.Roll + math.degrees(parameters['AHRS_TRIM_X'])
ATT.Pitch = ATT.Pitch + math.degrees(parameters['AHRS_TRIM_Y'])
ATT.Yaw = ATT.Yaw + math.degrees(parameters['AHRS_TRIM_Z'])
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
# use COMPASS 1 offsets as test for param scheme
if 'COMPASS_OFS_X' in parameters.keys():
new_param_format = False
elif 'COMPASS1_OFS_X' in parameters.keys():
new_param_format = True
if new_param_format is None:
print("Unknown param format")
sys.exit(1)
old_corrections.offsets = Vector3(
parameters.get(param_name('OFS', args.mag) + '_X', 0.0),
parameters.get(param_name('OFS', args.mag) + '_Y', 0.0),
parameters.get(param_name('OFS', args.mag) + '_Z', 0.0))
old_corrections.diag = Vector3(
parameters.get(param_name('DIA', args.mag) + '_X', 1.0),
parameters.get(param_name('DIA', args.mag) + '_Y', 1.0),
parameters.get(param_name('DIA', args.mag) + '_Z', 1.0))
if old_corrections.diag == Vector3(0, 0, 0):
old_corrections.diag = Vector3(1, 1, 1)
old_corrections.offdiag = Vector3(
parameters.get(param_name('ODI', args.mag) + '_X', 0.0),
parameters.get(param_name('ODI', args.mag) + '_Y', 0.0),
parameters.get(param_name('ODI', args.mag) + '_Z', 0.0))
if parameters.get('COMPASS_MOTCT', 0) == 2:
# only support current based corrections for now
old_corrections.cmot = Vector3(
parameters.get(param_name('MOT', args.mag) + '_X', 0.0),
parameters.get(param_name('MOT', args.mag) + '_Y', 0.0),
parameters.get(param_name('MOT', args.mag) + '_Z', 0.0))
old_corrections.scaling = parameters.get(param_name('SCALE', args.mag),
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))
if len(data) == 0:
return
# 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)
if args.save_params:
name = args.log.rsplit('.', 1)[0] + '-magfit-mag-%s.param' % args.mag
print("Saving params to %s" % name)
f = open(name, 'w')
c.show_parms(f)
f.close()
else:
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')
if args.save_plot:
name = args.log.rsplit('.', 1)[0] + '-magfit-mag-%s.png' % args.mag
print("Saving plot as %s" % name)
pyplot.savefig(name)
else:
pyplot.show()
#!/usr/bin/env python3
'''
generate some test vectors for autotest
'''
from pymavlink import mavextra
import random
import argparse
parser = argparse.ArgumentParser(description='generate test vectors')
parser.add_argument('--num-samples', type=int, default=100, help='number of samples')
args = parser.parse_args()
for i in range(args.num_samples):
lat = random.uniform(-89,89)
lon = random.uniform(-180,180)
mavextra.earth_field = None
m = mavextra.expected_earth_field_lat_lon(lat, lon)
print("{%f, %f, {%.3f, %.3f, %.3f}}," % (lat, lon, m.x, m.y, m.z))