Added more DID sane checks - found DID_INS1 with wrong LLA

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

@@ -101,6 +101,18 @@ public class Did_gps_pos {
 		getProperties(prefix, properties);
 	}
 
+	public boolean isDidSane() { // add more!
+		if (    (lla[0] <-90) || (lla[0] > 90) || // latitude
+				(lla[1] <-90) || (lla[1] > 90) || // longitude
+				(lla[2] <-10) || (lla[1] > 20000)) { // altitude
+			System.out.println("isDidSane(): bad lla=["+
+				lla[0]+", "+lla[1]+", "+lla[2]+"], timeOfWeekMs="+timeOfWeekMs);
+			return false;
+		}
+		return true;
+	}
+	
+	
 	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;

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

@@ -91,6 +91,13 @@ public abstract class Did_ins <T extends Did_ins <T>>{
 					(insStatus ^ eInsStatusFlags_data) & eInsStatusFlags_mask));
 			return false;
 		}
+		if (    (lla[0] <-90) || (lla[0] > 90) || // latitude
+				(lla[1] <-90) || (lla[1] > 90) || // longitude
+				(lla[2] <-10) || (lla[1] > 20000)) { // altitude
+			System.out.println("isDidSane(): bad lla=["+
+				lla[0]+", "+lla[1]+", "+lla[2]+"], timeOfWeek="+timeOfWeek);
+			return false;
+		}
 		return true;
 	}
 	

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

@@ -7,6 +7,14 @@ import java.util.Properties;
 import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
 
 public class Did_pimu {
+	public static final int IMU_STATUS_IMU_OK_MASK = 0x003F0000; // lower bits may have error
+	public static final int IMU_STATUS_SATURATION_MASK = 0x0000003F;
+	public static final int IMU_STATUS_ZEROS = 0xffc0f800;
+	public static final double IMU_MAX_THETA = 10.0;   //rad/s
+	public static final double IMU_MAX_VEL =   20.0;   //m/s/s
+	public static final double IMU_MIN_PER =    0.008; // s
+	public static final double IMU_MAX_PER =    1.0;   // s
+	
 	/** Time since boot up in seconds.  Convert to GPS time of week by adding gps.towOffset */
 	public double    time;
 	/** Integral period in seconds for delta theta and delta velocity.  This is configured using DID_FLASH_CONFIG.startupNavDtMs. */
@@ -44,6 +52,39 @@ public class Did_pimu {
 		return new_did;
 	}
 	
+	public boolean isDidSane() {
+		if (((status ^ IMU_STATUS_IMU_OK_MASK) &  IMU_STATUS_IMU_OK_MASK) != 0) {
+			System.out.println("Did_pimu.isDidSane(): not OK status = "+String.format("0x%08x", status));
+			return false;
+		}
+		if ((status & IMU_STATUS_SATURATION_MASK) != 0) {
+			System.out.println("Did_pimu.isDidSane(): saturated status = "+String.format("0x%08x", status));
+			return false;
+		}
+		if ((status & IMU_STATUS_ZEROS) != 0) {
+			System.out.println("Did_pimu.isDidSane(): garbage status = "+String.format("0x%08x", status));
+			return false;
+		}
+		if (dt < IMU_MIN_PER) {
+			System.out.println("Did_pimu.isDidSane(): insane dt= "+dt+" s < "+IMU_MIN_PER+" s");
+			return false;
+		}
+		if (dt > IMU_MAX_PER) {
+			System.out.println("Did_pimu.isDidSane(): insane dt= "+dt+" s > "+IMU_MAX_PER+" s");
+			return false;
+		}
+		for (int i = 0; i < 3; i++) {
+			if (Math.abs(theta[i]) > IMU_MAX_THETA) {
+				System.out.println("Did_pimu.isDidSane(): insane theta ["+i+"]= "+theta[i]+" rad/s");
+				return false;
+			}
+			if (Math.abs(vel[i]) > IMU_MAX_VEL) {
+				System.out.println("Did_pimu.isDidSane(): insane vel ["+i+"]= "+vel[i]+" m/s/s");
+				return false;
+			}
+		}
+		return true;
+	}
 	
 	public int pack(ByteBuffer bb) {
 		int p_start = bb.position();

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

@@ -134,7 +134,12 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 					// Timestamp is frame start for images
 					int [] full_type = getFullType(bb, nrec);
 					if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
+						// is DID sane?
+						if (isDidSane(nrec, did)) {
 							break search_opposite; // return nrec;
+						} else {
+							System.out.println("getLastBeforeIndex()-1: Skipping insane DID="+did);
+						}
 					}
 				}
 			}
@@ -147,7 +152,12 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 			if (after?(ts_master_indx > ts_master) :(ts_master_indx <= ts_master)) {
 				int [] full_type = getFullType(bb, nrec);
 				if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
+					// is DID sane?
+					if (isDidSane(nrec, did)) {
 						return nrec;
+					} else {
+						System.out.println("getLastBeforeIndex()-2: Skipping insane DID="+did);
+					}
 				}
 			}
 		}
@@ -441,7 +451,40 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 		return true;
 	}
 	
+	public boolean isDidSane(int nrec, int type) {
+		//		int did_ins_type = type_ins_2 ? Imx5.DID_INS_2 : Imx5.DID_INS_1;
+		byte[] payload;
+		try {
+			payload = getDidPayload(
+					null, // next_fileinfo, // may be null if payload does not extend beyond single record 
+					nrec);
+		} catch (IOException e) {
+			// TODO Auto-generated catch block
+			System.out.println("isDidSane(): failed to read record "+nrec);
+			e.printStackTrace();
+			return false;
+		}
+		if (payload == null) {
+			System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
+			return false;
+		}
+		ByteBuffer bb_payload = ByteBuffer.wrap(payload);
+		bb_payload.order(ByteOrder.LITTLE_ENDIAN);
 
+		switch (type) {
+		case Imx5.DID_INS_1: 
+			return (new Did_ins_1(bb_payload)).isDidSane();
+		case Imx5.DID_INS_2: 
+			return (new Did_ins_2(bb_payload)).isDidSane();
+		case Imx5.DID_GPS1_POS: 
+		case Imx5.DID_GPS2_POS: 
+		case Imx5.DID_GPS1_UBX_POS: 
+			return (new Did_gps_pos(bb_payload)).isDidSane();
+		case Imx5.DID_PIMU: 
+			return (new Did_pimu(bb_payload)).isDidSane();
+		}
+		return true;
+	}
 	
 	/**
 	 * Absolute timing calibration with local logging of GPS 1pps events (with IMX-5 it is REC_TYPE_ODO)

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

@@ -5352,12 +5352,14 @@ public class Interscene {
 			double [] quat_lma_enu_xyz = (quatCorr != null) ?
 					Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false):
 						cam_xyz_enu;
+			// NED without ims_mount_atr correction
 			sb.append("\t"+cam_xyz1[0]+  "\t"+cam_xyz1[1]+  "\t"+cam_xyz1[2]); // 
+			// NED with ims_mount_atr correction
 			sb.append("\t"+cam_xyz2[0]+  "\t"+cam_xyz2[1]+  "\t"+cam_xyz2[2]); //
 
 			sb.append("\t"+cam_xyz_ned[0]+      "\t"+cam_xyz_ned[1]+      "\t"+cam_xyz_ned[2]); // 
-			sb.append("\t"+cam_xyz_enu[0]+      "\t"+cam_xyz_enu[1]+      "\t"+cam_xyz_enu[2]); //
-			sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); //
+			sb.append("\t"+cam_xyz_enu[0]+      "\t"+cam_xyz_enu[1]+      "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr
+			sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr
 			
 			double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
 			

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

@@ -98,6 +98,13 @@ public class IntersceneMatchParameters {
 				ims_mount_atr[1] * Math.PI/180,
 				ims_mount_atr[2] * Math.PI/180};
 	}
+	public void setImsMountATR( double [] atr) {
+		ims_mount_atr[0] = atr[0] * 180/Math.PI;
+		ims_mount_atr[1] = atr[1] * 180/Math.PI;
+		ims_mount_atr[2] = atr[2] * 180/Math.PI;
+	}
+
+	
 	public  boolean center_reference =   false;
 	public  boolean overlap_sequences =  false; // overlap sequences: scan down from the previous center  
 	public  boolean reset_photometric =  true;  // reset photometric calibration - once for each new series
@@ -106,7 +113,7 @@ public class IntersceneMatchParameters {
 	public  boolean run_ly =             false; // will return just after LY adjustments, skipping all output generation
 	public  int min_num_orient =         2; // make from parameters, should be >= 1
 	public  int min_num_interscene =     2; // make from parameters, should be >= 1
-
+	public  boolean adjust_imu_orient =  false;// adjust IMU misalignment to the camera 
 	public  boolean calc_quat_corr =     true; // calculate camera orientation correction from predicted by IMS 
     public  boolean apply_quat_corr =    true; // apply camera orientation correction from predicted by IMS
     public  boolean use_quat_corr =      true; // Use orientation correction everywhere if available
@@ -631,6 +638,8 @@ public class IntersceneMatchParameters {
 				"Minimal number of fitting scenes cycles, should be >=1. First cycle includes spiral search for the first scene");
 		gd.addNumericField("Minimal number of interscene accumulations", this.min_num_interscene, 0,3,"",
 				"Minimal required number of re-calculations of the interscene-accumulated DSI.");
+		gd.addCheckbox ("Adjust IMU orientation",                    this.adjust_imu_orient,
+				"Adjust IMU misalignment to the camera.");
 		gd.addCheckbox ("Calculate IMS orientation correction",      this.calc_quat_corr,
 				"Calculate camera orientation correction from predicted by IMS .");
 		gd.addCheckbox ("Apply IMS orientation correction ",         this.apply_quat_corr,
@@ -1415,6 +1424,7 @@ public class IntersceneMatchParameters {
 		this.run_ly =                         gd.getNextBoolean();
 		this.min_num_orient =           (int) gd.getNextNumber(); if (min_num_orient < 1) min_num_orient = 1;
 		this.min_num_interscene =       (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
+		this.adjust_imu_orient =              gd.getNextBoolean();
 		this.calc_quat_corr =                 gd.getNextBoolean();
 		this.apply_quat_corr =                gd.getNextBoolean();
 		this.use_quat_corr =                  gd.getNextBoolean();
@@ -1845,6 +1855,7 @@ public class IntersceneMatchParameters {
 		properties.setProperty(prefix+"run_ly",                        this.run_ly + "");                      // boolean
 		properties.setProperty(prefix+"min_num_orient",                this.min_num_orient+"");                // int
 		properties.setProperty(prefix+"min_num_interscene",            this.min_num_interscene+"");            // int
+		properties.setProperty(prefix+"adjust_imu_orient",             this.adjust_imu_orient+"");             // boolean
 		properties.setProperty(prefix+"calc_quat_corr",                this.calc_quat_corr+"");                // boolean
 		properties.setProperty(prefix+"apply_quat_corr",               this.apply_quat_corr+"");               // boolean
 		properties.setProperty(prefix+"use_quat_corr",                 this.use_quat_corr+"");                 // boolean
@@ -2240,6 +2251,7 @@ public class IntersceneMatchParameters {
 		if (properties.getProperty(prefix+"run_ly")!=null)                        this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly"));		
 		if (properties.getProperty(prefix+"min_num_orient")!=null)                this.min_num_orient=Integer.parseInt(properties.getProperty(prefix+"min_num_orient"));
 		if (properties.getProperty(prefix+"min_num_interscene")!=null)            this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
+		if (properties.getProperty(prefix+"adjust_imu_orient")!=null)             this.adjust_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_imu_orient"));
 		if (properties.getProperty(prefix+"calc_quat_corr")!=null)                this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
 		if (properties.getProperty(prefix+"apply_quat_corr")!=null)               this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
 		if (properties.getProperty(prefix+"use_quat_corr")!=null)                 this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
@@ -2662,6 +2674,7 @@ public class IntersceneMatchParameters {
 		imp.run_ly                        = this.run_ly;
 		imp.min_num_orient                = this.min_num_orient;
 		imp.min_num_interscene            = this.min_num_interscene;
+		imp.adjust_imu_orient             = this.adjust_imu_orient;
 		imp.calc_quat_corr                = this.calc_quat_corr;
 		imp.apply_quat_corr               = this.apply_quat_corr;
 		imp.use_quat_corr                 = this.use_quat_corr;

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

@@ -4658,6 +4658,7 @@ public class OpticalFlow {
 				save_mapped_mono_color[1]  || gen_avi_mono_color[1] || show_mono_color[1]} ;
 		// skip completely if no color or mono, tiff or video
 		
+		boolean adjust_imu_orient =          clt_parameters.imp.adjust_imu_orient; // calculate camera orientation correction from predicted by IMS
 		boolean calc_quat_corr =             clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
 		// apply_quat_corr used inside getGroundIns() - used when generating output
 //		boolean apply_quat_corr =            clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
@@ -5420,7 +5421,7 @@ public class OpticalFlow {
 			}
 		}
 		// later move to the right place
-		if ((quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
+		if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
 			double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
 					clt_parameters, // final CLTParameters clt_parameters,
 					quadCLTs,       // final QuadCLT[]     quadCLTs,
@@ -5438,7 +5439,7 @@ public class OpticalFlow {
 	    	double [] rms = new double[5];
 	    	double [] quat = new double[4];
 	    	int quat_lma_mode = 4; // 3; //  2; // 1;
-	    	int debug_lev = 3;
+	    	int debug_lev = debugLevel; // 3;
 	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		
 	    	double  translation_weight =   1.0 / (avg_z + 1.0);
 	    	
@@ -5456,7 +5457,33 @@ public class OpticalFlow {
 	    			rms,                //double []     rms, // null or double[5];
 	    			quat,               // double []     quaternion, // null or double[4]       
 	    			debug_lev); // int           debugLevel
-
+	    	if (rotated_xyzatr != null) {
+	        	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
+	        	if (debugLevel > -3) {
+	        		System.out.println("Applying correction ot the IMS mount orientation:");
+					double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
+					double []  corr_degrees = new double[3];
+					for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
+		    		System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
+		    		System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
+		    		System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
+	        	}
+	        	double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
+	        	double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
+	        			ims_mount_atr, // double [] ims_atr,
+	        			quat); // double [] corr_q)
+	        	clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
+	        	if (debugLevel > -3) {
+	        		double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
+					double [] degrees = new double[3];
+					for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
+		    		System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
+		    		System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
+		    		System.out.println ("*** Need to save the main configuration file ***");
+	        	}
+	    	} else {
+	    		System.out.println ("*** Failed to calculate IMS mount correction! ***");
+	    	}
 		}
 		if (run_ly) {
 			if (debugLevel > -3) {

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

@@ -1210,13 +1210,31 @@ public class QuaternionLma {
 			String pfx,
 			double [] data) {
 //		if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
-		if ((mode == 1) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
+		if (mode == 0 ) {
+			System.out.println(String.format("%3s"+
+					"\t%9s\t%9s\t%9s",
+					"N",pfx+"X",pfx+"Y",pfx+"Z"));
+			for (int nscene = 0; nscene < N; nscene++) {
+				System.out.println(String.format("%3d"+
+						"\t%9.5f\t%9.5f\t%9.5f",
+						nscene,
+						data[samples*nscene + 0],data[samples*nscene + 1],data[samples*nscene + 2]));
+			}
+			System.out.println();
+		} else  if ((mode == 1) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
 			System.out.println(String.format("%3s"+
 					"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ //x,y,z, q0,q1,q2,q3,a,t,r
 					"\t%9s\t%9s\t%9s",
 					"N",pfx+"X",pfx+"Y",pfx+"Z",pfx+"q0",pfx+"q1",pfx+"q2",pfx+"q3",
 					pfx+"A",pfx+"T",pfx+"R"));
 			for (int nscene = 0; nscene < N; nscene++) {
+				if (   (data[7*nscene + 3]*data[7*nscene + 3] +
+						data[7*nscene + 4]*data[7*nscene + 4] +
+						data[7*nscene + 5]*data[7*nscene + 5]+
+						data[7*nscene + 6]*data[7*nscene + 6]) < 0.001) {
+					System.out.println(String.format("%3d\t-\t-\t-\t-\t-\t-\t-\t-\t-\t-", nscene));
+					continue;
+				}
 				Rotation rot = new Rotation(data[7*nscene + 3],data[7*nscene + 4],data[7*nscene + 5],data[7*nscene + 6], false);
 				double []  angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
 				System.out.println(String.format("%3d"+