API tools faq
paste
Advertisement
eldarofdeath

29.3

eldarofdeath
Mar 29th, 2017
88
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 11.39 KB | None | 0 0
raw download clone embed print report
  1. #include <Math.h>
  2. #include <Wire.h>
  3. #include <LSM303.h>
  4. #include <L3G.h>
  5.  
  6. /*
  7. L3G - gyro
  8. The sensor outputs provided by the library are the raw 16-bit values
  9. obtained by concatenating the 8-bit high and low gyro data registers.
  10. They can be converted to units of dps (degrees per second) using the
  11. conversion factors specified in the datasheet for your particular
  12. device and full scale setting (gain).
  13.  
  14. Example: An L3GD20H gives a gyro X axis reading of 345 with its
  15. default full scale setting of +/- 245 dps. The So specification
  16. in the L3GD20H datasheet (page 10) states a conversion factor of 8.75
  17. mdps/LSB (least significant bit) at this FS setting, so the raw
  18. reading of 345 corresponds to 345 * 8.75 = 3020 mdps = 3.02 dps.
  19. */
  20.  
  21. /*
  22. LSM303D Magnetometer
  23. The sensor outputs provided by the library are the raw 16-bit values
  24. obtained by concatenating the 8-bit high and low accelerometer and
  25. magnetometer data registers. They can be converted to units of g and
  26. gauss using the conversion factors specified in the datasheet for your
  27. particular device and full scale setting (gain).
  28.  
  29. Example: An LSM303D gives a magnetometer X axis reading of 1982 with
  30. its default full scale setting of +/- 4 gauss. The M_GN specification
  31. in the LSM303D datasheet (page 10) states a conversion factor of 0.160
  32. mgauss/LSB (least significant bit) at this FS setting, so the raw
  33. reading of -1982 corresponds to 1982 * 0.160 = 317.1 mgauss =
  34. 0.3171 gauss.
  35.  
  36. LSM303DLH - akcelerometer
  37. In the LSM303DLHC, LSM303DLM, and LSM303DLH, the acceleration data
  38. registers actually contain a left-aligned 12-bit number, so the lowest
  39. 4 bits are always 0, and the values should be shifted right by 4 bits
  40. (divided by 16) to be consistent with the conversion factors specified
  41. in the datasheets.
  42.  
  43. Example: An LSM303DLH gives an accelerometer Z axis reading of -16144
  44. with its default full scale setting of +/- 2 g. Dropping the lowest 4
  45. bits gives a 12-bit raw value of -1009. The LA_So specification in the
  46. LSM303DLH datasheet (page 11) states a conversion factor of 1 mg/digit
  47. at this FS setting, so the value of -1009 corresponds to -1009 * 1 =
  48. 1009 mg = 1.009 g.
  49. */
  50. L3G gyro;
  51. LSM303 compass;
  52.  
  53. char report[120];
  54. struct AccelRotation {
  55.   double pitch;
  56.   double roll;
  57.   double yaw;
  58.   };
  59.  
  60.  struct Rotation{
  61.   float pitch;
  62.   float roll;
  63.   float yaw;
  64.   float hding;
  65.   };
  66. void setup()
  67. {
  68.   Serial.begin(9600);
  69.   Wire.begin();
  70.   compass.init();
  71.   compass.enableDefault();
  72.    if (!gyro.init())
  73.   {
  74.     Serial.println("Failed to autodetect gyro type!");
  75.     while (1);
  76.   }
  77.  
  78.   gyro.enableDefault();
  79.  
  80. }
  81.   /*
  82.    *  Deklaracie premennych
  83.    */
  84.    int indexPreZrychlenie=0;
  85.   int vzorkovanie=50;
  86.   float g=9.80665;
  87.   float gx=0,gy=0,gz=0;
  88.   float rychlostX=0, rychlostY=0, rychlostZ=0;
  89.   float rychlost;
  90.   float priemerneZrychlenieX=0, priemerneZrychlenieY=0, priemerneZrychlenieZ=0;
  91.   float akceleraciaX, akceleraciaY, akceleraciaZ;
  92.   float heading1, heading2, yaw, yawU;
  93.   float gyroX, gyroY, gyroZ;
  94.   float akceX, akceY, akceZ;
  95.   float magX, magY, magZ;
  96.   float pi= 3.14159;
  97.   float Xh, Yh;
  98.   float clearAccelX=0,clearAccelY=0,clearAccelZ=0;
  99.   //Rotation rotat;
  100.   AccelRotation rot;
  101.   /*int xMagnetMax=0, yMagnetMax=0, zMagnetMax=2;
  102.   int xMagnetMin=10000, yMagnetMin=10000, zMagnetMin=10000;
  103.   float xMagnetMap=0,yMagnetMap=0,zMagnetMap=0;*/
  104.   float heading;
  105.   /*
  106.   * Funkcia sluzi na vypocitanie azimutu - potrebujeme ho na urcenie smeru pohybu
  107.   */
  108. void readFromSenzors(){
  109.   compass.read();
  110.   gyro.read();
  111.   //Prepocet raw dat z gyro na DBS - degrees by second
  112.    gyroX=(gyro.g.x*8.75/1000); //roll
  113.    gyroY=(gyro.g.y*8.75/1000); //yaw,
  114.    gyroZ=(gyro.g.z*8.75/1000); //pitch, naklananie do stran
  115.  
  116.   //prepocet raw dat z akcelerometra na  G
  117.    akceX=(compass.a.x>>4)*0.001;
  118.    akceY=(compass.a.y>>4)*0.001;
  119.    akceZ=(compass.a.z>>4)*0.001;
  120.  
  121.   //prepocet raw dat z magnetometra na mgauss
  122.    magX=(compass.m.x*0.160);
  123.    magY=(compass.m.y*0.160);
  124.    magZ=(compass.m.z*0.160);
  125.   }
  126. /*
  127.  * Funkcia sluzi na vypocitanie Roll, Pitch, Yaw, Heading. Vypocitava azimuth ale treba dorobit tretiu os, respektive naklonenie osi.
  128.  *
  129.  *
  130.  */
  131.  void noGravity(){
  132.   gx=g*sin(rot.pitch);
  133.   gy=g*sin(rot.roll);
  134.   //float az=ax*sin(rot.pitch)+ay*sin(rot.roll);
  135.   //float az2=sqrt((akceleraciaZ*akceleraciaZ)+(akceleraciaX*akceleraciaX)+(akceleraciaY*akceleraciaY));
  136.   //double az=g*(sin(rot.roll)-sin(rot.pitch));
  137.   gz=sqrt((g*g)-(gx*gx)-(gy*gy));
  138.   if (akceleraciaZ<0)
  139.    {
  140.     gz=gz*(-1);
  141.    }
  142.  // float az2=sqrt(10.25*cos(rot.pitch)+10.25*cos(rot.roll));
  143.   clearAccelX=akceleraciaX-gx;
  144.   clearAccelY=akceleraciaY-gy;
  145.   clearAccelZ=akceleraciaZ-gz;
  146.   }
  147.   void nastavPriemZrych(){
  148.     indexPreZrychlenie=0;
  149.     priemerneZrychlenieX=0;
  150.     priemerneZrychlenieY=0;
  151.     priemerneZrychlenieZ=0;
  152.     };
  153.  
  154.  void priemerneZrychlenie(){
  155.   priemerneZrychlenieX+=clearAccelX; //Pridavame 10x zrychlenie a potom ho delime 10
  156.   priemerneZrychlenieY+=clearAccelY;
  157.   priemerneZrychlenieZ+=clearAccelZ;
  158.   indexPreZrychlenie++;
  159.   if (indexPreZrychlenie==10){
  160.     priemerneZrychlenieX/=10;
  161.     priemerneZrychlenieY/=10;
  162.     priemerneZrychlenieZ/=10;
  163.     pocitajRychlost();
  164.     nastavPriemZrych();
  165.     }
  166.   };
  167.  
  168.     void pocitajRychlost(){
  169.       rychlostX+=priemerneZrychlenieX*1; //v=a*t , cas mam 1 pretoze pridavam rychlost len kazdu sekundu
  170.       rychlostY+=priemerneZrychlenieY*1;
  171.       rychlostZ+=priemerneZrychlenieZ*1;
  172.     //  rychlost=sqrt((rychlostX^2)+(rychlostY^2)+(rychlostZ^2));
  173.       };
  174.      
  175.   void pocitajZrychlenie(){
  176.     akceleraciaX=akceX*g; //akceleracia v m.s-1 pre kazdu os
  177.     akceleraciaY=akceY*g;
  178.     akceleraciaZ=akceZ*g;
  179.     }
  180.    
  181.   void vypocitaj(){
  182.     //POCITANIE HEADING
  183.  
  184.  /*    Pocitat Roll, pitch a YAW sa neda s raw datami - hadze blbe udaje
  185.     rotat.pitch = (atan2(compass.a.x,sqrt((compass.a.y)^2+(compass.a.z)^2))*180.0)/pi;
  186.     rotat.roll = (atan2(compass.a.y,sqrt((compass.a.x)^2+(compass.a.z)^2))*180.0)/pi;
  187.     //rotat.yaw= atan2( (-compass.m.y*cos(rotat.roll) + compass.m.z*sin(rotat.roll) ) , compass.m.x*cos(rotat.pitch) + compass.m.z*sin(rotat.pitch)*sin(rotat.roll)+ compass.m.z*sin(rotat.pitch)*cos(rotat.roll)) *180/PI;
  188.  
  189.      float XXH = (compass.m.x*cos(rotat.pitch))+(compass.m.z*sin(rotat.pitch));
  190.     if (XXH>360){
  191.       XXH=XXH-360;
  192.       }
  193.       float YYH = (compass.m.x*sin(rotat.roll)*sin(rotat.pitch))+((compass.m.y*cos(rotat.roll))-(compass.m.z*sin(rotat.roll)*cos(rotat.pitch)));
  194.       if (YYH>360){
  195.       YYH=YYH-360;
  196.       }
  197.  
  198.     */
  199.     /*rot.pitch = (atan2(akceX,sqrt(akceY*akceY+akceZ*akceZ))*180.0)/pi;
  200.     rot.roll = (atan2(akceY,sqrt(akceX*akceX+akceZ*akceZ))*180.0)/pi;*/
  201.     rot.pitch = (atan2(akceX,sqrt(akceY*akceY+akceZ*akceZ))); //v radianoch
  202.     rot.roll = (atan2(akceY,sqrt(akceX*akceX+akceZ*akceZ))); //v radianoch
  203.     rot.yaw=atan2( (magZ*sin(rot.roll)-magY*cos(rot.roll) ) , (magX*cos(rot.pitch) + magY*sin(rot.pitch)*sin(rot.roll))+ (magZ*sin(rot.pitch)*cos(rot.roll)));
  204.    
  205.     Xh = magX*cos(rot.pitch)+magZ*sin(rot.pitch);
  206.     if (Xh>360){
  207.       Xh=Xh-360;
  208.       }
  209.     Yh = magX*sin(rot.roll)*sin(rot.pitch)+magY*cos(rot.roll)-magZ*sin(rot.roll)*cos(rot.pitch);
  210.     if (Yh>360){
  211.       Yh=Yh-360;
  212.       }
  213.       /*
  214.     Heading = vychylenie v stupnoch od magnetickeho severu = nas smmer ktory potrebujeme, pouzivame len osi X a Y kedze nepotrebujeme vidiet zdvih
  215.     if (heading<0) - normalizujeme vystup ak by boli zaporne hodnoty tak ich dame do kladnych
  216.  
  217.     Heading arctan(Y / X )
  218.     Xh = Xm cos Pitch + Zm sin Pitch
  219.     Yh = Xm sinRoll sinPitch + YmcosRoll - Zm sinRoll cos Pitch
  220.     */
  221.    heading2 = (atan2(Yh,Xh) * 180) / pi; //problem solved - roll a pitch musia byt v radianoch
  222.     if (heading2 < 0){
  223.       heading2 = 360+heading2;
  224.       }
  225.    
  226.    
  227.     //heading = compass.heading(); original compass heading, neda sa ale zmenit os X,Y,Z - potrebujeme aby kompas fungoval aj pri zmene osí
  228.     /*heading1 = (atan2(magY,magX) * 180) / pi; //problem s tretou osou resp. naklonenim device
  229.     if (heading1 < 0){
  230.       heading1 = 360+heading1;
  231.       }*/
  232.      
  233.    
  234.    
  235.     }
  236.    
  237.    
  238.     void vypis(){
  239.        /*
  240.   *  RAW hodnoty
  241.   *  snprintf(report, sizeof(report), "A: %6d %6d %6d    M: %6d %6d %6d    G: %6d %6d %6d",
  242.     compass.a.x, compass.a.y, compass.a.z,
  243.     compass.m.x, compass.m.y, compass.m.z,
  244.     gyro.g.x, gyro.g.y, gyro.g.z);  
  245.   Serial.println(report);*/
  246.  
  247.  
  248.   //Serial.println(rot.roll);
  249.   //Serial.println(rot.pitch);
  250.   //Gyroskop
  251.   //Serial.print("Gyroskop: "); Serial.print(gyroX); Serial.print(" "); Serial.print(gyroY); Serial.print(" "); Serial.print(gyroZ); Serial.println(" ");
  252.  
  253.   //Akcelerometer
  254.   //Serial.print("Roll: "); Serial.print(rot.roll); Serial.print("Pitch: "); Serial.print(rot.pitch); Serial.print("Yaw: "); Serial.println(rot.yaw);
  255.   /*Serial.print("Roll2: "); Serial.print(rotat.roll); Serial.print("Pitch2: "); Serial.println(rotat.pitch); */
  256.   //Serial.print("Akcelerometer: "); Serial.print(akceX); Serial.print(" "); Serial.print(akceY); Serial.print(" "); Serial.print(akceZ); Serial.println(" ");
  257.  
  258.   //Magnetometer
  259.   //Serial.print("Magnetometer: "); Serial.print(magX); Serial.print(" "); Serial.print(magY); Serial.print(" "); Serial.print(magZ); Serial.print(" ");
  260.   //Serial.println(rychlost);
  261.  
  262.   Serial.print(rot.pitch); Serial.print(";");Serial.print(rot.roll); Serial.print(";");
  263.   Serial.print(clearAccelX); Serial.print(";"); Serial.print(clearAccelY); Serial.print(";"); Serial.println(clearAccelZ);
  264.   /*
  265.    *    Vypocitane zlozky gravitacneho zrychlenia
  266.   */
  267.   //Serial.print(gx); Serial.print(" ");  Serial.print(gy);  Serial.print(" ");Serial.print(gz);  Serial.print(" ");// Serial.print(gz2);  Serial.print(" ");
  268.   /*
  269.    *    Merana akceleracia
  270.   */
  271.   //Serial.print("      Merane     "); Serial.print(akceleraciaX); Serial.print(" "); Serial.print(akceleraciaY); Serial.print(" "); Serial.print(akceleraciaZ);
  272.   /*
  273.    *    Uhly naklonu - roll + pitch
  274.   */
  275.   //Serial.print("      Uhly     "); Serial.print(rot.roll); Serial.print(" "); Serial.println(rot.pitch);
  276.   /*
  277.    *     Rozdiely medzi meranou a vypocitanou akceleraciou - pouzivame iba s pokojnom stave na zistenie rozdielov - nepresnosti
  278.   */
  279.  // Serial.print("   Rozdiely   "); Serial.print(clearAccelX); Serial.print(" "); Serial.print(clearAccelY); Serial.print(" "); Serial.print(clearAccelZ);
  280.  /*
  281.   *     Poloha v zavislosti od severu
  282.  */
  283.   //Serial.print("Heading: "); Serial.print(heading); Serial.print("Heading1: "); Serial.println(heading2);
  284.  /*
  285.   *      Rychlost na X,Y,Z osi
  286.  */
  287.  // Serial.print(" Rychlosti "); Serial.print(rychlostX); Serial.print(" "); Serial.print(rychlostY); Serial.print(" "); Serial.println(rychlostZ);
  288.   //Serial.print("Heading: "); Serial.print(heading1); Serial.print(" Roll "); Serial.print(rot.roll); Serial.print(" Pitch "); Serial.print(rot.pitch); Serial.print("\n");
  289.   //Serial.print("Heading1: "); Serial.print(heading1); Serial.print(" Heading2: "); Serial.println(heading2);
  290.   //Serial.print(" magX: "); Serial.print(magX); Serial.print(" magY: "); Serial.print(magY); Serial.print(" Xh: "); Serial.print(Xh); Serial.print(" Yh: "); Serial.println(Yh);
  291.    
  292.      
  293.       }
  294.  
  295.  
  296.  
  297. void loop()
  298. {
  299.      
  300.      readFromSenzors();
  301.      vypocitaj();
  302.      pocitajZrychlenie();
  303.      priemerneZrychlenie();
  304.      pocitajRychlost();
  305.      noGravity();
  306.      vypis();
  307.      /*if (indexPreZrychlenie==10){
  308.       nastavPriemZrych();
  309.       }*/
  310.      delay(vzorkovanie);
  311. }
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!