Arduino: GPS, magnetometr, gyro, accelerometr

sns

#include <Wire.h>
#include <HMC5883L.h>
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include "I2Cdev.h"
#include "MPU6050.h"

  // POCZATEK DEKLARACJI HMC5883L ----------------------------------
HMC5883L compass;
  // KONIEC DEKLARACJI HMC5883L ----------------------------------
  
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

    // POCZATEK DEKLARACJI GPS ----------------------------------
/*
   This sample sketch demonstrates the normal use of a TinyGPS++ (TinyGPSPlus) object.
   It requires the use of SoftwareSerial, and assumes that you have a
   4800-baud serial GPS device hooked up on pins 4(rx) and 3(tx).
*/
static const int RXPin = 4, TXPin = 3;
static const uint32_t GPSBaud = 9600;

// The TinyGPS++ object
TinyGPSPlus gps;

// The serial connection to the GPS device
SoftwareSerial ss(RXPin, TXPin);
 
     // KONIEC DEKLARACJI GPS ----------------------------------
	 
void setup()
{
  //Serial.begin(9600);
      Serial.begin(115200); //GPS takie potrzebuje?
  
    // POCZATEK INIZJALIZACJI GPS ----------------------------------

  ss.begin(GPSBaud);
  Serial.println(F("A simple demonstration of TinyGPS++ with an attached GPS module"));
  Serial.print(F("Testing TinyGPS++ library v. ")); Serial.println(TinyGPSPlus::libraryVersion());
  Serial.println(F("by Mikal Hart"));
  Serial.println();
  
    // KONIEC INIZJALIZACJI GPS ----------------------------------
	
	
  // POCZATEK INIZJALIZACJI HMC5883L ----------------------------------
  Serial.println("Initialize HMC5883L");
  while (!compass.begin())
  {
    Serial.println("Nie odnaleziono HMC5883L, sprawdz polaczenie!");
    while (1) {}
  }
 
  // Ustawienie zakresu pomiarowego
  // +/- 0.88 Ga: HMC5883L_RANGE_0_88GA
  // +/- 1.30 Ga: HMC5883L_RANGE_1_3GA (domyslny)
  // +/- 1.90 Ga: HMC5883L_RANGE_1_9GA
  // +/- 2.50 Ga: HMC5883L_RANGE_2_5GA
  // +/- 4.00 Ga: HMC5883L_RANGE_4GA
  // +/- 4.70 Ga: HMC5883L_RANGE_4_7GA
  // +/- 5.60 Ga: HMC5883L_RANGE_5_6GA
  // +/- 8.10 Ga: HMC5883L_RANGE_8_1GA
  compass.setRange(HMC5883L_RANGE_1_3GA);
 
  // Ustawienie trybu pracy
  // Uspienie:              HMC5883L_IDLE
  // Pojedynczy pomiar:     HMC5883L_SINGLE
  // Ciagly pomiar: HMC5883L_CONTINOUS (domyslny)
  compass.setMeasurementMode(HMC5883L_CONTINOUS);
 
  // Ustawienie czestotliwosci pomiarow
  //  0.75Hz: HMC5883L_DATARATE_0_75HZ
  //  1.50Hz: HMC5883L_DATARATE_1_5HZ
  //  3.00Hz: HMC5883L_DATARATE_3HZ
  //  7.50Hz: HMC5883L_DATARATE_7_50HZ
  // 15.00Hz: HMC5883L_DATARATE_15HZ (domyslny)
  // 30.00Hz: HMC5883L_DATARATE_30HZ
  // 75.00Hz: HMC5883L_DATARATE_75HZ
  compass.setDataRate(HMC5883L_DATARATE_15HZ);
 
  // Liczba usrednionych probek
  // 1 probka:  HMC5883L_SAMPLES_1 (domyslny)
  // 2 probki: HMC5883L_SAMPLES_2
  // 4 probki: HMC5883L_SAMPLES_4
  // 8 probki: HMC5883L_SAMPLES_8
  compass.setSamples(HMC5883L_SAMPLES_1);
  
  // KONIEC INIZJALIZACJI HMC5883L ----------------------------------
  
    Wire.begin();
   // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}
 
void loop()
{
	// POCZATEK  GPS ----------------------------------
  // This sketch displays information every time a new sentence is correctly encoded.
  while (ss.available() > 0) {
    char c = byte(ss.read());
    if (gps.encode(c))
      //displayInfoFromGPS();
      return;
  }
  if (millis() > 5000 && gps.charsProcessed() < 10)
  {
    Serial.println(F("No GPS detected: check wiring."));
    while(true);
  }
   	// KONIEC  GPS ----------------------------------
	
    
    // POCZATEK  HMC5883L ----------------------------------
    
    // Pobranie pomiarow surowych
    Vector raw = compass.readRaw();
   
    // Pobranie pomiarow znormalizowanych
    Vector norm = compass.readNormalize();
   
    //printCompassData(raw, norm);
     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

        Serial.print("a/g:\t");
        Serial.print(ax); Serial.print("\t");
        Serial.print(ay); Serial.print("\t");
        Serial.print(az); Serial.print("\t");
        Serial.print(gx); Serial.print("\t");
        Serial.print(gy); Serial.print("\t");
        Serial.println(gz);
}


void printCompassData(Vector raw, Vector norm) {
// Wyswielnie wynikow
    Serial.print(" Xraw = ");
    Serial.print(raw.XAxis);
    Serial.print(" Yraw = ");
    Serial.print(raw.YAxis);
    Serial.print(" Zraw = ");
    Serial.print(raw.ZAxis);
    Serial.print(" Xnorm = ");
    Serial.print(norm.XAxis);
    Serial.print(" Ynorm = ");
    Serial.print(norm.YAxis);
    Serial.print(" ZNorm = ");
    Serial.print(norm.ZAxis);
    Serial.println();  
  
    // KONIEC HMC5883L ----------------------------------
}


void displayInfoFromGPS()
{
  Serial.print(F("Location: ")); 
  if (gps.location.isValid())
  {
    Serial.print(gps.location.lat(), 6);
    Serial.print(F(","));
    Serial.print(gps.location.lng(), 6);
  }
  else
  {
    Serial.print(F("INVALID"));
  }

  Serial.print(F("  Date/Time: "));
  if (gps.date.isValid())
  {
    Serial.print(gps.date.month());
    Serial.print(F("/"));
    Serial.print(gps.date.day());
    Serial.print(F("/"));
    Serial.print(gps.date.year());
  }
  else
  {
    Serial.print(F("INVALID"));
  }

  Serial.print(F(" "));
  if (gps.time.isValid())
  {
    if (gps.time.hour() < 10) Serial.print(F("0"));
    Serial.print(gps.time.hour());
    Serial.print(F(":"));
    if (gps.time.minute() < 10) Serial.print(F("0"));
    Serial.print(gps.time.minute());
    Serial.print(F(":"));
    if (gps.time.second() < 10) Serial.print(F("0"));
    Serial.print(gps.time.second());
    Serial.print(F("."));
    if (gps.time.centisecond() < 10) Serial.print(F("0"));
    Serial.print(gps.time.centisecond());
  }
  else
  {
    Serial.print(F("INVALID"));
  }

  Serial.println();
}