Added more DID sane checks - found DID_INS1 with wrong LLA (2014ae08) · Commits · Elphel / imagej-elphel

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

+12 −0

Original line number	Diff line number	Diff line
		@@ -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

+7 −0

Original line number	Diff line number	Diff line
		@@ -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

+41 −0

Original line number	Diff line number	Diff line
		@@ -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

+46 −3

Original line number	Diff line number	Diff line
		@@ -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/ims/Imx5.java

+116 −102

File changed.

Preview size limit exceeded, changes collapsed.

Admin message