Skip to content
Snippets Groups Projects
Commit c4853d64 authored by FrankR's avatar FrankR
Browse files

used current odometry-version

parent deb8ff6c
No related branches found
No related tags found
No related merge requests found
...@@ -23,7 +23,7 @@ public class Odometry { ...@@ -23,7 +23,7 @@ public class Odometry {
private static double timeOfLastCalibration; private static double timeOfLastCalibration;
private static double timeLastSensorEvent; private static double timeLastSensorEvent;
private static double [] a = new double[]{0, 0, 0}; private static double [] a = new double[]{0, 0, 0};
private static double [] a_ori = 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 [] aOffset;
private static double [] v = new double[]{0, 0, 0}; private static double [] v = new double[]{0, 0, 0};
private static double [] p = new double[]{0, 0, 0}; private static double [] p = new double[]{0, 0, 0};
...@@ -32,23 +32,33 @@ public class Odometry { ...@@ -32,23 +32,33 @@ public class Odometry {
private static double [] pLast = new double[]{0, 0, 0}; private static double [] pLast = new double[]{0, 0, 0};
private static double NS2S = 1/1000.0; private static double NS2S = 1/1000.0;
private static boolean calibrated = false; private static boolean calibrated = false;
private static int index_ori = 0;
static SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS"); static SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS");
//public Odometry(String agentId) { //public Odometry(String agentId) {
// this.agentId = agentId; // this.agentId = agentId;
//} //}
private static Date convertToDateViaInstant(LocalDateTime dateToConvert) { private static Date convertToDateViaInstant(LocalDateTime dateToConvert) {
return java.util.Date.from(dateToConvert.atZone(ZoneId.systemDefault()).toInstant()); return Date.from(dateToConvert.atZone(ZoneId.systemDefault()).toInstant());
} }
private static LocalDateTime convertToLocalDateTimeViaInstant(Date dateToConvert){ private static LocalDateTime convertToLocalDateTimeViaInstant(Date dateToConvert){
return dateToConvert.toInstant().atZone(ZoneId.systemDefault()).toLocalDateTime(); 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 { public static void calibrate (PositionEvent event) throws ParseException {
v = new double[] {0,0,0}; v = new double[] {0,0,0};
vLast = new double[] {0,0,0}; vLast = new double[] {0,0,0};
aOffset = new double[] {a_ori[0],a_ori[1],a_ori[2]}; aOffset = new double[] {average(a_ori[0]),average(a_ori[1]),average(a_ori[2])};
accuracy = 0.03; accuracy = 0.03;
if (event.getPlacing().getPosition().getPoint() instanceof Point3D) { if (event.getPlacing().getPosition().getPoint() instanceof Point3D) {
pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX(); pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX();
...@@ -71,9 +81,13 @@ public class Odometry { ...@@ -71,9 +81,13 @@ public class Odometry {
if (calibrated) { if (calibrated) {
double dt = (date.getTime() - timeLastSensorEvent) * NS2S; double dt = (date.getTime() - timeLastSensorEvent) * NS2S;
a_ori[0] = imuRawdataEvent.getAcceleration().getX(); a_ori[0][index_ori] = imuRawdataEvent.getAcceleration().getX();
a_ori[1] = imuRawdataEvent.getAcceleration().getY(); a_ori[1][index_ori] = imuRawdataEvent.getAcceleration().getY();
a_ori[2] = imuRawdataEvent.getAcceleration().getZ(); 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[0] = imuRawdataEvent.getAcceleration().getX() - aOffset[0];
a[1] = imuRawdataEvent.getAcceleration().getY() - aOffset[1]; a[1] = imuRawdataEvent.getAcceleration().getY() - aOffset[1];
a[2] = imuRawdataEvent.getAcceleration().getZ() - aOffset[2]; a[2] = imuRawdataEvent.getAcceleration().getZ() - aOffset[2];
...@@ -117,9 +131,13 @@ public class Odometry { ...@@ -117,9 +131,13 @@ public class Odometry {
} }
else { else {
timeLastSensorEvent = date.getTime(); timeLastSensorEvent = date.getTime();
a_ori[0] = imuRawdataEvent.getAcceleration().getX(); a_ori[0][index_ori] = imuRawdataEvent.getAcceleration().getX();
a_ori[1] = imuRawdataEvent.getAcceleration().getY(); a_ori[1][index_ori] = imuRawdataEvent.getAcceleration().getY();
a_ori[2] = imuRawdataEvent.getAcceleration().getZ(); 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"); throw new MissingResourceException("Odometry not calibrated!", "Odometry", "calibration");
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment