Before updating IMS interpolation

src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java

@@ -411,6 +411,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
 	public static EyesisCorrections EYESIS_CORRECTIONS = null;
 	public static EyesisCorrections EYESIS_CORRECTIONS_AUX = null;
 	public static EyesisCorrectionParameters.EquirectangularParameters EQUIRECTANGULAR_PARAMETERS = new EyesisCorrectionParameters.EquirectangularParameters();
+	public static EventLogger EVENT_LOGGER = null;
 
 	public static int CONVOLVE_FFT_SIZE = 128; // FFT size for sliding convolution with kernel
 	public static int THREADS_MAX = 100; // testing multi-threading, limit maximal number of threads
@@ -5523,19 +5524,22 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
 			DEBUG_LEVEL = MASTER_DEBUG_LEVEL;
 			EYESIS_CORRECTIONS.setDebug(DEBUG_LEVEL);
 			CLT_PARAMETERS.batch_run = true;
-			/*
-			Imx5 imx5 = new Imx5();
-			byte [] imx5_rslt = imx5.test_imx();
-			System.out.println("imx5_rslt.length="+imx5_rslt.length);
-			*/
-//			EventLogger eventLogger = new EventLogger ("/home/elphel/lwir16-proc/ims/ims_selected/");
-			String imx_logs = CORRECTION_PARAMETERS.sourceImsDirectory;
-			if ((imx_logs != null) && !imx_logs.equals("")) {
-				EventLogger eventLogger = new EventLogger (imx_logs);
+			String imx_logs = CORRECTION_PARAMETERS.getImsSourceDirectory(); // sourceImsDirectory;
+			if (EVENT_LOGGER == null) {
+				if ((imx_logs != null) && !imx_logs.equals("")) {
+					EVENT_LOGGER = new EventLogger (imx_logs);
+				} else {
+					System.out.println("sourceImsDirectory is not set. Use Setup CLT Batch parameters to define it.");
+				}
+			}
+			if (EVENT_LOGGER != null) {
+				EVENT_LOGGER.printPimu(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_pimu");
+				EVENT_LOGGER.printGps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_gps", 7);
+				EVENT_LOGGER.printDidIns1(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins1");
+				EVENT_LOGGER.printDidIns2(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins2");
 			} else {
-				System.out.println("sourceImsDirectory is not set. Use Setup CLT Batch parameters to define it.");
+				System.out.println("EVENT_LOGGEER is null.");
 			}
-			
 			return;
 //JTabbedTest
 // End of buttons code

src/main/java/com/elphel/imagej/ims/Did_gps_pos.java

@@ -2,6 +2,9 @@ package com.elphel.imagej.ims;
 
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
+import java.util.Properties;
+
+import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
 
 /** (DID_GPS1_POS, DID_GPS1_UBX_POS, DID_GPS2_POS) GPS position data */
 public class Did_gps_pos {
@@ -94,10 +97,14 @@ public class Did_gps_pos {
 	}
 	public Did_gps_pos() {}
 
+	public Did_gps_pos(String prefix, Properties properties) {
+		getProperties(prefix, properties);
+	}
+
 	public Did_gps_pos interpolate(double frac, Did_gps_pos next_did) {
 		Did_gps_pos new_did = new Did_gps_pos();
 		double time_ms_this = timeOfWeekMs + Did_strobe_in_time.WEEK_MS * week;
-		double time_ms_next = timeOfWeekMs + Did_strobe_in_time.WEEK_MS * week;
+		double time_ms_next = next_did.timeOfWeekMs + Did_strobe_in_time.WEEK_MS * next_did.week;
 		long   time_ms =      (long) Did_ins.interpolateDouble(frac, time_ms_this, time_ms_next);
 		new_did.week =         (int) Math.floor(time_ms / Did_strobe_in_time.WEEK_MS);
 		new_did.timeOfWeekMs = (int) (time_ms - new_did.week * Did_strobe_in_time.WEEK_MS); 
@@ -160,5 +167,46 @@ public class Did_gps_pos {
 		s += String.format("reserved:     %d",    ((int) reserved) & 0xff); 
 		return s;
 	}
+	
+ 	public void getProperties(String prefix, Properties properties) {
+		if (properties.getProperty(prefix+"week")!=null)         this.week= Integer.parseInt(properties.getProperty(prefix+"week"));
+		if (properties.getProperty(prefix+"timeOfWeekMs")!=null) this.timeOfWeekMs= Integer.parseInt(properties.getProperty(prefix+"week"));
+		if (properties.getProperty(prefix+"status")!=null)       this.status= Integer.parseInt(properties.getProperty(prefix+"status"));
+		if (properties.getProperty(prefix+"ecef")!=null) 	     this.ecef= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ecef"),3);
+		if (properties.getProperty(prefix+"lla")!=null) 	     this.lla= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"lla"),3);
+		if (properties.getProperty(prefix+"hMSL")!=null)         this.hMSL= Float.parseFloat(properties.getProperty(prefix+"hMSL"));
+		if (properties.getProperty(prefix+"hAcc")!=null)         this.hAcc= Float.parseFloat(properties.getProperty(prefix+"hAcc"));
+		if (properties.getProperty(prefix+"vAcc")!=null)         this.vAcc= Float.parseFloat(properties.getProperty(prefix+"vAcc"));
+		if (properties.getProperty(prefix+"pDop")!=null)         this.pDop= Float.parseFloat(properties.getProperty(prefix+"pDop"));
+		if (properties.getProperty(prefix+"cnoMean")!=null)      this.cnoMean= Float.parseFloat(properties.getProperty(prefix+"cnoMean"));
+		if (properties.getProperty(prefix+"towOffset")!=null)    this.towOffset= Double.parseDouble(properties.getProperty(prefix+"towOffset"));
+		if (properties.getProperty(prefix+"leapS")!=null)        this.leapS= Byte.parseByte(properties.getProperty(prefix+"leapS"));
+		if (properties.getProperty(prefix+"satsUsed")!=null)     this.satsUsed= Byte.parseByte(properties.getProperty(prefix+"satsUsed"));
+		if (properties.getProperty(prefix+"cnoMeanSigma")!=null) this.cnoMeanSigma= Byte.parseByte(properties.getProperty(prefix+"cnoMeanSigma"));
+		if (properties.getProperty(prefix+"reserved")!=null)     this.reserved= Byte.parseByte(properties.getProperty(prefix+"reserved"));
+		
+	}
+ 	
+	public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
+		if (properties == null) {
+			properties = new Properties();
+		}
+		properties.setProperty(prefix+"week",         this.week+"");
+		properties.setProperty(prefix+"timeOfWeekMs", this.timeOfWeekMs+"");
+		properties.setProperty(prefix+"status",       this.status+"");
+		properties.setProperty(prefix+"ecef",         IntersceneMatchParameters.doublesToString(this.ecef));
+		properties.setProperty(prefix+"lla",          IntersceneMatchParameters.doublesToString(this.lla));
+		properties.setProperty(prefix+"hMSL",         this.hMSL+"");
+		properties.setProperty(prefix+"hAcc",         this.hAcc+"");
+		properties.setProperty(prefix+"vAcc",         this.vAcc+"");
+		properties.setProperty(prefix+"pDop",         this.pDop+"");
+		properties.setProperty(prefix+"cnoMean",      this.cnoMean+"");
+		properties.setProperty(prefix+"towOffset",    this.towOffset+"");
+		properties.setProperty(prefix+"leapS",        this.leapS+"");
+		properties.setProperty(prefix+"satsUsed",     this.satsUsed+"");
+		properties.setProperty(prefix+"cnoMeanSigma", this.cnoMeanSigma+"");
+		properties.setProperty(prefix+"reserved",     this.reserved+"");
+		return properties;
+	}
 
 }
\ No newline at end of file

src/main/java/com/elphel/imagej/ims/Did_ins.java

 package com.elphel.imagej.ims;
 
+import java.util.Properties;
+
+import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
+
 public abstract class Did_ins <T extends Did_ins <T>>{
 	final static int WEEK_SECONDS = 7 * 24 * 3600;
 	/** GPS number of weeks since January 6th, 1980 */
@@ -42,4 +46,23 @@ public abstract class Did_ins <T extends Did_ins <T>>{
     		new_did.lla[i] =   interpolateDouble(frac, lla[i], next_did.lla[i]) ;
     	}
 	}
+	
+	public void getProperties(String prefix, Properties properties) {
+		if (properties.getProperty(prefix+"week")!=null)       this.week= Integer.parseInt(properties.getProperty(prefix+"week"));
+		if (properties.getProperty(prefix+"timeOfWeek")!=null) this.timeOfWeek= Double.parseDouble(properties.getProperty(prefix+"timeOfWeek"));
+		if (properties.getProperty(prefix+"insStatus")!=null)  this.insStatus= Integer.parseInt(properties.getProperty(prefix+"insStatus"));
+		if (properties.getProperty(prefix+"hdwStatus")!=null)  this.hdwStatus= Integer.parseInt(properties.getProperty(prefix+"hdwStatus"));
+		if (properties.getProperty(prefix+"lla")!=null) 	   this.lla= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"lla"),3);
+	}
+	public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
+		if (properties == null) {
+			properties = new Properties();
+		}
+		properties.setProperty(prefix+"week",       this.week+"");
+		properties.setProperty(prefix+"timeOfWeek", this.timeOfWeek+"");
+		properties.setProperty(prefix+"insStatus",  this.insStatus+"");
+		properties.setProperty(prefix+"hdwStatus",  this.hdwStatus+"");
+		properties.setProperty(prefix+"lla",        IntersceneMatchParameters.doublesToString(this.lla));		
+		return properties;
+	}
 }
\ No newline at end of file

src/main/java/com/elphel/imagej/ims/Did_ins_1.java

@@ -3,6 +3,9 @@ package com.elphel.imagej.ims;
 //import java.io.Serializable;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
+import java.util.Properties;
+
+import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
 
 //public class Did_ins_1 implements Serializable {
 //	static final long serialVersionUID = 4;
@@ -36,6 +39,10 @@ public class Did_ins_1 extends Did_ins <Did_ins_1>{
 		ned[0]=    bb.getFloat();  ned[1] =   bb.getFloat();  ned[2]=   bb.getFloat();
 	}
 	public Did_ins_1() {}
+	public Did_ins_1(String prefix, Properties properties) {
+		getProperties(prefix, properties);
+	}
+	
 
 	public Did_ins_1 interpolate(double frac, Did_ins_1 next_did) {
 		Did_ins_1 new_did = new Did_ins_1();
@@ -75,6 +82,19 @@ public class Did_ins_1 extends Did_ins <Did_ins_1>{
 		s += String.format("ned:        %f, %f, %f",   ned[0], ned[1], ned[2]); 
 		return s;
 	}
-	
+
+	public void getProperties(String prefix, Properties properties) {
+		super.getProperties(prefix, properties);
+		if (properties.getProperty(prefix+"theta")!=null) 	   this.theta= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"theta"),3);
+		if (properties.getProperty(prefix+"uvw")!=null) 	   this.uvw=   IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"uvw"),3);
+		if (properties.getProperty(prefix+"ned")!=null) 	   this.ned=   IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"ned"),3);
+	}
+	public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
+		properties = super.setProperties(prefix, properties);
+		properties.setProperty(prefix+"theta",        IntersceneMatchParameters.floatsToString(this.theta));		
+		properties.setProperty(prefix+"uvw",          IntersceneMatchParameters.floatsToString(this.uvw));		
+		properties.setProperty(prefix+"ned",          IntersceneMatchParameters.floatsToString(this.ned));		
+		return properties;
+	}	
 
 }
\ No newline at end of file

src/main/java/com/elphel/imagej/ims/Did_ins_2.java

+package com.elphel.imagej.ims;
+
+//import java.io.Serializable;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.Properties;
+
+import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
+
+//public class Did_ins_1 implements Serializable {
+//	static final long serialVersionUID = 4;
+public class Did_ins_2 extends Did_ins <Did_ins_2>{
+	/** GPS number of weeks since January 6th, 1980 */
+//	public int				week;      // uint32_t
+	/** GPS time of week (since Sunday morning) in seconds */
+//	public double			timeOfWeek;
+	/** INS status flags (eInsStatusFlags). Copy of DID_SYS_PARAMS.insStatus */
+//	public int				insStatus; //uint32_t
+	/** Hardware status flags (eHdwStatusFlags). Copy of DID_SYS_PARAMS.hdwStatus */
+//	public int				hdwStatus; // uint32_t
+	/** Euler angles: roll, pitch, yaw in radians with respect to NED */
+	public float []		qn2b = new float[4];
+	/** Velocity U, V, W in meters per second.  Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )". */
+	public float []		uvw =   new float [3];
+	/** WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters) */
+//	public double []		lla =   new double [3];
+
+	public Did_ins_2 (ByteBuffer bb) {
+		bb.order(ByteOrder.LITTLE_ENDIAN);		
+		week=      bb.getInt();
+		timeOfWeek=bb.getDouble();
+		insStatus= bb.getInt();
+		hdwStatus= bb.getInt();
+		qn2b[0]=  bb.getFloat();  qn2b[1] = bb.getFloat();  qn2b[2]= bb.getFloat(); qn2b[3]= bb.getFloat();
+		uvw[0]=    bb.getFloat();  uvw[1] =   bb.getFloat();  uvw[2]=   bb.getFloat();
+		lla[0]=    bb.getDouble(); lla[1] =   bb.getDouble(); lla[2]=   bb.getDouble();
+	}
+	public Did_ins_2() {}
+	public Did_ins_2(String prefix, Properties properties) {
+		getProperties(prefix, properties);
+	}
+	
+
+	public Did_ins_2 interpolate(double frac, Did_ins_2 next_did) {
+		Did_ins_2 new_did = new Did_ins_2();
+		interpolateBase(frac, next_did, new_did);
+    	for (int i = 0; i < qn2b.length; i++) {
+    		new_did.qn2b[i] =   interpolateFloat(frac, qn2b[i], next_did.qn2b[i]) ;
+    	}    	
+    	for (int i = 0; i < uvw.length; i++) {
+    		new_did.uvw[i] =   interpolateFloat(frac, uvw[i], next_did.uvw[i]) ;
+    	}    	
+		return new_did;
+	}
+	
+	
+	public int pack(ByteBuffer bb) {
+		int p_start = bb.position();
+		bb.putInt(week);
+		bb.putDouble(timeOfWeek);
+		bb.putInt(insStatus);
+		bb.putInt(hdwStatus);
+		bb.putFloat(qn2b[0]); bb.putFloat(qn2b[1]); bb.putFloat(qn2b[2]); bb.putFloat(qn2b[3]);
+		bb.putFloat(uvw[0]);   bb.putFloat(uvw[1]);   bb.putFloat(uvw[2]);
+		bb.putDouble(lla[0]);  bb.putDouble(lla[1]);  bb.putDouble(lla[2]);
+		int p_end = bb.position();
+		return p_end - p_start;
+	}
+	
+	public String toString() {
+		String s = "DID_INS_2\n";
+		s += String.format("week:       %d\n", week); 
+		s += String.format("timeOfWeek: %f\n", timeOfWeek); 
+		s += String.format("insStatus:  0x%x\n", insStatus); 
+		s += String.format("hdwStatus:  0x%x\n", hdwStatus); 
+		s += String.format("qn2b:       %f, %f, %f, %f\n", qn2b[0], qn2b[1], qn2b[2], qn2b[3]); 
+		s += String.format("uvw:        %f, %f, %f\n", uvw[0], uvw[1], uvw[2]); 
+		s += String.format("lla:        %f, %f, %f\n", lla[0], lla[1], lla[2]); 
+		return s;
+	}
+
+	public void getProperties(String prefix, Properties properties) {
+		super.getProperties(prefix, properties);
+		if (properties.getProperty(prefix+"qn2b")!=null) 	   this.qn2b= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"qn2b"),4);
+		if (properties.getProperty(prefix+"uvw")!=null) 	   this.uvw=   IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"uvw"),3);
+	}
+	public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
+		properties = super.setProperties(prefix, properties);
+		properties.setProperty(prefix+"qn2b",        IntersceneMatchParameters.floatsToString(this.qn2b));		
+		properties.setProperty(prefix+"uvw",          IntersceneMatchParameters.floatsToString(this.uvw));		
+		return properties;
+	}	
+}
\ No newline at end of file

src/main/java/com/elphel/imagej/ims/Did_pimu.java

@@ -2,6 +2,9 @@ package com.elphel.imagej.ims;
 
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
+import java.util.Properties;
+
+import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
 
 public class Did_pimu {
 	/** Time since boot up in seconds.  Convert to GPS time of week by adding gps.towOffset */
@@ -25,6 +28,10 @@ public class Did_pimu {
 	}
 	public Did_pimu() {}
 
+	public Did_pimu(String prefix, Properties properties) {
+		getProperties(prefix, properties);
+	}
+	
 	public Did_pimu interpolate(double frac, Did_pimu next_did) {
 		Did_pimu new_did = new Did_pimu();
 		new_did.time = Did_ins.interpolateDouble(frac, time, next_did.time);
@@ -58,5 +65,25 @@ public class Did_pimu {
 		s += String.format("vel:        %f, %f, %f",   vel[0], vel[1], vel[2]); 
 		return s;
 	}
+	
+	public void getProperties(String prefix, Properties properties) {
+		if (properties.getProperty(prefix+"time")!=null)       this.time=   Double.parseDouble(properties.getProperty(prefix+"time"));
+		if (properties.getProperty(prefix+"dt")!=null)         this.dt=     Float.parseFloat(properties.getProperty(prefix+"dt"));
+		if (properties.getProperty(prefix+"status")!=null)     this.status= Integer.parseInt(properties.getProperty(prefix+"status"));
+		if (properties.getProperty(prefix+"theta")!=null) 	   this.theta=  IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"theta"),3);
+		if (properties.getProperty(prefix+"vel")!=null) 	   this.vel=    IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"vel"),3);
+	}
 
+	public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
+		if (properties == null) {
+			properties = new Properties();
+		}
+		properties.setProperty(prefix+"time",     this.time+"");
+		properties.setProperty(prefix+"dt",       this.dt+"");
+		properties.setProperty(prefix+"status",   this.status+"");
+		properties.setProperty(prefix+"theta",    IntersceneMatchParameters.floatsToString(this.theta));
+		properties.setProperty(prefix+"vel",      IntersceneMatchParameters.floatsToString(this.vel));
+		return properties;
+	}
+	
 }
\ No newline at end of file

src/main/java/com/elphel/imagej/ims/Did_strobe_in_time.java

@@ -17,7 +17,7 @@ typedef struct PACKED
 } strobe_in_time_t;
 */
 public class Did_strobe_in_time {
-	public final static int WEEK_MS = 7 * 24 * 3600 * 1000; // milliseconds in a week
+	public final static long WEEK_MS = 7 * 24 * 3600 * 1000; // milliseconds in a week
 	public int 	week;
 
 	/** GPS time of week (since Sunday morning) in milliseconds */

src/main/java/com/elphel/imagej/ims/EventLogger.java

@@ -35,10 +35,10 @@ public class EventLogger {
 		}
 		Arrays.sort(logger_files); // increasing start time stamps
 		
-		testInterpolateDidIns1();
-		testInterpolateDidIns1();
-		testInterpolateDidIns1();
-		
+//		testInterpolateDidIns1();
+//		testInterpolateDidIns1();
+//		testInterpolateDidIns1();
+		/*
 //		String out_did_gps_path = "/home/elphel/lwir16-proc/office_04/did_gps";
 		String out_did_gps_path = dir+"../did_gps/did_gps";
 		out_did_gps_path = Paths.get(out_did_gps_path).normalize().toString();
@@ -56,8 +56,9 @@ public class EventLogger {
 				e.printStackTrace();
 			}
 		}
-		
+		*/
 //		String out_did1_path = "/media/elphel/SSD3-4GB/lwir16-proc/berdich/ims/test_out/did_ins1";
+		/*
 		String out_did1_path = dir+"../did_ins1/did_ins1";
 		out_did1_path = Paths.get(out_did1_path).normalize().toString();
 		Path op_did1 = Paths.get(out_did1_path);
@@ -73,9 +74,82 @@ public class EventLogger {
 				e.printStackTrace();
 			}
 		}
+		*/
 		System.out.println("Processed "+logger_files.length+" event log files");
 	}
 	
+	public void printGps(
+			String out_did_gps_path,
+			int gps_mask) { // 7: // +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
+		out_did_gps_path = Paths.get(out_did_gps_path).normalize().toString();
+		Path op_did_gps = Paths.get(out_did_gps_path);
+		op_did_gps=op_did_gps.normalize();
+		(new File(op_did_gps.getParent().toString())).mkdirs();
+		for (int nf = 0; nf < logger_files.length; nf++) {
+			String out_path= out_did_gps_path+"-"+String.format("%03d", nf)+".out";
+			System.out.println("Printing all DID_GPS data in "+logger_files[nf].abs_path+" to "+out_path);
+			try {
+				logger_files[nf].listGPS(out_path, gps_mask, false);
+			} catch (IOException e) {
+				// TODO Auto-generated catch block
+				e.printStackTrace();
+			}
+		}
+		
+	}
+	
+	public void printDidIns1(String out_did1_path) {
+		out_did1_path = Paths.get(out_did1_path).normalize().toString();
+		Path op_did1 = Paths.get(out_did1_path);
+		op_did1=op_did1.normalize();
+		(new File(op_did1.getParent().toString())).mkdirs();
+		for (int nf = 0; nf < logger_files.length; nf++) {
+			String out_path= out_did1_path+"-"+String.format("%03d", nf)+".out";
+			System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
+			try {
+				logger_files[nf].listDidIns1(out_path,false);
+			} catch (IOException e) {
+				// TODO Auto-generated catch block
+				e.printStackTrace();
+			}
+		}
+	}
+
+	public void printDidIns2(String out_did2_path) {
+		out_did2_path = Paths.get(out_did2_path).normalize().toString();
+		Path op_did2 = Paths.get(out_did2_path);
+		op_did2=op_did2.normalize();
+		(new File(op_did2.getParent().toString())).mkdirs();
+		for (int nf = 0; nf < logger_files.length; nf++) {
+			String out_path= out_did2_path+"-"+String.format("%03d", nf)+".out";
+			System.out.println("Printing all DID_INS_2 data in "+logger_files[nf].abs_path+" to "+out_path);
+			try {
+				logger_files[nf].listDidIns2(out_path,false);
+			} catch (IOException e) {
+				// TODO Auto-generated catch block
+				e.printStackTrace();
+			}
+		}
+	}
+	
+	public void printPimu(String out_pimu_path) {
+		out_pimu_path = Paths.get(out_pimu_path).normalize().toString();
+		Path op_pimu = Paths.get(out_pimu_path);
+		op_pimu=op_pimu.normalize();
+		(new File(op_pimu.getParent().toString())).mkdirs();
+		for (int nf = 0; nf < logger_files.length; nf++) {
+			String out_path= out_pimu_path+"-"+String.format("%03d", nf)+".out";
+			System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
+			try {
+				logger_files[nf].listPimu(out_path,false);
+			} catch (IOException e) {
+				// TODO Auto-generated catch block
+				e.printStackTrace();
+			}
+		}
+	}
+	
+	
 	public void testInterpolateDidIns1() {
 		boolean exit_now = false;
 		double ts_master = 1694576078.939126;
@@ -96,7 +170,7 @@ public class EventLogger {
 	
 	
 	//Did_ins_1
-	Did_ins_1 interpolateDidIns1(
+	public Did_ins_1 interpolateDidIns1(
 			double ts_master,
 			int       debug_level) throws IOException {
 		byte [][] payload = new byte[2][];
@@ -118,6 +192,77 @@ public class EventLogger {
 				frac, // double frac,
 				did_above); // Did_ins_1 next_did))
 	}
+	//Did_ins_2
+	public Did_ins_2 interpolateDidIns2(
+			double ts_master,
+			int       debug_level) throws IOException {
+		byte [][] payload = new byte[2][];
+		int did = Imx5.DID_INS_2;
+		double frac = getDidAtTS(
+				ts_master, // double    ts_master,
+				did, // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
+				payload, // byte [][] payload,
+				debug_level); // int       debug_level)
+		if (Double.isNaN(frac)) {
+			if (debug_level > -1){
+				System.out.println("interpolateDidIns1(): Failed to extract DID_INS_2 data");
+			}
+			return null;
+		}
+		Did_ins_2 did_below = new Did_ins_2(ByteBuffer.wrap(payload[0]));
+		Did_ins_2 did_above = new Did_ins_2(ByteBuffer.wrap(payload[1]));
+		return did_below.interpolate(
+				frac, // double frac,
+				did_above); // Did_ins_1 next_did))
+	}
+	
+	public Did_gps_pos interpolateDidGpsPos(
+			double ts_master,
+			int    did, // Imx5.DID_GPS1_POS, Imx5.DID_GPS2_POS, Imx5.DID_GPS1_UBX_POS
+			int       debug_level) throws IOException {
+		byte [][] payload = new byte[2][];
+		double frac = getDidAtTS(
+				ts_master, // double    ts_master,
+				did, // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
+				payload, // byte [][] payload,
+				debug_level); // int       debug_level)
+		if (Double.isNaN(frac)) {
+			if (debug_level > -1){
+				System.out.println("interpolateDidIns1(): Failed to extract DID_GPS_POS data");
+			}
+			return null;
+		}
+		Did_gps_pos did_below = new Did_gps_pos(ByteBuffer.wrap(payload[0]));
+		Did_gps_pos did_above = new Did_gps_pos(ByteBuffer.wrap(payload[1]));
+		return did_below.interpolate(
+				frac, // double frac,
+				did_above); // Did_gps_pos next_did))
+	}
+	
+	public Did_pimu interpolateDidPimu(
+			double ts_master,
+			int       debug_level) throws IOException {
+		byte [][] payload = new byte[2][];
+		int did = Imx5.DID_PIMU;
+		double frac = getDidAtTS(
+				ts_master, // double    ts_master,
+				did, // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
+				payload, // byte [][] payload,
+				debug_level); // int       debug_level)
+		if (Double.isNaN(frac)) {
+			if (debug_level > -1){
+				System.out.println("interpolateDidIns1(): Failed to extract DID_PIMU data");
+			}
+			return null;
+		}
+		Did_pimu did_below = new Did_pimu(ByteBuffer.wrap(payload[0]));
+		Did_pimu did_above = new Did_pimu(ByteBuffer.wrap(payload[1]));
+		return did_below.interpolate(
+				frac, // double frac,
+				did_above); // Did_ins_1 next_did))
+	}
+	
+	
 	
 	public double getDidAtTS(
 			double    ts_master,

src/main/java/com/elphel/imagej/ims/EventLoggerFileInfo.java

@@ -403,7 +403,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 		}
 	}
 	
-	public void listDid1(String path, boolean keep) throws IOException {
+	public void listDidIns1(String path, boolean keep) throws IOException {
 		double max_offset = 1000; // m
 		open();
 		float [] start_ned = null;
@@ -430,7 +430,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
 		for (int nrec = 0; nrec < num_recs; nrec++) {
 			if (nrec == dbg_nrec) {
-				System.out.println("listDid1(): nrec="+nrec);
+				System.out.println("listDidIns1(): nrec="+nrec);
 			}
 			int [] full_type = getFullType(bb, nrec);
 			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
@@ -487,6 +487,75 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 		}
 	}
 
+	public void listDidIns2(String path, boolean keep) throws IOException {
+		open();
+		FileWriter writer = null;
+		if (path != null) {
+			writer = new FileWriter (path);
+		}
+		String header = "record "+
+		        "     master TS    "+
+		        "     local TS     "+
+		        "timeOfWeek " +
+		        " insStat "+
+		        " hdwStat "+
+		        "                     qn2b                     "+
+		        "           uvw                "+
+		        "                 lla                      \n"; 
+		if (writer != null) {
+			writer.write(header);
+		} else {
+			System.out.print(header);
+		}
+		int dbg_nrec = -5000;
+		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
+		for (int nrec = 0; nrec < num_recs; nrec++) {
+			if (nrec == dbg_nrec) {
+				System.out.println("listDidIns1(): nrec="+nrec);
+			}
+			int [] full_type = getFullType(bb, nrec);
+			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_2)) {
+				byte [] payload = getDidPayload(
+						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
+						nrec);
+				if (payload == null) {
+					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
+					break;
+				}
+				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
+				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
+				Did_ins_2 did_ins_2 = new Did_ins_2(bb_payload);
+				if ((did_ins_2.uvw[0] == 0) && (did_ins_2.uvw[1] == 0) && (did_ins_2.uvw[2] == 0)) {
+					continue;
+				}
+				String line =String.format(
+						"%6d %17.6f %17.6f %10.3f %08x %08x [%8.4f, %8.4f, %8.4f, %8.4f] "+
+				        "[%8.3f, %8.3f, %8.3f] [%12.7f, %12.7f, %12.7f]\n",
+						nrec,
+						getMasterTS(nrec),
+						getLocalTS(nrec),
+						did_ins_2.timeOfWeek,
+						did_ins_2.insStatus,
+						did_ins_2.hdwStatus,
+						did_ins_2.qn2b[0], did_ins_2.qn2b[1], did_ins_2.qn2b[2], did_ins_2.qn2b[3], // Quaternion body rotation with respect to NED: W, X, Y, Z
+						did_ins_2.uvw[0], did_ins_2.uvw[1], did_ins_2.uvw[2],                       // Velocity U, V, W in meters per second.  Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )".
+						did_ins_2.lla[0], did_ins_2.lla[1], did_ins_2.lla[2]                        //  WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
+						);
+				if (writer != null) {
+					writer.write(line);
+				} else {
+					System.out.print(line);
+				}
+			}
+		}
+		if (writer != null) {
+			writer.close();
+		}
+		if (!keep) {
+			close();
+		}
+	}
+	
 	public void listGPS (String path, int gps_mask, boolean keep) throws IOException {
 		// +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
 //		double max_offset = 1000; // m
@@ -594,6 +663,76 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 	}
 	
 	
+	public void listPimu(String path, boolean keep) throws IOException {
+		open();
+		FileWriter writer = null;
+		if (path != null) {
+			writer = new FileWriter (path);
+		}
+		String header = "record "+
+		        "     master TS    "+
+		        "     local TS     "+
+		        " time since boot  " +
+		        "    dt   "+
+		        "  status "+
+
+		        "           omega (rad/s)              "+
+		        "           a (m/s^2)             "+"\n"; 
+		if (writer != null) {
+			writer.write(header);
+		} else {
+			System.out.print(header);
+		}
+		int dbg_nrec = -5000;
+		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
+		for (int nrec = 0; nrec < num_recs; nrec++) {
+			if (nrec == dbg_nrec) {
+				System.out.println("listPimu(): nrec="+nrec);
+			}
+			int [] full_type = getFullType(bb, nrec);
+			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_PIMU)) {
+				byte [] payload = getDidPayload(
+						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
+						nrec);
+				if (payload == null) {
+					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
+					break;
+				}
+				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
+				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
+				Did_pimu did_pimu = new Did_pimu(bb_payload);
+				if ((did_pimu.vel[0] == 0) && (did_pimu.vel[1] == 0) && (did_pimu.vel[2] == 0)) {
+					continue; // skip no-movement
+				}
+				String line =String.format(
+						"%6d %17.6f %17.6f %17.6f %8.6f %08x [%9.5f, %9.5f, %9.5f] "+
+				        "[%9.4f, %9.4f, %9.4f]\n",
+						nrec,
+						getMasterTS(nrec),
+						getLocalTS(nrec),
+						did_pimu.time,
+						did_pimu.dt,
+						did_pimu.status,
+						did_pimu.theta[0]/did_pimu.dt, did_pimu.theta[1]/did_pimu.dt, did_pimu.theta[2]/did_pimu.dt, // Euler angles: roll, pitch, yaw in radians with respect to NED
+						did_pimu.vel[0]/did_pimu.dt, did_pimu.vel[1]/did_pimu.dt, did_pimu.vel[2]/did_pimu.dt       // Velocity U, V, W in meters per second.  Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )".
+						);
+				if (writer != null) {
+					writer.write(line);
+				} else {
+					System.out.print(line);
+				}
+			}
+		}
+		if (writer != null) {
+			writer.close();
+		}
+		if (!keep) {
+			close();
+		}
+	}
+	
+	
+	
 	byte [] getDidPayload(
 			EventLoggerFileInfo next_fileinfo, // may be null if payload does not extend beyond single record 
 			int nrec) throws IOException {

src/main/java/com/elphel/imagej/ims/Imx5.java

 package com.elphel.imagej.ims;
 
-import java.io.ByteArrayOutputStream;
-import java.io.IOException;
-import java.io.ObjectOutputStream;
+//import java.io.ByteArrayOutputStream;
+//import java.io.IOException;
+//import java.io.ObjectOutputStream;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
 
+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
+import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
+import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
+
+
 public class Imx5 {
 	
+	
 	// defines adapted from Inertial Sense data_sets.h 
 	public static int DID_NULL                        = 0 ; /** NULL (INVALID) */
 	public static int DID_DEV_INFO                    = 1 ; /** (dev_info_t) Device information */
@@ -119,6 +125,7 @@ public class Imx5 {
 	/** Count of data ids (including null data id 0) - MUST BE MULTPLE OF 4 and larger than last DID number! */
 	public static int DID_COUNT		= 120;	// Used in SDK
 	public static int DID_COUNT_UINS	= 100;	// Used in uINS
+	public static double EARTH_RADIUS =	6356752.0; // meters
 
 	/** Maximum number of data ids */
 	public static int DID_MAX_COUNT  = 256;
@@ -150,44 +157,80 @@ public class Imx5 {
 		Did_ins_1 did_ins_1_2 = new Did_ins_1(bb);
 		System.out.println("did_ins_1_2 = "+did_ins_1_2);
 		return bb.array();
-		
-/*
-ByteArrayOutputStream bos = new ByteArrayOutputStream();
-ObjectOutputStream out = null;
-try {
-  out = new ObjectOutputStream(bos);   
-  out.writeObject(yourObject);
-  out.flush();
-  byte[] yourBytes = bos.toByteArray();
-  ...
-} finally {
-  try {
-    bos.close();
-  } catch (IOException ex) {
-    // ignore close exception
-  }
-}
-
-Create an object from a byte array:
-
-ByteArrayInputStream bis = new ByteArrayInputStream(yourBytes);
-ObjectInput in = null;
-try {
-  in = new ObjectInputStream(bis);
-  Object o = in.readObject(); 
-  ...
-} finally {
-  try {
-    if (in != null) {
-      in.close();
-    }
-  } catch (IOException ex) {
-    // ignore close exception
-  }
-}
+	}
+//	static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
+//	RotationConvention.VECTOR_OPERATOR
+//RotationOrder.YXZ, ROT_CONV
+	public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) {
+		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
+		double [] rslt = new double[3];
+		if (inverse) {
+			ims_rot.applyInverseTo(vector, rslt);
 
- */
+		} else {
+			ims_rot.applyTo(vector, rslt);
+		}
+		return rslt;
+	}
+	
+	public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) {
+		RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
+//		RotationOrder ro =  zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
+		RotationOrder ro = RotationOrder.XYZ;
+		switch (ord) {
+		case 0: ro = RotationOrder.XYZ; break;
+		case 1: ro = RotationOrder.XZY; break;
+		case 2: ro = RotationOrder.YXZ; break;
+		case 3: ro = RotationOrder.YZX; break;
+		case 4: ro = RotationOrder.ZXY; break;
+		case 5: ro = RotationOrder.ZYX; break;
+		}
+		RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
+		Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
+//		Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
+		Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc,  -Math.PI/2, 0,  -Math.PI/2)):(new Rotation(ro, rc,  Math.PI/2, 0,  Math.PI/2));
+		double [] angles1 = cam_to_ims.getAngles(ro1, rc);
+		Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
+				//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
+		Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
+		double [] angles = comb_rot.getAngles(ro1, rc);
+		return angles;
 	}
 	
+	public static double [] imsQToCamRotations(double [] ims_qn2b, int ord, boolean rev_order, boolean rev_matrix ) {
+		RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
+//		RotationOrder ro =  zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
+		RotationOrder ro = RotationOrder.XYZ;
+		switch (ord) {
+		case 0: ro = RotationOrder.XYZ; break;
+		case 1: ro = RotationOrder.XZY; break;
+		case 2: ro = RotationOrder.YXZ; break;
+		case 3: ro = RotationOrder.YZX; break;
+		case 4: ro = RotationOrder.ZXY; break;
+		case 5: ro = RotationOrder.ZYX; break;
+		}
+		RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
+		Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
+//		Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
+		Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc,  -Math.PI/2, 0,  -Math.PI/2)):(new Rotation(ro, rc,  Math.PI/2, 0,  Math.PI/2));
+		double [] angles1 = cam_to_ims.getAngles(ro1, rc);
+		
+//		Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
+//  public Rotation(double q0, double q1, double q2, double q3,  boolean needsNormalization) {
+		Rotation ims_rot = new Rotation(ims_qn2b[0],ims_qn2b[1],ims_qn2b[2],ims_qn2b[3],true);
+		//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
+		Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
+		double [] angles = comb_rot.getAngles(ro1, rc);
+		return angles;
+	}
+
+	public static double [] nedFromLla (double [] lla, double [] lla_ref) {
+		double [] ned = new double[] { 
+				EARTH_RADIUS* Math.cos(lla[0] * Math.PI / 180)*(lla[1] - lla_ref[1]) * Math.PI / 180.0,
+				EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,
+				-(lla[2] - lla_ref[2])
+		};
+		return ned;
+	}
 	
 }

src/main/java/com/elphel/imagej/tileprocessor/QuadCLT.java

@@ -37,7 +37,7 @@ import java.nio.channels.Channels;
 import java.nio.channels.WritableByteChannel;
 import java.util.Arrays;
 import java.util.Properties;
-import java.util.Random;
+//import java.util.Random;
 import java.util.concurrent.atomic.AtomicInteger;
 import java.util.concurrent.atomic.DoubleAccumulator;
 
@@ -52,13 +52,13 @@ import com.elphel.imagej.correction.EyesisCorrections;
 import com.elphel.imagej.gpu.GPUTileProcessor;
 import com.elphel.imagej.gpu.GpuQuad;
 import com.elphel.imagej.gpu.TpTask;
-import com.elphel.imagej.tileprocessor.QuadCLTCPU.SetChannels;
+//import com.elphel.imagej.tileprocessor.QuadCLTCPU.SetChannels;
 
 import ij.IJ;
 import ij.ImagePlus;
 import ij.ImageStack;
-import ij.Prefs;
-import ij.io.FileSaver;
+//import ij.Prefs;
+//import ij.io.FileSaver;
 public class QuadCLT extends QuadCLTCPU {
 	int dbg_lev = 1;
 	private GpuQuad gpuQuad =              null; 	// use updateQuadCLT() to update after switching to a different

src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java

@@ -2982,7 +2982,7 @@ if (debugLevel > -100) return true; // temporarily !
 	 * @param quadCLT_aux    secondary RGB or LWIR QuadCLT instance
 	 * @param clt_parameters CLT parameters
 	 * @param skip_existing  do not copy if destination files exist
-	 * @param search_KML     search fpr geo data and extract it to KML file if found
+	 * @param search_KML     search for geo data and extract it to KML file if found
 	 * @param debugLevel     debug level
 	 */
 	public static void copyJP4src(
@@ -3012,6 +3012,7 @@ if (debugLevel > -100) return true; // temporarily !
 			set_name = set_channels[00].set_name;
 		}
 
+		
 		QuadCLT.SetChannels [] set_channels_main = quadCLT_main.setChannels(set_name,debugLevel); // only for specified image timestamp
 		QuadCLT.SetChannels [] set_channels_aux =  (quadCLT_aux !=null) ? quadCLT_aux.setChannels(set_name,debugLevel) : null;
 
@@ -3036,20 +3037,39 @@ if (debugLevel > -100) return true; // temporarily !
 				true,  // smart,
 				true);  //newAllowed, // save
 		File dir = (new File(jp4_copy_path)); // .getParentFile();
+		boolean already_copied = false;
 		if (!dir.exists()){
 			dir.mkdirs();
 			System.out.println("Created "+dir);
 		} else if (skip_existing) {
 			if (dir.list().length > 0) {
-				System.out.println("Skipping existing directory "+dir);
-				return;
+				already_copied = true;
 			}
 		}
+		
+		if (clt_parameters.imp.ims_use) {
+			String ims_path = dir.getParent() + Prefs.getFileSeparator()+set_name+quadCLT_main.correctionsParameters.imsSuffix;
+			double [] saved_offset =  quadCLT_main.readImsOffset(
+					ims_path, // String ims_path,
+					debugLevel); // int debugLevel)
+			if ((saved_offset == null) || (saved_offset[0] != clt_parameters.imp.ims_offset) || (saved_offset[1] != clt_parameters.imp.gmt_plus)) {
+				quadCLT_main.saveIms(
+						clt_parameters.imp.ims_offset,
+						clt_parameters.imp.gmt_plus,   //  double gmt_plus,
+						ims_path,
+						debugLevel);
+			}
+		}
+		
 		if (search_KML && (set_channels_main !=null)) { // TODO: make it look in both MAIN and AUX. Look in only the same scene?
 			quadCLT_main.writeKml(
 					set_name,
 					debugLevel ); // also generated with x3d model
 		}
+		if (already_copied) {
+			System.out.println("Skipping existing directory "+dir);
+			return;
+		}
 		for (String fname:path_list) {
 			if (fname.contains(set_name)) { // only files containing set name // TODO:improve
 				File file = new File(fname);