testing did is sane

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

@@ -16,10 +16,23 @@ public abstract class Did_ins <T extends Did_ins <T>>{
 	public int				hdwStatus; // uint32_t
 	/** WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters) */
 	public double []		lla =   new double [3];
+
+	/** for DID_INS verification */
+	public static int eHdwStatusFlags_mask = 0xffffffd0;
+    public static int eHdwStatusFlags_data = 0x0008007f;
+//	public static int eInsStatusFlags_mask = 0x8c0059ee;
+//	public static int eInsStatusFlags_mask = 0x8c00596e; // once in 300-400 - GPS aided heading
+	public static int eInsStatusFlags_mask = 0x8800596e; // once in 300-400 - INS_STATUS_RTK_COMPASSING_VALID
+	//
+    public static int eInsStatusFlags_data = 0x00635177;
+	public static int MIN_WEEK =             1766; // -10 yrs
+	public static int MAX_WEEK =             2806; // +10 yrs
+	
 	
 	static int interpolateInt(double frac, int v_this, int v_next) {
 		return (int) Math.round(frac * v_next + (1.0 - frac) * v_this);
 	}
+	
 	static float interpolateFloat(double frac, float v_this, float v_next) {
 		return (float) (frac * v_next + (1.0 - frac) * v_this);
 	}
@@ -36,6 +49,34 @@ public abstract class Did_ins <T extends Did_ins <T>>{
 		return new int [] { week, (int)Math.round(timeOfWeek * 1000.0)}; 
 	}
 	
+	public boolean isDidSane() {
+		if ((week < MIN_WEEK) || (week > MAX_WEEK)) {
+			System.out.println("isDidSane(): bad week = "+week+
+					" - should be in range ["+MIN_WEEK+","+MAX_WEEK+"]");
+			return false;
+		}
+		if ((timeOfWeek < 0) || (timeOfWeek > WEEK_SECONDS)) {
+			System.out.println("isDidSane(): bad timeOfWeek = "+timeOfWeek+
+					" - should be in range [0,"+WEEK_SECONDS+"]");
+			return false;
+		}
+		if (((hdwStatus ^ eHdwStatusFlags_data) & eHdwStatusFlags_mask) != 0) {
+			System.out.println(String.format("isDidSane(): bad hdwStatus=0x%08x"+
+					" (0x%08x ^ 0x%08x) & 0x%08x = 0x%08x",
+					hdwStatus, hdwStatus, eHdwStatusFlags_data,eHdwStatusFlags_mask,
+					(hdwStatus ^ eHdwStatusFlags_data) & eHdwStatusFlags_mask));
+			return false;
+		}
+		if (((insStatus ^ eInsStatusFlags_data) & eInsStatusFlags_mask) != 0) {
+			System.out.println(String.format("isDidSane(): bad insStatus=0x%08x"+
+					" (0x%08x ^ 0x%08x) & 0x%08x = 0x%08x",
+					insStatus, insStatus, eInsStatusFlags_data,eInsStatusFlags_mask,
+					(insStatus ^ eInsStatusFlags_data) & eInsStatusFlags_mask));
+			return false;
+		}
+		return true;
+	}
+	
 //https://www.swtestacademy.com/return-subclass-instance-java-generics/	
 	public void interpolateBase(double frac, T next_did, T new_did) {
 		new_did.week =     this.week;