API tools faq
paste
Advertisement
danh8569

MagnetometerCalibrationSimulation_3

danh8569
Mar 5th, 2020
98
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 9.53 KB | None | 0 0
raw download clone embed print report
  1. import java.util.*;
  2.  
  3. public class center {
  4.  
  5.     public static void main(String[] args) {
  6.         System.out.println("--- Hard Iron ---------------------------------------------------------");
  7.         printSensorxyz(sensor.getSensor_x(), sensor.getSensor_y(), sensor.getSensor_z());
  8.         printOffsetxyz(hardiron.getOffset_x(100, 0), hardiron.getOffset_y(100, 0), hardiron.getOffset_z(100, 0));
  9.         printHardCorrectedxyz(
  10.                 hardiron.getHardCorrected_x(sensor.getSensor_x(), hardiron.getOffset_x(100, 0)),
  11.                 hardiron.getHardCorrected_y(sensor.getSensor_y(), hardiron.getOffset_y(100, 0)),
  12.                 hardiron.getHardCorrected_z(sensor.getSensor_z(), hardiron.getOffset_z(100, 0)));
  13.        
  14.         System.out.println("\n" + "--- Soft Iron ---------------------------------------------------------");
  15.         printSensorxyz(sensor.getSensor_x(), sensor.getSensor_y(), sensor.getSensor_z());
  16.         printOffsetxyz(hardiron.getOffset_x(100, 0), hardiron.getOffset_y(100, 0), hardiron.getOffset_z(100, 0));
  17.         printAvgDeltaxyz(softiron.getAvg_delta_x(100, 0), softiron.getAvg_delta_y(100, 0), softiron.getAvg_delta_z(100, 0));
  18.         printAvg(softiron.getAvg_delta(softiron.getAvg_delta_x(100, 0), softiron.getAvg_delta_y(100, 0), softiron.getAvg_delta_z(100, 0)));
  19.         printScalexyz(
  20.                 softiron.getScale_x(softiron.getAvg_delta(softiron.getAvg_delta_x(100, 0), softiron.getAvg_delta_y(100, 0), softiron.getAvg_delta_z(100, 0)), softiron.getAvg_delta_x(100, 0)),
  21.                 softiron.getScale_y(softiron.getAvg_delta(softiron.getAvg_delta_x(100, 0), softiron.getAvg_delta_y(100, 0), softiron.getAvg_delta_z(100, 0)), softiron.getAvg_delta_y(100, 0)),
  22.                 softiron.getScale_z(softiron.getAvg_delta(softiron.getAvg_delta_x(100, 0), softiron.getAvg_delta_y(100, 0), softiron.getAvg_delta_z(100, 0)), softiron.getAvg_delta_z(100, 0)));
  23.         printSoftCorrectedxyz(
  24.                 softiron.getSoftCorrected_x(sensor.getSensor_x(), hardiron.getOffset_x(100, 0), softiron.getScale_x(softiron.getAvg_delta(softiron.getAvg_delta_x(100, 0), softiron.getAvg_delta_y(100, 0), softiron.getAvg_delta_z(100, 0)), softiron.getAvg_delta_x(100, 0))),
  25.                 softiron.getSoftCorrected_y(sensor.getSensor_y(), hardiron.getOffset_y(100, 0), softiron.getScale_y(softiron.getAvg_delta(softiron.getAvg_delta_x(100, 0), softiron.getAvg_delta_y(100, 0), softiron.getAvg_delta_z(100, 0)), softiron.getAvg_delta_y(100, 0))),
  26.                 softiron.getSoftCorrected_z(sensor.getSensor_z(), hardiron.getOffset_z(100, 0), softiron.getScale_z(softiron.getAvg_delta(softiron.getAvg_delta_x(100, 0), softiron.getAvg_delta_y(100, 0), softiron.getAvg_delta_z(100, 0)), softiron.getAvg_delta_z(100, 0))));
  27.  
  28.     }
  29.    
  30.     private static void printSensorxyz(double sensor_x, double sensor_y, double sensor_z) {
  31.         System.out.println("Sensor Info: [" + sensor_x + ", " + sensor_y + ", " + sensor_z + "]");
  32.     }
  33.    
  34.     private static void printOffsetxyz(double offset_x, double offset_y, double offset_z) {
  35.         System.out.println("Offset: [" + offset_x + ", " + offset_y + ", " + offset_z + "]");
  36.     }
  37.    
  38.     private static void printHardCorrectedxyz(double hardCorrected_x, double hardCorrected_y, double hardCorrected_z) {
  39.         System.out.println("Corrected: [" + hardCorrected_x + ", " + hardCorrected_y + ", " + hardCorrected_z + "]");
  40.     }
  41.    
  42.     private static void printAvgDeltaxyz(double avg_delta_x, double avg_delta_y, double avg_delta_z) {
  43.         System.out.println("Avarage_xyz: [" + avg_delta_x + ", " + avg_delta_y + ", " + avg_delta_z + "]");
  44.     }
  45.    
  46.     private static void printAvg(double avg_delta) {
  47.         System.out.println("Avarage_xyz: [" + avg_delta + "]");
  48.     }
  49.    
  50.     private static void printScalexyz(double scale_x, double scale_y, double scale_z) {
  51.         System.out.println("Scale_xyz: [" + scale_x + ", " + scale_y + ", " + scale_z + "]");
  52.     }
  53.    
  54.     private static void printSoftCorrectedxyz(double softCorrected_x, double softCorrected_y, double softCorrected_z) {
  55.         System.out.println("Corrected: [" + softCorrected_x + ", " + softCorrected_y + ", " + softCorrected_z + "]");
  56.     }
  57.    
  58. }
  59. //center ^^^
  60.  
  61. import java.util.*;
  62.  
  63. public class sensor extends center {
  64.     static Random rnd = new Random();
  65.     static double sensor_x = rnd.nextDouble()*1000;
  66.     static double sensor_y = rnd.nextDouble()*1000;
  67.     static double sensor_z = rnd.nextDouble()*1000;
  68.    
  69.     public sensor(double sensor_x, double sensor_y, double sensor_z) {
  70.         this.sensor_x = sensor_x;
  71.         this.sensor_y = sensor_y;
  72.         this.sensor_z = sensor_z;
  73.     }
  74.  
  75.     public static double getSensor_x() {
  76.         return sensor_x;
  77.     }
  78.  
  79.     public void setSensor_x(double sensor_x) {
  80.         this.sensor_x = sensor_x;
  81.     }
  82.  
  83.     public static double getSensor_y() {
  84.         return sensor_y;
  85.     }
  86.  
  87.     public void setSensor_y(double sensor_y) {
  88.         this.sensor_y = sensor_y;
  89.     }
  90.  
  91.     public static double getSensor_z() {
  92.         return sensor_z;
  93.     }
  94.  
  95.     public void setSensor_z(double sensor_z) {
  96.         this.sensor_z = sensor_z;
  97.     }
  98. }
  99. //sensor ^^^
  100.  
  101.  
  102. public class hardiron {
  103.     static double offset_x;
  104.     static double offset_y;
  105.     static double offset_z;
  106.     static double hardCorrected_x;
  107.     static double hardCorrected_y;
  108.     static double hardCorrected_z;
  109.    
  110.     public hardiron(double offset_x, double offset_y, double offset_z) {
  111.         this.offset_x = offset_x;
  112.         this.offset_y = offset_y;
  113.         this.offset_z = offset_z;
  114.     }
  115.    
  116.     public hardiron() {}
  117.    
  118.     public static double getOffset_x(double max_x, double min_x) {
  119.         double offset_x = (max_x + min_x) / 2;
  120.         return offset_x;
  121.     }
  122.     public static double getOffset_y(double max_y, double min_y) {
  123.         double offset_y = (max_y + min_y) / 2;
  124.         return offset_y;
  125.     }
  126.     public static double getOffset_z(double max_z, double min_z) {
  127.         double offset_z = (max_z + min_z) / 2;
  128.         return offset_z;
  129.     }
  130.     public static double getHardCorrected_x(double sensor_x, double offset_x) {
  131.         hardCorrected_x = sensor_x - offset_x;
  132.         return hardCorrected_x;
  133.     }
  134.     public static double getHardCorrected_y(double sensor_y, double offset_y) {
  135.         hardCorrected_y = sensor_y - offset_y;
  136.         return hardCorrected_y;
  137.     }
  138.     public static double getHardCorrected_z(double sensor_z, double offset_z) {
  139.         hardCorrected_z = sensor_z - offset_z;
  140.         return hardCorrected_z;
  141.     }
  142.  
  143. }
  144. //hardiron ^^^
  145.  
  146.  
  147. public class softiron {
  148.     static double offset_x;
  149.     static double offset_y;
  150.     static double offset_z;
  151.     static double avg_delta_x;
  152.     static double avg_delta_y;
  153.     static double avg_delta_z;
  154.     static double avg_delta;
  155.     static double scale_x;
  156.     static double scale_y;
  157.     static double scale_z;
  158.     static double softCorrected_x;
  159.     static double softCorrected_y;
  160.     static double softCorrected_z;
  161.    
  162.     public softiron(double avg_delta_x, double avg_delta_y, double avg_delta_z,
  163.                     double scale_x, double scale_y, double scale_z,
  164.                     double offset_x, double offset_y, double offset_z) {
  165.         this.avg_delta_x = avg_delta_x;
  166.         this.avg_delta_y = avg_delta_y;
  167.         this.avg_delta_z = avg_delta_z;
  168.         this.scale_x = scale_x;
  169.         this.scale_y = scale_y;
  170.         this.scale_z = scale_z;
  171.         this.offset_x = offset_x;
  172.         this.offset_y = offset_y;
  173.         this.offset_z = offset_z;
  174.     }
  175.    
  176.     public softiron() {}
  177.    
  178.     public static double getOffset_x(double max_x, double min_x) {
  179.         double offset_x = (max_x + min_x) / 2;
  180.         return offset_x;
  181.     }
  182.     public static double getOffset_y(double max_y, double min_y) {
  183.         double offset_y = (max_y + min_y) / 2;
  184.         return offset_y;
  185.     }
  186.     public static double getOffset_z(double max_z, double min_z) {
  187.         double offset_z = (max_z + min_z) / 2;
  188.         return offset_z;
  189.     }  
  190.     public static double getAvg_delta_x(double max_x, double min_x) {
  191.         double avg_delta_x = (max_x - min_x) / 2;
  192.         return avg_delta_x;
  193.     }  
  194.     public static double getAvg_delta_y(double max_y, double min_y) {
  195.         double avg_delta_y = (max_y - min_y) / 2;
  196.         return avg_delta_y;
  197.     }  
  198.     public static double getAvg_delta_z(double max_z, double min_z) {
  199.         double avg_delta_z = (max_z - min_z) / 2;
  200.         return avg_delta_z;
  201.     }  
  202.     public static double getAvg_delta(double avg_delta_x, double avg_delta_y, double avg_delta_z) {
  203.         double avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z) / 3;
  204.         return avg_delta;
  205.     }    
  206.     public static double getScale_x(double avg_delta, double avg_delta_x) {
  207.         double scale_x = avg_delta / avg_delta_x;
  208.         return scale_x;
  209.     }  
  210.     public static double getScale_y(double avg_delta, double avg_delta_y) {
  211.         double scale_y = avg_delta / avg_delta_y;
  212.         return scale_y;
  213.     }  
  214.     public static double getScale_z(double avg_delta, double avg_delta_z) {
  215.         double scale_z = avg_delta / avg_delta_z;
  216.         return scale_z;
  217.     }
  218.     public static double getSoftCorrected_x(double sensor_x, double offset_x, double scale_x) {
  219.         double softCorrected_x = (sensor_x - offset_x) * scale_x;
  220.         return softCorrected_x;
  221.     }
  222.     public static double getSoftCorrected_y(double sensor_y, double offset_y, double scale_y) {
  223.         double softCorrected_y = (sensor_y - offset_y) * scale_y;
  224.         return softCorrected_y;
  225.     }
  226.     public static double getSoftCorrected_z(double sensor_z, double offset_z, double scale_z) {
  227.         double softCorrected_z = (sensor_z - offset_z) * scale_z;
  228.         return softCorrected_z;
  229.     }
  230. }
  231. //softiron ^^^
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!