diff --git a/src/main/java/ipos/project/Functionality/Odometry.java b/src/main/java/ipos/project/Functionality/Odometry.java
index aef3edc7bccbea12d32c68284edfc49b25d0902e..ddd7a51cfb5e00033bf65194cc2e10586ed70d56 100644
--- a/src/main/java/ipos/project/Functionality/Odometry.java
+++ b/src/main/java/ipos/project/Functionality/Odometry.java
@@ -1,6 +1,7 @@
package ipos.project.Functionality;
import ipos.project.DataModellntegration.iPos_Datamodel.*;
+import org.apache.logging.log4j.LogManager;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.springframework.stereotype.Component;
@@ -8,66 +9,98 @@ import org.springframework.stereotype.Component;
import java.sql.Timestamp;
import java.text.ParseException;
import java.text.SimpleDateFormat;
+import java.time.LocalDateTime;
+import java.time.ZoneId;
+import java.time.format.DateTimeFormatter;
import java.util.Date;
import java.util.MissingResourceException;
@Component
public class Odometry {
- private final String agentId;
- private double accuracy;
- private double timeOfLastCalibration;
- private double timeLastSensorEvent;
- private double [] a = new double[]{0, 0, 0};
- private double [] aOffset;
- private double [] v = new double[]{0, 0, 0};
- private double [] p = new double[]{0, 0, 0};
- private double [] delta = new double[]{0, 0, 0};
- private double [] vLast = new double[]{0, 0, 0};
- private double [] pLast = new double[]{0, 0, 0};
- private double NS2S = 1/10000.0;
- private boolean calibrated = false;
- SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS");
+ private static org.apache.logging.log4j.Logger LOG = LogManager.getLogger();
+ private final String agentId = "";
+ private static double accuracy;
+ private static double timeOfLastCalibration;
+ private static double timeLastSensorEvent;
+ private static double [] a = new double[]{0, 0, 0};
+ private static double [][] a_ori = new double[][]{{0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0}};
+ private static double [] aOffset;
+ private static double [] v = new double[]{0, 0, 0};
+ private static double [] p = new double[]{0, 0, 0};
+ private static double [] delta = new double[]{0, 0, 0};
+ private static double [] vLast = new double[]{0, 0, 0};
+ private static double [] pLast = new double[]{0, 0, 0};
+ private static double NS2S = 1/1000.0;
+ private static boolean calibrated = false;
+ private static int index_ori = 0;
+ static SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS");
- public Odometry(String agentId) {
- this.agentId = agentId;
+ //public Odometry(String agentId) {
+ // this.agentId = agentId;
+ //}
+ private static Date convertToDateViaInstant(LocalDateTime dateToConvert) {
+ return Date.from(dateToConvert.atZone(ZoneId.systemDefault()).toInstant());
}
- public void calibrate (PositionEvent event) throws ParseException {
- this.vLast[0] = 0;
- this.vLast[1] = 0;
- this.vLast[2] = 0;
- aOffset =a;
- this.accuracy = 0.03;
+ private static LocalDateTime convertToLocalDateTimeViaInstant(Date dateToConvert){
+ return dateToConvert.toInstant().atZone(ZoneId.systemDefault()).toLocalDateTime();
+ }
+ public static double average(double[] data) {
+ double sum = 0;
+ double average;
+
+ for(int i=0; i < data.length; i++){
+ sum = sum + data[i];
+ }
+ average = sum/data.length;
+ return average;
+ }
+ public static void calibrate (PositionEvent event) throws ParseException {
+ v = new double[] {0,0,0};
+ vLast = new double[] {0,0,0};
+ aOffset = new double[] {average(a_ori[0]),average(a_ori[1]),average(a_ori[2])};
+ accuracy = 0.03;
if (event.getPlacing().getPosition().getPoint() instanceof Point3D) {
- this.pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX();
- this.pLast[1] = ((Point3D) event.getPlacing().getPosition().getPoint()).getY();
- this.pLast[2] = ((Point3D) event.getPlacing().getPosition().getPoint()).getZ();
+ pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX();
+ pLast[1] = ((Point3D) event.getPlacing().getPosition().getPoint()).getY();
+ pLast[2] = ((Point3D) event.getPlacing().getPosition().getPoint()).getZ();
+ p = pLast;
}
- Date date = sdf2.parse(event.getTimeStamp());
+ LOG.info("Odometry: timestamp" + event.getTimeStamp());
+ LocalDateTime eventTime = LocalDateTime.parse(event.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
+ Date date = convertToDateViaInstant(eventTime);
+
timeOfLastCalibration = date.getTime();
+ calibrated = true;
}
- public PositionEvent Update (IMU imuRawdataEvent) throws ParseException {
+ public static PositionEvent update(IMU imuRawdataEvent) throws ParseException {
IPos_DatamodelFactory datamodelFactory = IPos_DatamodelFactory.eINSTANCE;
Timestamp timestamp = new Timestamp(System.currentTimeMillis());
-
-
-
Date date = sdf2.parse(imuRawdataEvent.getTimeStamp());
- timeLastSensorEvent = date.getTime();
+
if (calibrated) {
double dt = (date.getTime() - timeLastSensorEvent) * NS2S;
+ a_ori[0][index_ori] = imuRawdataEvent.getAcceleration().getX();
+ a_ori[1][index_ori] = imuRawdataEvent.getAcceleration().getY();
+ a_ori[2][index_ori] = imuRawdataEvent.getAcceleration().getZ();
+ index_ori += 1;
+ if (index_ori>9) {
+ index_ori =0;
+ }
a[0] = imuRawdataEvent.getAcceleration().getX() - aOffset[0];
a[1] = imuRawdataEvent.getAcceleration().getY() - aOffset[1];
a[2] = imuRawdataEvent.getAcceleration().getZ() - aOffset[2];
for (int index = 0; index < 3; ++index) {
- double last_velocity = vLast[index];
- v[index] += (a[index] + vLast[index]) / 2 * dt;
- delta[index] += (v[index] + last_velocity) / 2 * dt;
+ v[index] = a[index] * dt + vLast[index];
+ delta[index] = (v[index] + vLast[index]) / 2 * dt;
p[index] = pLast[index] + delta[index];
+ pLast[index] = p[index];
+ vLast[index] = v[index];
}
- accuracy = (date.getTime() - timeOfLastCalibration) * 0.01;
-
+ accuracy = (date.getTime() - timeOfLastCalibration + 500) * 0.0001;
+ timeLastSensorEvent = date.getTime();
+ Quaternion calOrientation = datamodelFactory.createQuaternion();
PositionEvent calPositionEvent = datamodelFactory.createPositionEvent();
Placing calPlacing = datamodelFactory.createPlacing();
LocalizableObject calLObject = datamodelFactory.createLocalizableObject();
@@ -78,18 +111,33 @@ public class Odometry {
calPoint3D.setX((float) p[0]);
calPoint3D.setY((float) p[1]);
calPoint3D.setZ((float) p[2]);
+ calOrientation.setX(0);
+ calOrientation.setY(0);
+ calOrientation.setZ(0);
+ calOrientation.setW(0);
calPosition.setPoint(calPoint3D);
calPosition.setAccuracy(calGaussian);
+ calPosition.setReferenceSystem(DataServices.getReferenceSystemByIdOrNull("ROOT"));
calPlacing.setPosition(calPosition);
- calLObject.setId(imuRawdataEvent.getSensorId());
- calPositionEvent.setTimeStamp(imuRawdataEvent.getTimeStamp());
+ calPlacing.setOrientation(calOrientation);
+ //calLObject.setId(imuRawdataEvent.getSensorId());
+ LocalDateTime timestamp_LDT=convertToLocalDateTimeViaInstant(date);
+
+
+ calPositionEvent.setLObjectId(imuRawdataEvent.getSensorId());
+ calPositionEvent.setTimeStamp(timestamp_LDT.toString() + "+00:00");
calPositionEvent.setPlacing(calPlacing);
return calPositionEvent;
}
else {
- a[0] = imuRawdataEvent.getAcceleration().getX();
- a[1] = imuRawdataEvent.getAcceleration().getY();
- a[2] = imuRawdataEvent.getAcceleration().getZ();
+ timeLastSensorEvent = date.getTime();
+ a_ori[0][index_ori] = imuRawdataEvent.getAcceleration().getX();
+ a_ori[1][index_ori] = imuRawdataEvent.getAcceleration().getY();
+ a_ori[2][index_ori] = imuRawdataEvent.getAcceleration().getZ();
+ index_ori += 1;
+ if (index_ori>9) {
+ index_ori =0;
+ }
throw new MissingResourceException("Odometry not calibrated!", "Odometry", "calibration");
}