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;
import java.util.Collections;
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
public static final int REC_TYPE_IMU = 0; // not used
public static final int REC_TYPE_GPS = 1; // now IMX-5 also
public static final int REC_TYPE_IMG = 2; // local image or external trigger TS
public static final int REC_TYPE_ODO = 3; // "Odometer" or any other external event
// bits 28.31 of local usec
public static final int REC_SUBTYPE_IMX5 = 8; // lower bits - number of subpacket (8,9,10,..,15)
public static final int REC_SUBPACKET_IMX5 = 7; // bit mask for DID subpacket number (0- first)
public static final int LOCAL_USEC_OFFS = 0;
public static final int LOCAL_SEC_OFFS = 4;
public static final int MASTER_USEC_OFFS = 8;
public static final int MASTER_SEC_OFFS = 12;
public static final int IMX5_DID = 8;
public static final int IMX5_LENGTH = 10;
public static final int IMX5_PAYLOAD_FIRST = 12;
public static final int IMX5_PAYLOAD_NEXT = 8;
String abs_path;
int num_recs;
int num_subs = 0; // multiply type[0] to combine subtypes with types
int [] type_lut;
int [][] rec_types; // [][0] - 1 byte with swapped nibbles - MSB of microseconds (byte 3 of each 64-byte record),
// [][1] - image channel or IMX5 DID
int [][] rec_first_last; // indexed by rec_types, {first_index_in_file, last_index_in_file}
double [][] first_last_ts; // {first, last} master timestamps for each record type
double local_lag = 0.0; // local timestamp lag to master camera corresponding to the first record in a file)
double local_rel_lag = 1.0; // local timestamp lag correction scale local_lag by local_rel_lag * master_ts_difference
double ts_local_start;
double ts_master_start; // master (image) timestamp in seconds, corresponding to the first record in this file
double ts_master_end; // master (image) timestamp in seconds, corresponding to the last record in this file
int [] gnss_start = null; // gnss time (in weeks, ms) corresponding to the first record in this file
int [] gnss_end = null; // gnss time (in weeks, ms) corresponding to the last record in this file
double gnss_frac_start = Double.NaN; // Fractional seconds of GNSS 1pps in master clock seconds corresponding to the first record in this file
double gnss_frac_end = Double.NaN; // Fractional seconds of GNSS 1pps in master clock seconds corresponding to the last record in this file
/// -0.5 <= gnss_frac_start < 0.5, abs (gnss_frac_end - gnss_frac_start) < 0.5 while abs (gnss_frac_end) may be > 0.5 !
ByteBuffer bb;
// add time-conversion methods here?
public void open() throws IOException{
if (bb == null) {
byte[] allBytes = Files.readAllBytes(Paths.get(abs_path));
bb = ByteBuffer.wrap(allBytes);
bb.order(ByteOrder.LITTLE_ENDIAN);
}
}
public void close() {
bb = null;
}
public int getTypeIndex (
int type,
int did) {
for (int i = 0; i < rec_types.length; i++) {
if ((rec_types[i][0] == type) && (rec_types[i][1] == did)) {
return i;
}
}
return -1; // not found
}
public double [] getFirstLastTS(
int type,
int did) {
int indx = getTypeIndex (type, did);
return (indx >=0) ? first_last_ts[indx] : null;
}
public int [] getFirstLastIndex(
int type,
int did) {
int indx = getTypeIndex (type, did);
return (indx >=0) ? rec_first_last[indx] : null;
}
public int getLastBeforeIndex( // bb should be open
double ts_master,
boolean after, // false - before (including ==)
int type,
int did) {
double [] first_last_ts = getFirstLastTS(type, did);
if (first_last_ts == null) {
return -1;
}
if (ts_master < first_last_ts[0]) {
return -2;
}
if (ts_master > first_last_ts[1]) {
// timestamp later than last, return last (assuming next file start with larger timestamp)
return getFirstLastIndex(type, did)[1];
}
if (after) {
if (ts_master == first_last_ts[1]) {
return -3;
}
}
int [] first_last_index = getFirstLastIndex(type, did); // should not be null as it matches getFirstLastTS()
if (first_last_index[0] == first_last_index[1]) {
return first_last_index[0]; // single choice
}
// initial approximation
int nrec0 = first_last_index[0] + (int) Math.round((first_last_index[1]-first_last_index[0]) *
(ts_master - first_last_ts[0])/ (first_last_ts[1]-first_last_ts[0]));
// find any type in the opposite direction
int drec = after ? -1: 1;
int nrec = nrec0;
search_opposite: {
for (; (nrec > first_last_index[0]) && (nrec < first_last_index[1]); nrec += drec) {
double ts_master_indx = getMasterTS(bb, nrec);
if (after?(ts_master_indx <= ts_master):(ts_master_indx > ts_master)) {
// still need to check type, as master timestamp may be in wrong order
// 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);
}
}
}
}
System.out.println("getLastBeforeIndex(): could not find oppposite, ts_master="+ts_master+", after="+after);
return first_last_index[after ? 1 : 0]; // could not find
}
drec = after ? 1: -1;
for (; (nrec > first_last_index[0]) && (nrec < first_last_index[1]); nrec += drec) {
double ts_master_indx = getMasterTS(bb, nrec);
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);
}
}
}
}
return first_last_index[after ? 1 : 0];
}
public int getNextIndex( // assuming open
int indx,
int type,
int did) {
int [] first_last_index = getFirstLastIndex(type, did); // should not be null as it matches getFirstLastTS()
if (first_last_index == null) {
return -1;
}
if (indx >= first_last_index[1]) {
return -2;
}
for (int nrec = indx; nrec <= first_last_index[1]; nrec++) {
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
return nrec;
}
}
return -3; // should never get here. Make it throw?
}
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);
open();
num_recs = bb.array().length / REC_SIZE;
Set <Point> types_set = new HashSet<Point>();
for (int nrec = 0; nrec < num_recs; nrec++) {
int [] full_type = getFullType(bb, nrec);
if (full_type != null) {
int before_size = types_set.size();
types_set.add(new Point(full_type[0], full_type[1]));
if ((debugLevel > -1) && (types_set.size() > before_size)) {
System.out.println("EventLoggerFileInfo(): added "+full_type[0]+":"+full_type[1]+" @ nrec="+nrec);
}
}
}
ArrayList<Point> types_list = new ArrayList<Point>(types_set);
Collections.sort(types_list, new Comparator<Point>() {
@Override
public int compare(Point lhs, Point rhs) { // ascending
return (rhs.x > lhs.x) ? -1 : ((rhs.x < lhs.x) ? 1 : ( (rhs.y > lhs.y) ? 1 :( (rhs.y < lhs.y) ? -1 :0))) ;
}
});
rec_types = new int [types_list.size()][2];
num_subs = 0;
int [] maxes = new int[rec_types[0].length];
for (int nt = 0; nt < rec_types.length; nt++) {
rec_types[nt][0] = types_list.get(nt).x;
rec_types[nt][1] = types_list.get(nt).y;
for (int i = 0; i < maxes.length; i++) {
if (rec_types[nt][i] > maxes[i]) {
maxes[i] = rec_types[nt][i];
}
}
}
num_subs = maxes[1] + 1;
type_lut = new int [num_subs * (maxes[0] + 1)];
Arrays.fill(type_lut, -1); // just to detect errors
for (int nt = 0; nt < rec_types.length; nt++) {
type_lut[rec_types[nt][0] * num_subs + rec_types[nt][1]] = nt;
}
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)");
}
calibrateFromDidIns(false, debugLevel);
if (debugLevel > -1) {
System.out.println("calibrateFromDidIns(true)");
}
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();
}
}
/**
* 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) {
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]) {
// Find all Did_strobe_in_time in the file, then preceding frame syncs
// processing master timestamps to
double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
int [] week_time0 = null;
double ts_master0 = 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] == Imx5.DID_STROBE_IN_TIME)) {
// find previous image
int nrec_img = nrec -1;
for (; nrec_img >= 0; nrec_img--) {
int [] full_type_img = getFullType(bb, nrec_img);
if ((full_type_img != null) && (full_type_img[0] >> 4) == REC_TYPE_IMG) {
// get master time
double ts_master = getMasterTSImg(bb, nrec_img);
// get gnss 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);
int [] week_time = did_strobe_in_time.getWeekTimeMs();
double ts_gnss = 0.0;
if (Double.isNaN(ts_master0)) {
week_time0 = week_time;
ts_master0 = ts_master;
}
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 %6d %10.6f %10.6f",nrec_img, nrec, ts_master, ts_gnss));
}
break;
}
}
}
}
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_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;
}
/**
* 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));
if (!did_ins.isDidSane()) {
continue;
}
// 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 (sy>2*sx) { // -1
System.out.println (
String.format("%6d %10.6f %10.6f", nrec, ts_master, ts_gnss)+" sx="+sx+" sy="+sy);
System.out.println(Did_strobe_in_time.getWeekTimeMsDelta(week_time0, week_time));
}
if ((sy>2*sx) || (debugLevel > 0)) { // -1
System.out.println (
String.format("%6d %10.6f %10.6f", nrec, ts_master, ts_gnss)+" sx="+sx+" sy="+sy);
}
}
}
if (s0 == 0){
System.out.println("calibrateFromDidIns(): no valid records available, aborting.");
return false;
}
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;
}
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)
* 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) {
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
double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
double ts_local0 = Double.NaN;
double ts_frac0 = 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_1pps)) {
double ts_local = getLocalTS(bb, nrec);
if (Double.isNaN(ts_local0)) {
ts_local0 = ts_local;
ts_frac0 = ts_local - Math.round(ts_local);
}
ts_local -= ts_local0;
double ts_frac = ts_local - Math.round(ts_local);
s0 += 1;
sx += ts_local;
sx2 += ts_local * ts_local;
sy += ts_frac;
sxy += ts_local * ts_frac;
if (debugLevel > 0) {
System.out.println (
String.format("%6d %10.6f %10.6f", nrec, ts_local, ts_frac));
}
}
}
double d = (s0 * sx2 - sx * sx);
double a = (sxy * s0 - sy * sx) / d;
double b = (sx2 * sy - sx * sxy) / d;
double ts_local_end = getLocalTS(bb, num_recs - 1);
double local_frac_start = ts_frac0 + b + (ts_local_start - ts_local0) * a;
double local_frac_end = ts_frac0 + b + (ts_local_end - ts_local0) * a;
double master_frac_start = local_frac_start+ (ts_local_start - ts_master_start);
double master_frac_end = local_frac_end+ (ts_local_end - ts_master_end);
gnss_frac_start = master_frac_start - Math.round(master_frac_start);
gnss_frac_end = master_frac_end - Math.round(master_frac_start);
if (debugLevel > -1) {
System.out.println(String.format("gnss_frac_start: %f", gnss_frac_start));
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);
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
open();
float [] start_ned = null;
FileWriter writer = null;
if (path != null) {
writer = new FileWriter (path);
}
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 {
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_1)) {
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_1 did_ins_1 = new Did_ins_1(bb_payload);
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\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),
did_ins_1.timeOfWeek,
did_ins_1.insStatus,
did_ins_1.hdwStatus,
did_ins_1.theta[0], did_ins_1.theta[1], did_ins_1.theta[2], // Euler angles: roll, pitch, yaw in radians with respect to NED
did_ins_1.uvw[0], did_ins_1.uvw[1], did_ins_1.uvw[2], // Velocity U, V, W in meters per second. Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )".
did_ins_1.lla[0], did_ins_1.lla[1], did_ins_1.lla[2], // WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
did_ins_1.ned[0] - start_ned[0], did_ins_1.ned[1] - start_ned[1], did_ins_1.ned[2] - start_ned[2] // North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude
);
if (writer != null) {
writer.write(line);
} else {
System.out.print(line);
}
}
}
if (writer != null) {
writer.close();
}
if (!keep) {
close();
}
}
public void listDidIns2(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"+
"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 {
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\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),
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
open();
// float [] start_ned = null;
FileWriter writer = null;
if (path != null) {
writer = new FileWriter (path);
}
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);
} else {
System.out.print(header);
}
int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
for (int nrec = 0; nrec < num_recs; nrec++) {
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) &&
(
(((gps_mask & 1) != 0) && (full_type[1] == Imx5.DID_GPS1_POS)) ||
(((gps_mask & 2) != 0) && (full_type[1] == Imx5.DID_GPS2_POS)) ||
(((gps_mask & 4) != 0) && (full_type[1] == Imx5.DID_GPS1_UBX_POS))
)
) {
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_gps_pos did_gps_pos = new Did_gps_pos(bb_payload);
if ((did_gps_pos.lla[0] == 0) && (did_gps_pos.lla[1] == 0) && (did_gps_pos.lla[2] == 0)) {
continue;
}
String line =String.format(
"%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),
0.001 * did_gps_pos.timeOfWeekMs,
full_type[1], // DID
did_gps_pos.status,
did_gps_pos.ecef[0], did_gps_pos.ecef[1], did_gps_pos.ecef[2], // Position in ECEF {x,y,z} (m)
did_gps_pos.lla[0], did_gps_pos.lla[1], did_gps_pos.lla[2], // WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
did_gps_pos.hMSL, did_gps_pos.hAcc, did_gps_pos.vAcc, did_gps_pos.pDop, did_gps_pos.cnoMean,
did_gps_pos.towOffset,
did_gps_pos.leapS, did_gps_pos.satsUsed, did_gps_pos.cnoMeanSigma, did_gps_pos.reserved
);
if (writer != null) {
writer.write(line);
} else {
System.out.print(line);
}
}
}
if (writer != null) {
writer.close();
}
if (!keep) {
close();
}
}
public void listPimu(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"+
"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 {
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\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),
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();
}
}
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(
EventLoggerFileInfo next_fileinfo, // may be null if payload does not extend beyond single record
int nrec) throws IOException {
EventLoggerFileInfo fileinfo = this;
fileinfo.open();
int [] full_type = fileinfo.getFullType(nrec); // 0x18
if (full_type == null) {
System.out.println("Wrong packet");
return null; // error - length too large
}
int did_len = fileinfo.getIMX5Length(nrec);
if (did_len < 0) {
System.out.println("Wrong packet length (-"+did_len+")");
return null; // error - length too large
}
byte [] payload = new byte [did_len];
int bytes_left = did_len;
int bytes_offset = 0;
// read all/some from the initial sub packet;
int subpacket = 0;
while (bytes_left > 0) {
int bytes_copied = fileinfo.copyPayload (
nrec, // int nrec,
payload, // byte [] payload,
bytes_offset, // int offset,
bytes_left); // int len) {
bytes_left -= bytes_copied;
bytes_offset += bytes_copied;
subpacket ++;
if (bytes_left > 0) {
// find next subpacket, open new file if needed
if (subpacket > EventLoggerFileInfo.REC_SUBPACKET_IMX5) {
System.out.println("Too many subpackets (>"+EventLoggerFileInfo.REC_SUBPACKET_IMX5+")");
return null; // throw - illegal subpacket index
}
nrec = fileinfo.getNextIndex( // assuming open
nrec, // int indx,
full_type[0] + subpacket, // int type,
0); // full_type[1]); //int did) // subpacket has no dids
if (nrec < 0) {
System.out.println("Reached the end of the current log file, proceeding to the next one");
if ((next_fileinfo == null) || (next_fileinfo == fileinfo) ) {
System.out.println("No log files left");
return null;
}
fileinfo = next_fileinfo;
fileinfo.open();
nrec = fileinfo.getFirstLastIndex( // earliest in a file
full_type[0] + subpacket,
full_type[1])[0];
}
}
}
return payload;
}
public static int getRecType(ByteBuffer bb, int nrec) { // high microseconds byte with nibbles swapped
int usec_msb = bb.get(nrec * REC_SIZE + LOCAL_USEC_OFFS + 3) & 0xff;
if (usec_msb == 0) { // Probably log_imu.c tries to read too early? Full 32-byte line of 0
System.out.println("getRecType(): usec_msb==0, nrec="+
String.format("%d (0x%x) byte=0x%x", nrec, nrec, 64*nrec));
}
return ((usec_msb >> 4) & 0xf) + ((usec_msb & 0xf) << 4);
}
public double getLocalTS(int nrec) {
return getLocalTS(bb, nrec);
}
public static double getLocalTS(ByteBuffer bb, int nrec) {
int usec = bb.getInt(nrec * REC_SIZE + LOCAL_USEC_OFFS) & 0xfffff;
int sec = bb.getInt(nrec * REC_SIZE + LOCAL_SEC_OFFS);
return sec + 0.000001 * usec;
}
// only applies to image trigger (0-3 - local channel, 4 - external trigger)
public static int getMasterChannel(ByteBuffer bb, int nrec) { // high microseconds byte with nibbles swapped
int usec_msb = bb.get(nrec * REC_SIZE + MASTER_USEC_OFFS + 3) & 0xff;
// return (usec_msb >> 4) + ((usec_msb & 0xff) << 4);
return usec_msb;
}
public static double getMasterTSImg(ByteBuffer bb, int nrec) { // high microseconds byte with nibbles swapped
int usec = bb.getInt(nrec * REC_SIZE + MASTER_USEC_OFFS) & 0xfffff;
int sec = bb.getInt(nrec * REC_SIZE + MASTER_SEC_OFFS);
return sec + 0.000001 * usec;
}
double getMasterTS(double ts_local) {
return ts_master_start + (ts_local - ts_local_start) / local_rel_lag ;
}
double getMasterTS(ByteBuffer bb, int nrec) {
return getMasterTS(getLocalTS(bb, nrec));
}
double getMasterTS(int nrec) {
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
public static int getIMX5Did(ByteBuffer bb, int nrec) {
return bb.getShort(nrec * REC_SIZE + IMX5_DID) & 0xffff;
}
public int getIMX5Did(int nrec) {
return getIMX5Did(bb, nrec);
}
public static int getIMX5Length(ByteBuffer bb, int nrec) {
int len = bb.getShort(nrec * REC_SIZE + IMX5_LENGTH) & 0x7fff;
int max_len = (REC_SIZE - IMX5_PAYLOAD_NEXT) + (REC_SIZE - IMX5_PAYLOAD_FIRST) * REC_SUBPACKET_IMX5;
if (len > max_len) {
return - len; // error
}
return len;
}
public int getIMX5Length(int nrec) {
return getIMX5Length(bb, nrec);
}
public int copyPayload (
int nrec,
byte [] payload,
int offset,
int len) {
return copyPayload (
bb, // ByteBuffer bb,
nrec, // int nrec,
payload, // byte [] payload,
offset, // int offset,
len); // int len)
}
public static int copyPayload (
ByteBuffer bb,
int nrec,
byte [] payload,
int offset,
int len) {
int [] full_type = getFullType(bb, nrec);
if (full_type == null) {
return 0;
}
boolean is_first = (full_type[0] & REC_SUBPACKET_IMX5) == 0;
int payload_offset = is_first?IMX5_PAYLOAD_FIRST:IMX5_PAYLOAD_NEXT;
int copy_len = Math.min(len, REC_SIZE - payload_offset);
bb.position(nrec * REC_SIZE + payload_offset); // start of payload data
bb.get(payload, offset, copy_len);
System.arraycopy(bb.array(), nrec * REC_SIZE + payload_offset, payload, offset, copy_len);
return copy_len;
}
public int [] getFullType(int nrec) {
return getFullType(bb, nrec);
}
/**
*
* Adding invalid image (usec == 0xffffff - what does it mean)?
* Check FPGA if it is the previous TS, not the current
* @param bb
* @param nrec
* @return
*/
public static int [] getFullType(ByteBuffer bb, int nrec) {
int [] types = new int[2];
types[0] = getRecType(bb, nrec); // nibbles swapped
int root_type = (types[0] >> 4) & 0xf;
switch (root_type) {
case REC_TYPE_IMG:
types[1] = getMasterChannel(bb, nrec);
if ((bb.getInt(nrec * REC_SIZE + MASTER_USEC_OFFS) & 0xfffff) == 0xfffff) {
System.out.println("Wrong master TS (=0xfffff) for nrec=" + nrec);
return null;
}
break;
case REC_TYPE_GPS:
int gps_subtype = types[0] & 0xf;
if (gps_subtype == REC_SUBTYPE_IMX5) { // only first subpacket
types[1] = getIMX5Did(bb, nrec);
}
break;
case REC_TYPE_ODO:
types[1] = 0; // not needed, just a placeholder
break;
}
return types;
}
@Override
public int compareTo(EventLoggerFileInfo arg0) {
return Double.compare(ts_master_start,arg0.ts_master_start) ;
}
}