Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[tools] calibrate.py: minor improvements

Browse Source 
- actually check if it is a *.data file before checking for AC_ID
- still show result of last iteration if optimization failed to better see what was missing
- add some subplot titles to the plot
This commit is contained in:
Felix Ruess
2015-07-01 11:05:23 +02:00
parent f052e69b91
commit dd5dddf2e0
2 changed files with 34 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/tools/calibration/calibrate.py
+16 -11
View File
@@ -58,6 +58,10 @@ def main():
else:
print(args[0] + " not found")
sys.exit(1)
if not filename.endswith(".data"):
parser.error("Please specify a *.data log file")
ac_ids = calibration_utils.get_ids_in_log(filename)
if options.ac_id is None:
if len(ac_ids) == 1:
@@ -78,8 +82,6 @@ def main():
noise_window = 10
noise_threshold = 1000
if not filename.endswith(".data"):
parser.error("Please specify a *.data log file")
if options.verbose:
print("reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor)
@@ -120,27 +122,30 @@ def main():
return err
p1, cov, info, msg, success = optimize.leastsq(err_func, p0[:], args=(flt_meas, sensor_ref), full_output=1)
if not success in [1, 2, 3, 4]:
optimze_failed = success not in [1, 2, 3, 4]
if optimze_failed:
print("Optimization error: ", msg)
print("Please try to provide a clean logfile.")
sys.exit(1)
print("Please try to provide a clean logfile with proper distribution of measurements.")
#sys.exit(1)
cp1, np1 = calibration_utils.scale_measurements(flt_meas, p1)
print("optimized guess : avg "+str(np1.mean())+" std "+str(np1.std()))
# print p1
if optimze_failed:
print("last iteration of failed optimized guess : avg "+str(np1.mean())+" std "+str(np1.std()))
else:
print("optimized guess : avg "+str(np1.mean())+" std "+str(np1.std()))
calibration_utils.print_xml(p1, options.sensor, sensor_res)
print("")
if not optimze_failed:
calibration_utils.print_xml(p1, options.sensor, sensor_res)
if options.plot:
# if we are calibrating a mag, just draw first plot (non-blocking), then show the second
if options.sensor == "MAG":
calibration_utils.plot_results(False, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
calibration_utils.plot_results(options.sensor, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref, blocking=False)
calibration_utils.plot_mag_3d(flt_meas, cp1, p1)
# otherwise show the first plot (blocking)
else:
calibration_utils.plot_results(True, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
calibration_utils.plot_results(options.sensor, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref)
if __name__ == "__main__":
main()
sw/tools/calibration/calibration_utils.py
+18 -7
View File
@@ -144,6 +144,7 @@ def print_xml(p, sensor, res):
print("<define name=\""+sensor+"_X_SENS\" value=\""+str(p[3]*2**res)+"\" integer=\"16\"/>")
print("<define name=\""+sensor+"_Y_SENS\" value=\""+str(p[4]*2**res)+"\" integer=\"16\"/>")
print("<define name=\""+sensor+"_Z_SENS\" value=\""+str(p[5]*2**res)+"\" integer=\"16\"/>")
print("")
def print_imu_scaled(sensor, measurements, attrs):
@@ -158,8 +159,9 @@ def print_imu_scaled(sensor, measurements, attrs):
def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref):
def plot_results(sensor, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref, blocking=True):
"""Plot calibration results."""
# plot raw measurements with filtered ones marked as red circles
plt.subplot(3, 1, 1)
plt.plot(measurements[:, 0])
plt.plot(measurements[:, 1])
@@ -167,43 +169,52 @@ def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sen
plt.plot(flt_idx, flt_meas[:, 0], 'ro')
plt.plot(flt_idx, flt_meas[:, 1], 'ro')
plt.plot(flt_idx, flt_meas[:, 2], 'ro')
plt.xlabel('time (s)')
plt.ylabel('ADC')
plt.title('Raw sensors')
plt.title('Raw '+sensor+', red dots are actually used measurements')
plt.tight_layout()
# show scaled measurements with initial guess
plt.subplot(3, 2, 3)
plt.plot(cp0[:, 0])
plt.plot(cp0[:, 1])
plt.plot(cp0[:, 2])
plt.plot(-sensor_ref*np.ones(len(flt_meas)))
plt.plot(sensor_ref*np.ones(len(flt_meas)))
plt.title('scaled '+sensor+' (initial guess)')
plt.xticks([])
plt.subplot(3, 2, 4)
plt.plot(np0)
plt.plot(sensor_ref*np.ones(len(flt_meas)))
plt.title('norm of '+sensor+' (initial guess)')
plt.xticks([])
# show scaled measurements after optimization
plt.subplot(3, 2, 5)
plt.plot(cp1[:, 0])
plt.plot(cp1[:, 1])
plt.plot(cp1[:, 2])
plt.plot(-sensor_ref*np.ones(len(flt_meas)))
plt.plot(sensor_ref*np.ones(len(flt_meas)))
plt.title('scaled '+sensor+' (optimized)')
plt.xticks([])
plt.subplot(3, 2, 6)
plt.plot(np1)
plt.plot(sensor_ref*np.ones(len(flt_meas)))
plt.title('norm of '+sensor+' (optimized)')
plt.xticks([])
# if we want to have another plot we only draw the figure (non-blocking)
# also in matplotlib before 1.0.0 there is only one call to show possible
if block:
if blocking:
plt.show()
else:
plt.draw()
def plot_imu_scaled(sensor, measurements, attrs):
"""Plot imu scaled results."""
plt.figure("Sensor Scaled")
plt.subplot(4, 1, 1)
@@ -231,9 +242,9 @@ def plot_imu_scaled(sensor, measurements, attrs):
plt.show()
def plot_imu_scaled_fft(sensor, measurements, attrs):
"""Plot imu scaled fft results."""
#dt = 0.0769
#Fs = 1/dt
Fs = 26.0