integracion_v1.4_NANO

#include <TinyGPS.h>
#include <SoftwareSerial.h>
#include <Wire.h>     // Reference the I2C Library
#include <HMC5883L.h> // Reference the HMC5883L Compass Library

SoftwareSerial GPS(9,10);
TinyGPS gps;
unsigned long fix_age;
long lat, lon;
float LAT, LON;
HMC5883L compass; // Store our compass as a variable.
int error = 0; // Record any errors that may occur in the compass.
float heading, headingDegrees;
int sail_pos = 0, rudder_pos = 0;
int ledgps = 12;


void setup(){
  GPS.begin(9600);
  Serial.begin(9600);

  Wire.begin(); // Start the I2C interface.
  compass = HMC5883L(); // Construct a new HMC5883 compass.
  Serial.println("Setting HMC5883L scale to +/- 1.3 Ga "); //0.88, 1.3, 1.9, 2.5, 4.0, 4.7, 5.6, 8.1
  error = compass.SetScale(1.3); // Set the scale of the compass.
  if(error != 0) // If there is an error, print it out.
  Serial.println(compass.GetErrorText(error));
  
  Serial.println("Setting HMC5883L measurement mode to continous.");
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  if(error != 0) // If there is an error, print it out.
  Serial.println(compass.GetErrorText(error));
  
  pinMode(ledgps, OUTPUT);
  
}

void loop(){
  short sat;
  float flat, flon;
  unsigned short sentences, failed_checksum;
  bool newdata = false;
  unsigned long start = millis();
  unsigned long fix_age, time, date, altitude, speed, course;
  unsigned long chars;
  int wind_vane = 0, mast_pos=0;
      
  /*---SKM53 GPS-------------------------------------------------------------*/
  gps.get_datetime(&date, &time, &fix_age); // get time in hhmmsscc, date in ddmmyy
  gps.get_position(&lat, &lon, &fix_age);   // get +/- lat/long in 100000ths of a degree
 
  speed = gps.speed();                      // get speed in 100ths of a knot
  altitude = gps.altitude();                // get in centimeters
  course = gps.course();                    // get course in 100ths of a degree
  sat = gps.satellites();                   // get # sats available
  
  time = (time - 3000000);                           //Set TMZ GMT-3//
  if (time < 0) {(time = time + 24000000);};         //Set TMZ GMT-3//
  
  /*---HMC5883L--------------------------------------------------------------*/
  // Retrive the raw values from the compass (not scaled). MAX 15Hz
  MagnetometerRaw raw = compass.ReadRawAxis();
  MagnetometerScaled scaled = compass.ReadScaledAxis();
  int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
  heading = atan2(scaled.YAxis, scaled.XAxis);

  headingDegrees =  adjustcompass(heading);
  
  /*-------------------------------------------------------------------------*/
  
  verifygps(fix_age);
  
  /**  Main loop /* Ask GPS position at 2Hz */
  while (millis() - start < 500)
    { 
    if (feedgps ()){newdata = true;}
    }

  if (newdata) {gpsdump(gps);}
  
  /*---------------WindVane ------------------------------*/
  wind_vane = analogRead(A0); /*WindVane pin 0 - AS5030*/
  mast_pos = analogRead(A1);  /*Mast Pos pin 1 - Potentiomenter @ 10K ohm, */
  
  wind_vane = map(wind_vane, 0, 1023, 0, 360);
  mast_pos = map(mast_pos, 0, 1023, 0, 360);
  
  wind_vane = (wind_vane - 285);
  if (wind_vane < 0) (wind_vane = wind_vane+360);
  
  //true_wind = mast_pos + wind_vane;
  //if (true_wind > 360) (true_wind = true_wind-360);
  /*------------------------------------------------------*/
  
  /*Actuators*/
  set_rudder(rudder_pos);
  set_sail(sail_pos);
   
  /*-----------------Print Output-------------------------*/
  Serial.print("Date/Time:");
  Serial.print(date);
  Serial.print("-");
  Serial.print(time);
  Serial.print(", Lat,Long: ");
  Serial.print(LAT/100000,7);
  Serial.print(",");
  Serial.print(LON/100000,6);
//  Serial.print(", Alt:");
//  Serial.print(altitude/100);
  Serial.print(", Course:");
  Serial.print(course/100);
  Serial.print(", Sat:");
  Serial.print(sat);
  Serial.print(", Vel(km/h):");
  Serial.print(speed/100*1.852,2); // Velocidad en km/h

  /*WindVane AS5030*/
  Serial.print(" | WindVane:");
  Serial.print(wind_vane);
  Serial.print(", Mast:");
  Serial.print(mast_pos);

  /*Compass HMC5883L*/  
  Serial.print(", | Compass:");
  Serial.print(headingDegrees);

  /*Actuators*/  
  Serial.print(",| Rudder:");
  Serial.print(rudder_pos);
  Serial.print(", SailWinch:");
  Serial.print(sail_pos);
  Serial.print(" | ");
  
  digitalWrite(ledgps, LOW);
  
}

int set_rudder(int rudder_pos)
{
  rudder_pos = map(rudder_pos, 0, 360, 0, 1024); //Adjust servo pulses with Rudder-degrees
  return 0;
}

int set_sail(int sail_pos)
{
  sail_pos = map(sail_pos, 0, 360, 0, 1024); //Adjust servo pulses with Winch-Turns
  return 0;
}

float adjustcompass(float heading)
{
  float declinationAngle = 0.1376;
  heading += declinationAngle;
  
  if(heading < 0)
    heading += 2*PI;

  if(heading > 2*PI)
    heading -= 2*PI;

  headingDegrees = heading * 180/M_PI;
  
  return headingDegrees;
}

boolean verifygps(unsigned long fix_age)
{
if (fix_age == TinyGPS::GPS_INVALID_AGE) {
      Serial.println("Error: No Fixed GPS!");
      digitalWrite(ledgps, LOW);
      return false;
      }
  else 
      if (fix_age > 5000) {
        Serial.println("Warning: Possible Invalid GPS data!");
        digitalWrite(ledgps, LOW);
        return false;
        }
      else {
        Serial.println("Status: Fixed!");
        digitalWrite(ledgps, HIGH);
        return true;
        }
}

bool feedgps(){
  while (GPS.available())
  {
    if (gps.encode(GPS.read()))
    return true;
  }
  return false;
}


void gpsdump(TinyGPS &gps)
{
  gps.get_position(&lat, &lon);
  LAT = lat;
  LON = lon;
  {
    feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
  }
}