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 {
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};
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};
......@@ -32,23 +32,33 @@ public class Odometry {
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;
//}
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){
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[] {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;
if (event.getPlacing().getPosition().getPoint() instanceof Point3D) {
pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX();
......@@ -71,9 +81,13 @@ public class Odometry {
if (calibrated) {
double dt = (date.getTime() - timeLastSensorEvent) * NS2S;
a_ori[0] = imuRawdataEvent.getAcceleration().getX();
a_ori[1] = imuRawdataEvent.getAcceleration().getY();
a_ori[2] = imuRawdataEvent.getAcceleration().getZ();
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];
......@@ -117,9 +131,13 @@ public class Odometry {
}
else {
timeLastSensorEvent = date.getTime();
a_ori[0] = imuRawdataEvent.getAcceleration().getX();
a_ori[1] = imuRawdataEvent.getAcceleration().getY();
a_ori[2] = imuRawdataEvent.getAcceleration().getZ();
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");
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment