SHARE
TWEET

firmware arduino imu+sunlight+water

JoaoJ Oct 10th, 2017 106 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "Wire.h"
  2.  
  3. // I2Cdev and MPU9250 must be installed as libraries, or else the .cpp/.h files
  4. // for both classes must be in the include path of your project
  5. #include "I2Cdev.h"
  6. #include "MPU9250.h"
  7. #include "BMP180.h"
  8.  
  9. // class default I2C address is 0x68
  10. // specific I2C addresses may be passed as a parameter here
  11. // AD0 low = 0x68 (default for InvenSense evaluation board)
  12. // AD0 high = 0x69
  13. MPU9250 accelgyro;
  14. I2Cdev   I2C_M;
  15.  
  16. //Sunlight sensor
  17. #include "Arduino.h"
  18. #include "SI114X.h"
  19. SI114X SI1145 = SI114X();
  20.  
  21. uint8_t buffer_m[6];
  22.  
  23.  
  24. int16_t ax, ay, az;
  25. int16_t gx, gy, gz;
  26. int16_t   mx, my, mz;
  27.  
  28.  
  29.  
  30. float heading;
  31. float tiltheading;
  32.  
  33. float Axyz[3];
  34. float Gxyz[3];
  35. float Mxyz[3];
  36.  
  37.  
  38. #define sample_num_mdate  5000
  39.  
  40. volatile float mx_sample[3];
  41. volatile float my_sample[3];
  42. volatile float mz_sample[3];
  43.  
  44. static float mx_centre = 0;
  45. static float my_centre = 0;
  46. static float mz_centre = 0;
  47.  
  48. volatile int mx_max = 0;
  49. volatile int my_max = 0;
  50. volatile int mz_max = 0;
  51.  
  52. volatile int mx_min = 0;
  53. volatile int my_min = 0;
  54. volatile int mz_min = 0;
  55.  
  56. float temperature;
  57. float pressure;
  58. float atm;
  59. float altitude;
  60. BMP180 Barometer;
  61.  
  62. void setup()
  63. {
  64.    
  65.     // join I2C bus (I2Cdev library doesn't do this automatically)
  66.     Wire.begin();
  67.  
  68.     // initialize serial communication
  69.     // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
  70.     // it's really up to you depending on your project)
  71.     Serial.begin(115200);
  72.     Serial.println("Beginning Si1145!");
  73.     while (!SI1145.Begin()) {
  74.       Serial.println("Si1145 is not ready!");
  75.       delay(1000);
  76.     }
  77.     Serial.println("Si1145 is ready!");
  78.  
  79.  
  80.     // initialize device
  81.     Serial.println("Initializing I2C devices...");
  82.     accelgyro.initialize();
  83.     Barometer.init();
  84.  
  85.     // verify connection
  86.     Serial.println("Testing device connections...");
  87.     Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
  88.  
  89.     delay(1000);
  90.     Serial.println("     ");
  91.  
  92.     //  Mxyz_init_calibrated ();
  93.  
  94. }
  95.  
  96. void loop()
  97. {
  98.  
  99.     getAccel_Data();
  100.     getGyro_Data();
  101.     getCompassDate_calibrated(); // compass data has been calibrated here
  102.     getHeading();               //before we use this function we should run 'getCompassDate_calibrated()' frist, so that we can get calibrated data ,then we can get correct angle .
  103.     getTiltHeading();
  104.  
  105.     Serial.println("calibration parameter: ");
  106.     Serial.print(mx_centre);
  107.     Serial.print("         ");
  108.     Serial.print(my_centre);
  109.     Serial.print("         ");
  110.     Serial.println(mz_centre);
  111.     Serial.println("     ");
  112.  
  113.  
  114.     Serial.println("Acceleration(g) of X,Y,Z:");
  115.     Serial.print(Axyz[0]);
  116.     Serial.print(",");
  117.     Serial.print(Axyz[1]);
  118.     Serial.print(",");
  119.     Serial.println(Axyz[2]);
  120.     Serial.println("Gyro(degress/s) of X,Y,Z:");
  121.     Serial.print(Gxyz[0]);
  122.     Serial.print(",");
  123.     Serial.print(Gxyz[1]);
  124.     Serial.print(",");
  125.     Serial.println(Gxyz[2]);
  126.     Serial.println("Compass Value of X,Y,Z:");
  127.     Serial.print(Mxyz[0]);
  128.     Serial.print(",");
  129.     Serial.print(Mxyz[1]);
  130.     Serial.print(",");
  131.     Serial.println(Mxyz[2]);
  132.     Serial.println("The clockwise angle between the magnetic north and X-Axis:");
  133.     Serial.print(heading);
  134.     Serial.println(" ");
  135.     Serial.println("The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:");
  136.     Serial.println(tiltheading);
  137.     Serial.println("   ");
  138.  
  139.     temperature = Barometer.bmp180GetTemperature(Barometer.bmp180ReadUT()); //Get the temperature, bmp180ReadUT MUST be called first
  140.     pressure = Barometer.bmp180GetPressure(Barometer.bmp180ReadUP());//Get the temperature
  141.     altitude = Barometer.calcAltitude(pressure); //Uncompensated caculation - in Meters
  142.     atm = pressure / 101325;
  143.  
  144.     Serial.print("Temperature: ");
  145.     Serial.print(temperature, 2); //display 2 decimal places
  146.     Serial.println("deg C");
  147.  
  148.     Serial.print("Pressure: ");
  149.     Serial.print(pressure, 0); //whole number only.
  150.     Serial.println(" Pa");
  151.  
  152.     Serial.print("Ralated Atmosphere: ");
  153.     Serial.println(atm, 4); //display 4 decimal places
  154.  
  155.     Serial.print("Altitude: ");
  156.     Serial.print(altitude, 2); //display 2 decimal places
  157.     Serial.println(" m");
  158.  
  159.     Serial.println();
  160.     delay(1000);
  161.     Serial.print("//--------------------------------------//\r\n");
  162.     Serial.print("Vis: "); Serial.println(SI1145.ReadVisible());
  163.     Serial.print("IR: "); Serial.println(SI1145.ReadIR());
  164.     //the real UV value must be div 100 from the reg value , datasheet for more information.
  165.     Serial.print("UV: ");  Serial.println((float)SI1145.ReadUV()/100);
  166.     delay(1000);
  167.  
  168. }
  169.  
  170.  
  171. void getHeading(void)
  172. {
  173.     heading = 180 * atan2(Mxyz[1], Mxyz[0]) / PI;
  174.     if (heading < 0) heading += 360;
  175. }
  176.  
  177. void getTiltHeading(void)
  178. {
  179.     float pitch = asin(-Axyz[0]);
  180.     float roll = asin(Axyz[1] / cos(pitch));
  181.  
  182.     float xh = Mxyz[0] * cos(pitch) + Mxyz[2] * sin(pitch);
  183.     float yh = Mxyz[0] * sin(roll) * sin(pitch) + Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
  184.     float zh = -Mxyz[0] * cos(roll) * sin(pitch) + Mxyz[1] * sin(roll) + Mxyz[2] * cos(roll) * cos(pitch);
  185.     tiltheading = 180 * atan2(yh, xh) / PI;
  186.     if (yh < 0)    tiltheading += 360;
  187. }
  188.  
  189.  
  190.  
  191. void Mxyz_init_calibrated ()
  192. {
  193.  
  194.     Serial.println(F("Before using 9DOF,we need to calibrate the compass frist,It will takes about 2 minutes."));
  195.     Serial.print("  ");
  196.     Serial.println(F("During  calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
  197.     Serial.print("  ");
  198.     Serial.println(F("If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
  199.     while (!Serial.find("ready"));
  200.     Serial.println("  ");
  201.     Serial.println("ready");
  202.     Serial.println("Sample starting......");
  203.     Serial.println("waiting ......");
  204.  
  205.     get_calibration_Data ();
  206.  
  207.     Serial.println("     ");
  208.     Serial.println("compass calibration parameter ");
  209.     Serial.print(mx_centre);
  210.     Serial.print("     ");
  211.     Serial.print(my_centre);
  212.     Serial.print("     ");
  213.     Serial.println(mz_centre);
  214.     Serial.println("    ");
  215. }
  216.  
  217.  
  218. void get_calibration_Data ()
  219. {
  220.     for (int i = 0; i < sample_num_mdate; i++)
  221.     {
  222.         get_one_sample_date_mxyz();
  223.         /*
  224.         Serial.print(mx_sample[2]);
  225.         Serial.print(" ");
  226.         Serial.print(my_sample[2]);                            //you can see the sample data here .
  227.         Serial.print(" ");
  228.         Serial.println(mz_sample[2]);
  229.         */
  230.  
  231.  
  232.  
  233.         if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
  234.         if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; //find max value
  235.         if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];
  236.  
  237.         if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
  238.         if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; //find min value
  239.         if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];
  240.  
  241.     }
  242.  
  243.     mx_max = mx_sample[1];
  244.     my_max = my_sample[1];
  245.     mz_max = mz_sample[1];
  246.  
  247.     mx_min = mx_sample[0];
  248.     my_min = my_sample[0];
  249.     mz_min = mz_sample[0];
  250.  
  251.  
  252.  
  253.     mx_centre = (mx_max + mx_min) / 2;
  254.     my_centre = (my_max + my_min) / 2;
  255.     mz_centre = (mz_max + mz_min) / 2;
  256.  
  257. }
  258.  
  259.  
  260.  
  261.  
  262.  
  263.  
  264. void get_one_sample_date_mxyz()
  265. {
  266.     getCompass_Data();
  267.     mx_sample[2] = Mxyz[0];
  268.     my_sample[2] = Mxyz[1];
  269.     mz_sample[2] = Mxyz[2];
  270. }
  271.  
  272.  
  273. void getAccel_Data(void)
  274. {
  275.     accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
  276.     Axyz[0] = (double) ax / 16384;
  277.     Axyz[1] = (double) ay / 16384;
  278.     Axyz[2] = (double) az / 16384;
  279. }
  280.  
  281. void getGyro_Data(void)
  282. {
  283.     accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
  284.     Gxyz[0] = (double) gx * 250 / 32768;
  285.     Gxyz[1] = (double) gy * 250 / 32768;
  286.     Gxyz[2] = (double) gz * 250 / 32768;
  287. }
  288.  
  289. void getCompass_Data(void)
  290. {
  291.     I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
  292.     delay(10);
  293.     I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);
  294.  
  295.     mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
  296.     my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
  297.     mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;
  298.  
  299.     Mxyz[0] = (double) mx * 1200 / 4096;
  300.     Mxyz[1] = (double) my * 1200 / 4096;
  301.     Mxyz[2] = (double) mz * 1200 / 4096;
  302. }
  303.  
  304. void getCompassDate_calibrated ()
  305. {
  306.     getCompass_Data();
  307.     Mxyz[0] = Mxyz[0] - mx_centre;
  308.     Mxyz[1] = Mxyz[1] - my_centre;
  309.     Mxyz[2] = Mxyz[2] - mz_centre;
  310. }
RAW Paste Data
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
 
Top