Skip to content

I
imagej-elphel
Commit b9a32b13 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Improved DID_INS_* interpolation by local correction of ts_master to GPS 

time using near 1pps sample
parent 2b8ae048
Show whitespace changes
Inline Side-by-side
Showing with 702 additions and 291 deletions
src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java
View file @ b9a32b13
......@@ -5525,14 +5525,20 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
EYESIS_CORRECTIONS.setDebug(DEBUG_LEVEL);
CLT_PARAMETERS.batch_run = true;
String imx_logs = CORRECTION_PARAMETERS.getImsSourceDirectory(); // sourceImsDirectory;
String debug_path = null; // CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"debug_local_master";
if (EVENT_LOGGER == null) {
if ((imx_logs != null) && !imx_logs.equals("")) {
EVENT_LOGGER = new EventLogger (imx_logs);
EVENT_LOGGER = new EventLogger (
imx_logs,
debug_path);
} else {
System.out.println("sourceImsDirectory is not set. Use Setup CLT Batch parameters to define it.");
}
}
if (EVENT_LOGGER != null) {
//print2Pps
EVENT_LOGGER.print2Pps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"rec_1pps");
EVENT_LOGGER.printStrobeInTime(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_strobe_in_time");
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");
......
src/main/java/com/elphel/imagej/ims/Did_ins.java
View file @ b9a32b13
......@@ -28,6 +28,14 @@ public abstract class Did_ins <T extends Did_ins <T>>{
return frac * v_next + (1.0 - frac) * v_this;
}
public double getDoubleTime() {
return ((double) week) * WEEK_SECONDS + timeOfWeek;
}
public int [] getWeekTimeMs() { // for compatibility with DID_STROBE_IN_TIME
return new int [] { week, (int)Math.round(timeOfWeek * 1000.0)};
}
//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;
......
src/main/java/com/elphel/imagej/ims/Did_strobe_in_time.java
View file @ b9a32b13
......@@ -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_STROBE_IN_TIME) Timestamp for input strobe.
typedef struct PACKED
......@@ -121,4 +124,25 @@ public class Did_strobe_in_time {
s += String.format("count: %d\n", count);
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+"pin")!=null) this.pin= Integer.parseInt(properties.getProperty(prefix+"pin"));
if (properties.getProperty(prefix+"count")!=null) this.count= Integer.parseInt(properties.getProperty(prefix+"count"));
}
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+"pin", this.pin+"");
properties.setProperty(prefix+"count", this.count+"");
return properties;
}
}
\ No newline at end of file
src/main/java/com/elphel/imagej/ims/EventLogger.java
View file @ b9a32b13
......@@ -14,8 +14,7 @@ import ij.Prefs;
public class EventLogger {
public EventLoggerFileInfo [] logger_files;
public EventLogger (String dir) { // dir should contain only event logger files
public EventLogger (String dir, String debug_prefix) { // dir should contain only event logger files
if (!dir.endsWith(Prefs.getFileSeparator())) {
dir+=Prefs.getFileSeparator();
}
......@@ -26,8 +25,9 @@ public class EventLogger {
.collect(Collectors.toSet()));
logger_files = new EventLoggerFileInfo [files.size()];
for (int nf = 0; nf < logger_files.length; nf++) { // may use threads (per file)
String debug_path= (debug_prefix != null) ? (debug_prefix+nf+".csv") : null;
try {
logger_files[nf] = new EventLoggerFileInfo(dir+files.get(nf), false);
logger_files[nf] = new EventLoggerFileInfo(dir+files.get(nf), false, debug_path);
} catch (IOException e) {
logger_files[nf] = null;
e.printStackTrace();
......@@ -38,43 +38,6 @@ public class EventLogger {
// 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();
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();
int gps_mask = 7; // +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
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();
}
}
*/
// 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);
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].listDid1(out_path,false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
*/
System.out.println("Processed "+logger_files.length+" event log files");
}
......@@ -86,7 +49,7 @@ public class EventLogger {
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";
String out_path= out_did_gps_path+"-"+String.format("%03d", nf)+".csv";
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);
......@@ -104,7 +67,7 @@ public class EventLogger {
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";
String out_path= out_did1_path+"-"+String.format("%03d", nf)+".csv";
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);
......@@ -121,7 +84,7 @@ public class EventLogger {
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";
String out_path= out_did2_path+"-"+String.format("%03d", nf)+".csv";
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);
......@@ -138,7 +101,7 @@ public class EventLogger {
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";
String out_path= out_pimu_path+"-"+String.format("%03d", nf)+".csv";
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);
......@@ -149,6 +112,40 @@ public class EventLogger {
}
}
public void printStrobeInTime(String out_sit_path) {
out_sit_path = Paths.get(out_sit_path).normalize().toString();
Path op_sit = Paths.get(out_sit_path);
op_sit=op_sit.normalize();
(new File(op_sit.getParent().toString())).mkdirs();
for (int nf = 0; nf < logger_files.length; nf++) {
String out_path= out_sit_path+"-"+String.format("%03d", nf)+".csv";
System.out.println("Printing all DID_STROBE_IN_TIME data in "+logger_files[nf].abs_path+" to "+out_path);
try {
logger_files[nf].listStrobeInTime(out_path,false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
public void print2Pps(String out_1pps_path) {
out_1pps_path = Paths.get(out_1pps_path).normalize().toString();
Path op_1pps = Paths.get(out_1pps_path);
op_1pps=op_1pps.normalize();
(new File(op_1pps.getParent().toString())).mkdirs();
for (int nf = 0; nf < logger_files.length; nf++) {
String out_path= out_1pps_path+"-"+String.format("%03d", nf)+".csv";
System.out.println("Printing all DID_STROBE_IN_TIME data in "+logger_files[nf].abs_path+" to "+out_path);
try {
logger_files[nf].list1Pps(out_path,false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
public void testInterpolateDidIns1() {
boolean exit_now = false;
......@@ -353,6 +350,29 @@ public class EventLogger {
}
return Double.NaN;
}
// if it is DID_INS_1 or DID_INS_2 use TOW for calculating fraction
if ((did == Imx5.DID_INS_1) || (did == Imx5.DID_INS_2)) {
double tow_corr = logger_files[file_index_below].getTimeOfWeekCorrection(ts_master);
int week0, week1;
double tow0, tow1;
if (!Double.isNaN(tow_corr)) {
double tow = logger_files[file_index_below].getLinearTimeOfWeek(ts_master) - tow_corr;
@SuppressWarnings("rawtypes")
Did_ins did_ins_0 = (did == Imx5.DID_INS_2) ?
(new Did_ins_2(ByteBuffer.wrap(payload[0]))) : (new Did_ins_1(ByteBuffer.wrap(payload[0])));
@SuppressWarnings("rawtypes")
Did_ins did_ins_1 = (did == Imx5.DID_INS_2) ?
(new Did_ins_2(ByteBuffer.wrap(payload[1]))) : (new Did_ins_1(ByteBuffer.wrap(payload[1])));
tow0 = did_ins_0.timeOfWeek;
tow1 = did_ins_1.timeOfWeek;
if (did_ins_1.week > did_ins_0.week) {
tow1+= Did_ins.WEEK_SECONDS;
}
frac = (tow - tow0) /(tow1-tow0);
} else {
System.out.println("ERROR: getDidAtTS(): tow_corr is NaN, keeping frac="+frac);
}
}
return frac;
}
......
src/main/java/com/elphel/imagej/ims/EventLoggerFileInfo.java
View file @ b9a32b13
package com.elphel.imagej.ims;
import java.awt.Point;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Arrays;
......@@ -14,6 +16,7 @@ import java.util.Comparator;
import java.util.HashSet;
import java.util.Set;
public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
public static final int REC_SIZE = 64;
// bits 24.27 of local usec
......@@ -167,7 +170,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
}
public EventLoggerFileInfo (String abs_path, boolean keep) throws IOException {
public EventLoggerFileInfo (String abs_path, boolean keep, String debug_path) throws IOException {
int debugLevel = 0;
this.abs_path = abs_path;
System.out.println("EventLoggerFileInfo(): processing "+abs_path);
......@@ -209,72 +212,58 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
for (int nt = 0; nt < rec_types.length; nt++) {
type_lut[rec_types[nt][0] * num_subs + rec_types[nt][1]] = nt;
}
rec_first_last = new int [rec_types.length][];
{ // processing local to master timestamps correspondence
double ts_local0 = Double.NaN;
double ts_master0 = Double.NaN;
double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
int nimg = 0;
for (int nrec = 0; nrec < num_recs; nrec++) {
int [] full_type = getFullType(bb, nrec);
if (full_type != null) {
int ti = type_lut [full_type[0] * num_subs + full_type[1]];
if (rec_first_last[ti] == null) {
rec_first_last[ti] = new int [] {nrec,nrec};
} else {
rec_first_last[ti][1] = nrec;
}
if ((full_type[0] >> 4) == REC_TYPE_IMG) {
double ts_local = getLocalTS(bb, nrec);
double ts_master = getMasterTSImg(bb, nrec);
if (debugLevel > 0) { // -1
System.out.println (
String.format("%6d %6d %10.6f",nimg, nrec, ts_master));
}
nimg++;
if (Double.isNaN(ts_local0)) {
ts_local0 = ts_local;
ts_master0 = ts_master;
}
ts_local -= ts_local0;
ts_master -= ts_master0;
s0 += 1;
sx += ts_master;
sx2 += ts_master * ts_master;
sy += ts_local;
sxy += ts_master * ts_local;
}
// just print time and gps:
int type_imx5 = (REC_TYPE_GPS << 4) | REC_SUBTYPE_IMX5; // 0x18 - first subpacket
if ((full_type[0] >> 4) == type_imx5) {
}
processLocalToMaster(
debug_path, // String debug_out_path,
debugLevel
); // false if no data
// Timing calibration with DID_STROBE_IN_TIME
// can be inaccurate if serial data from IMS has to wait. Also sometimes DID_STROBE_IN_TIME was stopping unexplained
// if file contains DID_STROBE_IN_TIME, calculate absolute week an time of week in milliseconds
if (debugLevel > -1) {
System.out.println("calibrateFromStrobeInTime()");
}
calibrateFromStrobeInTime(debugLevel);
if (debugLevel > -1) {
System.out.println("calibrateFromDidIns(false)");
}
// calculate offsets and timestamp range (master?)
double d = (s0 * sx2 - sx * sx);
double a = (sxy * s0 - sy * sx) / d;
double b = (sx2 * sy - sx * sxy) / d;
ts_local_start = getLocalTS(bb, 0);
double ts_local_end = getLocalTS(bb, num_recs - 1);
local_rel_lag = a;
ts_master_start = ts_master0 + (ts_local_start - ts_local0 - b) / local_rel_lag;
calibrateFromDidIns(false, debugLevel);
local_lag = ts_local_start - ts_master_start;
ts_master_end = ts_master_start + (ts_local_end - ts_local_start) / local_rel_lag;
if (debugLevel > -1) {
System.out.println("calibrateFromDidIns(true)");
}
first_last_ts = new double [rec_first_last.length][2];
for (int i = 0; i < rec_first_last.length; i++) {
for (int j = 0; j < rec_first_last[i].length; j++) {
double ts_local = getLocalTS(bb, rec_first_last[i][j]);
first_last_ts[i][j] = ts_master_start + (ts_local - ts_local_start) / local_rel_lag ;
calibrateFromDidIns(true, debugLevel);
// Timing calibration with local logging of GPS 1pps events (with IMX-5 it is REC_TYPE_ODO)
// Only fractional seconds can be calibrated, the record appears 0.1 sec after the event, local
// timestamp corresponds to the actual leading edge of the 1pps pulse.
calibrateFrom1Pps(debugLevel); // false if no data
if (!keep) {
close();
}
}
// Timing calibration with DID_STROBE_IN_TIME
/**
* Absolute timing calibration with DID_STROBE_IN_TIME
* can be inaccurate if serial data from IMS has to wait. Also sometimes DID_STROBE_IN_TIME was stopping unexplained
* if file contains DID_STROBE_IN_TIME, calculate absolute week an time of week in milliseconds
*
* @param debugLevel - debug level
* @return false if no pps data is available, true if OK
* @throws IOException
*/
public boolean calibrateFromStrobeInTime(int debugLevel) throws IOException {
// Absolute Timing calibration with DID_STROBE_IN_TIME
// can be inacurate if serial data from IMS has to wait. Also sometimes DID_STROBE_IN_TIME was stopping unexplained
// if file contains DID_STROBE_IN_TIME, calculate absolute week an time of week in milliseconds
int type_imx5 = (REC_TYPE_GPS << 4) | REC_SUBTYPE_IMX5; // 0x18 - first subpacket
int indx_strobe_in_time = getTypeIndex (type_imx5, Imx5.DID_STROBE_IN_TIME);
if (indx_strobe_in_time >=0) {
if (indx_strobe_in_time < 0) {
System.out.println("calibrateFromStrobeInTime(): no DID_STROBE_IN_TIME available, aborting.");
return false;
}
double [] first_last_ts = getFirstLastTS(type_imx5, Imx5.DID_STROBE_IN_TIME); // master timestamps
int [] first_last_index = getFirstLastIndex(type_imx5, Imx5.DID_STROBE_IN_TIME);
if (first_last_ts[1] > first_last_ts[0]) {
......@@ -339,19 +328,117 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
week_time0, // int [] week_time_ms,
b + (ts_master_end - ts_master0) * a);
if (debugLevel > -1) {
System.out.println(String.format("gnss_start: week=%d, sec=%f", gnss_start[0], 0.001*gnss_start[1]));
System.out.println(String.format("gnss_end: week=%d, sec=%f", gnss_end[0], 0.001*gnss_end[1]));
double offs_0 = 1694300600.0;
System.out.println(String.format("gnss_start: week=%d, sec=%f, diff=%f",
gnss_start[0], 0.001*gnss_start[1], (ts_master_start - 0.001*gnss_start[1] - offs_0)));
System.out.println(String.format("gnss_end: week=%d, sec=%f, diff=%f",
gnss_end[0], 0.001*gnss_end[1], (ts_master_end - 0.001*gnss_end[1] - offs_0)));
}
}
return true;
}
// Timing calibration with local logging of GPS 1pps events (with IMX-5 it is REC_TYPE_ODO)
// Only fractional seconds can be calibrated, the record appears 0.1 sec after the event, local
// timestamp corresponds to the actual leading edge of the 1pps pulse.
/**
* Absolute timing calibration with DID_INS_1 and DID_INS_2
* Is inaccurate as serial data from IMS has to wait.
* Used only to find full seconds correspondence, fraction seconds will be found
* by calibrateFrom1Pps()
* @param type_ins_2 - true - use DID_INS_2, false - use DID_INS_1
* @param debugLevel - debug level
* @return false if no data is available, true if OK
* @throws IOException
*/
public boolean calibrateFromDidIns(boolean type_ins_2, int debugLevel) throws IOException {
int did_ins_type = type_ins_2 ? Imx5.DID_INS_2 : Imx5.DID_INS_1;
int type_imx5 = (REC_TYPE_GPS << 4) | REC_SUBTYPE_IMX5; // 0x18 - first subpacket
int indx_did_ins = getTypeIndex (type_imx5, did_ins_type);
if (indx_did_ins < 0){
System.out.println("calibrateFromDidIns(): no DID_INS_* available, aborting.");
return false;
}
double [] first_last_ts = getFirstLastTS(type_imx5, did_ins_type); // master timestamps
int [] first_last_index = getFirstLastIndex(type_imx5, did_ins_type);
if (first_last_ts[1] > first_last_ts[0]) {
// processing master timestamps to
double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
int [] week_time0 = null;
double ts_master0 = Double.NaN;
// double full_time0 = Double.NaN;
for (int nrec = first_last_index[0]; nrec <= first_last_index[1]; nrec++) {
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type_imx5) && (full_type[1] == did_ins_type)) {
double ts_master = getMasterTS(bb, nrec);
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);
@SuppressWarnings("rawtypes")
Did_ins did_ins = type_ins_2 ? (new Did_ins_2(bb_payload)) : (new Did_ins_1(bb_payload));
// double full_time = did_ins.getDoubleTime();
int [] week_time = did_ins.getWeekTimeMs();
if (week_time0 == null) {
week_time0 = week_time;
ts_master0 = ts_master;
}
double ts_gnss = 0.0;
ts_gnss = Did_strobe_in_time.getWeekTimeMsDelta(week_time0, week_time);
ts_master -= ts_master0;
s0 += 1;
sx += ts_master;
sx2 += ts_master * ts_master;
sy += ts_gnss;
sxy += ts_master * ts_gnss;
if (debugLevel > 0) { // -1
System.out.println (
String.format("%6d %10.6f %10.6f", nrec, ts_master, ts_gnss));
}
}
}
double d = (s0 * sx2 - sx * sx);
double a = (sxy * s0 - sy * sx) / d;
double b = (sx2 * sy - sx * sxy) / d;
gnss_start = Did_strobe_in_time.getWeekTimeMs(
week_time0, // int [] week_time_ms,
b + (ts_master_start - ts_master0) * a);
gnss_end = Did_strobe_in_time.getWeekTimeMs(
week_time0, // int [] week_time_ms,
b + (ts_master_end - ts_master0) * a);
if (debugLevel > -1) {
double offs_0 = 1694300600.0;
System.out.println(String.format("gnss_did_start: week=%d, sec=%f, diff=%f",
gnss_start[0], 0.001*gnss_start[1], (ts_master_start - 0.001*gnss_start[1] - offs_0)));
System.out.println(String.format("gnss_did_end: week=%d, sec=%f, diff=%f",
gnss_end[0], 0.001*gnss_end[1], (ts_master_end - 0.001*gnss_end[1] - offs_0)));
}
}
return true;
}
/**
* Absolute timing calibration with local logging of GPS 1pps events (with IMX-5 it is REC_TYPE_ODO)
* Only fractional seconds can be calibrated, the record appears 0.1 sec after the event, local
* timestamp corresponds to the actual leading edge of the 1pps pulse.
*
* @param debugLevel - debug level
* @return false if no pps data is available, true if OK
*/
public boolean calibrateFrom1Pps(int debugLevel) {
int type_1pps = REC_TYPE_ODO << 4;
int indx_1pps = getTypeIndex (type_1pps, 0);
if (indx_1pps >= 0) {
if (indx_1pps < 0) {
System.out.println("calibrateFrom1Pps(): no , aborting.");
return false;
}
double [] first_last_ts = getFirstLastTS(type_1pps, 0); // master timestamps
int [] first_last_index = getFirstLastIndex(type_1pps, 0);
if (first_last_ts[1] > first_last_ts[0]) { // has >=2 records of type REC_TYPE_ODO
......@@ -395,13 +482,109 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
System.out.println(String.format("gnss_frac_end: %f", gnss_frac_end));
}
}
return true;
}
/**
* processing local to master timestamps correspondence
* @param debug_out_path - null or file path to save master/local TS matching
* @param debugLevel - debug level
* @throws IOException
*/
public void processLocalToMaster(
String debug_out_path,
int debugLevel) throws IOException {
rec_first_last = new int [rec_types.length][];
double ts_local0 = Double.NaN;
double ts_master0 = Double.NaN;
double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
int nimg = 0;
for (int nrec = 0; nrec < num_recs; nrec++) {
int [] full_type = getFullType(bb, nrec);
if (full_type != null) {
int ti = type_lut [full_type[0] * num_subs + full_type[1]];
if (rec_first_last[ti] == null) {
rec_first_last[ti] = new int [] {nrec,nrec};
} else {
rec_first_last[ti][1] = nrec;
}
if ((full_type[0] >> 4) == REC_TYPE_IMG) {
double ts_local = getLocalTS(bb, nrec);
double ts_master = getMasterTSImg(bb, nrec);
if (debugLevel > 0) { // -1
System.out.println (
String.format("%6d %6d %10.6f",nimg, nrec, ts_master));
}
nimg++;
if (Double.isNaN(ts_local0)) { // first image - sample local and master
ts_local0 = ts_local;
ts_master0 = ts_master;
}
ts_local -= ts_local0;
ts_master -= ts_master0;
s0 += 1;
sx += ts_master;
sx2 += ts_master * ts_master;
sy += ts_local;
sxy += ts_master * ts_local;
}
// just print time and gps:
int type_imx5 = (REC_TYPE_GPS << 4) | REC_SUBTYPE_IMX5; // 0x18 - first subpacket
if ((full_type[0] >> 4) == type_imx5) {
}
}
}
// calculate offsets and timestamp range (master?)
double d = (s0 * sx2 - sx * sx);
double a = (sxy * s0 - sy * sx) / d;
double b = (sx2 * sy - sx * sxy) / d;
ts_local_start = getLocalTS(bb, 0);
double ts_local_end = getLocalTS(bb, num_recs - 1);
local_rel_lag = a;
ts_master_start = ts_master0 + (ts_local_start - ts_local0 - b) / local_rel_lag;
local_lag = ts_local_start - ts_master_start;
ts_master_end = ts_master_start + (ts_local_end - ts_local_start) / local_rel_lag;
first_last_ts = new double [rec_first_last.length][2];
for (int i = 0; i < rec_first_last.length; i++) {
for (int j = 0; j < rec_first_last[i].length; j++) {
double ts_local = getLocalTS(bb, rec_first_last[i][j]);
first_last_ts[i][j] = ts_master_start + (ts_local - ts_local_start) / local_rel_lag ;
}
}
if (debug_out_path != null) {
debug_out_path = Paths.get(debug_out_path).normalize().toString();
Path op = Paths.get(debug_out_path);
op=op.normalize();
(new File(op.getParent().toString())).mkdirs();
nimg = 0;
FileWriter writer = new FileWriter (debug_out_path);
String header =
"image\t"+
"nrec\t" +
"local TS\t"+
"master TS\t"+
"predicted master\t"+
"prediction error\n";
writer.write(header);
if (!keep) {
close();
for (int nrec = 0; nrec < num_recs; nrec++) {
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && ((full_type[0] >> 4) == REC_TYPE_IMG)) {
double ts_local = getLocalTS(bb, nrec);
double ts_master = getMasterTSImg(bb, nrec);
double ts_master_calc = getMasterTS(ts_local);
writer.write(
String.format("%6d\t%6d\t%10.6f\t%10.6f\t%10.6f\t%10.6f\n",
nimg, nrec, ts_local, ts_master, ts_master_calc, ts_master-ts_master_calc));
nimg++;
}
}
writer.close();
}
return;
}
public void listDidIns1(String path, boolean keep) throws IOException {
double max_offset = 1000; // m
......@@ -411,16 +594,16 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
if (path != null) {
writer = new FileWriter (path);
}
String header = "record "+
" master TS "+
" local TS "+
"timeOfWeek " +
" insStat "+
" hdwStat "+
" theta "+
" uvw "+
" lla "+
" ned \n";
String header = "record\t"+
"master TS\t"+
"local TS\t"+
"timeOfWeek\t" +
"insStat\t"+
"hdwStat\t"+
"theta-0\ttheta-1\ttheta-2\t"+
"u\tv\tw\t"+
"lat\tlong\talt\t"+
"ned:N\tned:E\tned:D\n";
if (writer != null) {
writer.write(header);
} else {
......@@ -459,8 +642,8 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
}
start_ned = new float[] {0,0,0};
String line =String.format(
"%6d %17.6f %17.6f %10.3f %08x %08x [%8.4f, %8.4f, %8.4f] "+
"[%8.3f, %8.3f, %8.3f] [%12.7f, %12.7f, %12.7f] [%8.3f, %8.3f, %8.3f]\n",
"%6d\t%17.6f\t%17.6f\t%10.3f\t%08x\t%08x\t%8.4f\t%8.4f\t%8.4f\t"+
"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\t%8.3f\t%8.3f\t%8.3f\n",
nrec,
getMasterTS(nrec),
getLocalTS(nrec),
......@@ -493,15 +676,15 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
if (path != null) {
writer = new FileWriter (path);
}
String header = "record "+
" master TS "+
" local TS "+
"timeOfWeek " +
" insStat "+
" hdwStat "+
" qn2b "+
" uvw "+
" lla \n";
String header = "record\t"+
"master TS\t"+
"local TS\t"+
"timeOfWeek\t" +
"insStat\t"+
"hdwStat\t"+
"qn2b0\tqn2b1\tqn2b2\tqn2b3\t"+
"u\tv\tw\t"+
"lat\tlong\talt\n";
if (writer != null) {
writer.write(header);
} else {
......@@ -529,8 +712,8 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
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",
"%6d\t%17.6f\t%17.6f\t%10.3f\t%08x\t%08x\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t"+
"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\n",
nrec,
getMasterTS(nrec),
getLocalTS(nrec),
......@@ -565,23 +748,23 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
if (path != null) {
writer = new FileWriter (path);
}
String header = "record "+
" master TS "+
" local TS "+
"timeOfWeek " +
"did "+
" status "+
" ecef "+
" lla "+
" hMSL "+ // 7.2f
" hAcc "+ // 7.3f
" vAcc "+ // 7.3f
" pDop "+ // 7.3f
"cnoMean "+ // 7.3f
" towOffset "+ // 11.4f
" lS " + // 3d
"sat " + // 3d
"sgm " + // 3d
String header = "record\t"+
" master TS\t"+
" local TS\t"+
"timeOfWeek\t" +
"did\t"+
"status\t"+
"ecef-X\tecef-Y\tecef-Z\t"+
"lat\tlong\talt\t"+
"hMSL\t"+ // 7.2f
"hAcc\t"+ // 7.3f
"vAcc\t"+ // 7.3f
"pDop\t"+ // 7.3f
"cnoMean\t"+ // 7.3f
"towOffset\t"+ // 11.4f
"lS\t" + // 3d
"sat\t" + // 3d
"sgm\t" + // 3d
"rsv\n";// 3d
if (writer != null) {
writer.write(header);
......@@ -613,28 +796,10 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
if ((did_gps_pos.lla[0] == 0) && (did_gps_pos.lla[1] == 0) && (did_gps_pos.lla[2] == 0)) {
continue;
}
// replace with GPS fix type?
/*
if ((did_ins_1.uvw[0] == 0) && (did_ins_1.uvw[1] == 0) && (did_ins_1.uvw[2] == 0)) {
continue;
}
if ((start_ned != null) &&
( (Math.abs(start_ned[0] - did_ins_1.ned[0]) > max_offset) ||
(Math.abs(start_ned[1] - did_ins_1.ned[1]) > max_offset) ||
(Math.abs(start_ned[2] - did_ins_1.ned[2]) > max_offset)
)){
start_ned = null;
}
if (start_ned == null) {
start_ned = did_ins_1.ned.clone();
}
start_ned = new float[] {0,0,0};
*/
String line =String.format(
"%6d %17.6f %17.6f %10.3f %3d %08x [%13.4f, %13.4f, %13.4f] "+
"[%12.7f, %12.7f, %12.7f] %7.2f %7.3f %7.3f %7.3f %7.3f %11.4f "+
"%3d %3d %3d %3d\n",
"%6d\t%17.6f\t%17.6f\t%10.3f\t%3d\t%08x\t%13.4f\t%13.4f\t%13.4f\t"+
"%12.7f\t%12.7f\t%12.7f\t%7.2f\t%7.3f\t%7.3f\t%7.3f\t%7.3f\t%11.4f\t"+
"%3d\t%3d\t%3d\t%3d\n",
nrec,
getMasterTS(nrec),
getLocalTS(nrec),
......@@ -669,15 +834,14 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
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";
String header = "record\t"+
"master TS\t"+
"local TS\t"+
"time since boot\t" +
"dt\t"+
"status\t"+
"omega-X (rad/s)\tomega-Y (rad/s)\tomega-Z (rad/s)\t"+
"a-X (m/s^2)\ta-Y (m/s^2)\ta-Z (m/s^2)\n";
if (writer != null) {
writer.write(header);
} else {
......@@ -705,8 +869,8 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
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",
"%6d\t%17.6f\t%17.6f\t%17.6f\t%8.6f\t%08x\t%9.5f\t%9.5f\t%9.5f\t"+
"%9.4f\t%9.4f\t%9.4f\n",
nrec,
getMasterTS(nrec),
getLocalTS(nrec),
......@@ -731,6 +895,131 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
}
}
public void listStrobeInTime(String path, boolean keep) throws IOException {
open();
FileWriter writer = null;
if (path != null) {
writer = new FileWriter (path);
}
String header = "record\t"+
"master TS\t"+
"local TS\t"+
"week\t" +
"timeOfWeek\t" +
"pin\t"+
"count"+"\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("listStrobeInTime(): nrec="+nrec);
}
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_STROBE_IN_TIME)) {
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_strobe_in_time did_strobe_in_time = new Did_strobe_in_time(bb_payload);
if (did_strobe_in_time.week == 0) {
continue; // skip no-movement
}
String line =String.format(
"%6d\t%17.6f\t%17.6f\t%7d\t%10.3f\t%3d\t%7d\n",
nrec,
getMasterTS(nrec),
getLocalTS(nrec),
did_strobe_in_time.week,
0.001 * did_strobe_in_time.timeOfWeekMs,
did_strobe_in_time.pin,
did_strobe_in_time.count);
if (writer != null) {
writer.write(line);
} else {
System.out.print(line);
}
}
}
if (writer != null) {
writer.close();
}
if (!keep) {
close();
}
}
public void list1Pps(String path, boolean keep) throws IOException {
open();
FileWriter writer = null;
if (path != null) {
writer = new FileWriter (path);
}
String header = "record\t"+
"master TS\t"+
"local TS\t"+
"master frac\t"+
"local frac\t" +
"ms\t" +
"ms-master\t"+
"frac ms-master\t"+
"tow_corr\n";
if (writer != null) {
writer.write(header);
} else {
System.out.print(header);
}
int dbg_nrec = -5000;
int type_1pps = EventLoggerFileInfo.REC_TYPE_ODO << 4; // 0x30
for (int nrec = 0; nrec < num_recs; nrec++) {
if (nrec == dbg_nrec) {
System.out.println("list1Pps(): nrec="+nrec);
}
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type_1pps)) {
double local_ts = getLocalTS(nrec);
double master_ts = getMasterTS(nrec);
// double ms = 0.001* gnss_start[1] + (0.001*(gnss_end[1]-gnss_start[1])*nrec)/num_recs;
double ms = 0.001* (gnss_start[1] +
(gnss_end[1]-gnss_start[1])*(master_ts-ts_master_start)/(ts_master_end-ts_master_start));
String line =String.format(
"%6d\t%17.6f\t%17.6f\t%9.6f\t%9.6f\t%17.6f\t%17.6f\t%9.6f\t%9.6f\n",
nrec,
master_ts,
local_ts,
master_ts-Math.round(master_ts),
local_ts-Math.round(local_ts),
ms,
(ms-master_ts),
(ms-master_ts)-Math.round(ms-master_ts),
getTimeOfWeekCorrection(master_ts));
if (writer != null) {
writer.write(line);
} else {
System.out.print(line);
}
}
}
if (writer != null) {
writer.close();
}
if (!keep) {
close();
}
}
byte [] getDidPayload(
......@@ -836,6 +1125,70 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
return getMasterTS(getLocalTS(bb, nrec));
}
/**
* Interpolate assuming linear difference between GPS/absolute and master times
* @param ts_master master timestamp (from the image name corrected to time zone and lag)
* @return time of week in seconds (not normalized, may be negative or >= week)
*/
double getLinearTimeOfWeek(double ts_master) {
if (gnss_start[0] == gnss_end[0]) {
return 0.001* (gnss_start[1] +
(gnss_end[1]-gnss_start[1])*(ts_master-ts_master_start)/(ts_master_end-ts_master_start));
} else { // if switched weeks count from start week
return 0.001* (gnss_start[1] +
(gnss_end[0] + Did_ins.WEEK_SECONDS + gnss_end[1]-gnss_start[1])*(ts_master-ts_master_start)/(ts_master_end-ts_master_start));
}
}
/**
* Use 1pps calibration to improve accuracy of the linear time of week
* @param ts_master
* @return TOW correction (subtract from the linear interpolation)
*/
double getTimeOfWeekCorrection(double ts_master) { // bb should be open
int type_1pps = EventLoggerFileInfo.REC_TYPE_ODO << 4; // 0x30
double [] first_last_ts = getFirstLastTS(type_1pps, 0);
int [] first_last_index = getFirstLastIndex(type_1pps, 0); // should not be null as it matches getFirstLastTS()
if (first_last_ts == null) {
System.out.println ("ERROR: getCalibratedTimeOfWeek() getFirstLastTS(type_1pps, 0)-> null");
return Double.NaN;
}
int index_1pps = -1;
if (ts_master <= first_last_ts[0]) {
index_1pps = first_last_index[0];
} else if (ts_master >= first_last_ts[1]) {
index_1pps = first_last_index[1];
} else {
index_1pps = getLastBeforeIndex( // bb should be open
ts_master, // double ts_master,
false, // boolean after, // false - before (including ==)
type_1pps, // int type,
0); // int did)
if (index_1pps <0) {
index_1pps = getLastBeforeIndex( // bb should be open
ts_master, // double ts_master,
true, // boolean after, // false - before (including ==)
type_1pps, // int type,
0); // int did)
}
}
if (index_1pps <0) {
System.out.println ("ERROR: getCalibratedTimeOfWeek() getFirstLastTS(type_1pps, 0)-> null");
return Double.NaN;
}
double ts_1pps = getMasterTS(bb, index_1pps); // this record corresponds to zero fractional part of tow
// pps_tow - interpolated should be close to integer
double pps_tow = 0.001* (gnss_start[1] +
(gnss_end[1]-gnss_start[1])*(ts_1pps-ts_master_start)/(ts_master_end-ts_master_start));
double tow_corr = pps_tow-Math.round(pps_tow);
// if tow_corr > 0, then interpolated value is too high, reduce it
/*
double linear_tow = 0.001* (gnss_start[1] +
(gnss_end[1]-gnss_start[1])*(ts_master-ts_master_start)/(ts_master_end-ts_master_start));
return linear_tow - tow_corr;
*/
return tow_corr;
}
// only applies to the first subpacket (rec_type = 0x18 - after swapping) of IMX5 packet
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ b9a32b13
......@@ -2264,7 +2264,7 @@ public class QuadCLTCPU {
if (Eyesis_Correction.EVENT_LOGGER == null) {
System.out.println("Preparing IMS data as it is needed but does not exist");
String imx_logs = correctionsParameters.getImsSourceDirectory(); // sourceImsDirectory;
Eyesis_Correction.EVENT_LOGGER = new EventLogger (imx_logs);
Eyesis_Correction.EVENT_LOGGER = new EventLogger (imx_logs, null);
}
EventLogger eventLogger = Eyesis_Correction.EVENT_LOGGER;
double ims_timestamp = this_ts + this.ims_offs + 3600 * gmt_plus;
......@@ -2380,14 +2380,14 @@ public class QuadCLTCPU {
null, // Properties properties)
true); // boolean silent)
// temporary to recalculate if no did_ins_2:
{
if (ims_properties != null){ // may become null, so
Did_ins_2 did_ins_2 = new Did_ins_2("did_ins_2-", ims_properties);
if ((did_ins_2.lla == null) || ((did_ins_2.lla[0]==0) && (did_ins_2.lla[1]==0))) {
ims_properties = null;
System.out.println("restoreIms(): no did_ins_2 - invalidating and recalculating config");
}
}
if (ims_properties == null) {
if (ims_properties == null) { // may become null above
if (create) {
saveIms(ims_offset, gmt_plus, ims_path, debugLevel);
ims_properties = loadProperties(
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment