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()
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 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)