integracion_v1.9.3_NANO

#include <TinyGPS.h>          
#include <SoftwareSerial.h>    
#include <Wire.h>              
#include <HMC5883L.h>          

#define WARNING_LED 11
#define STATUS_LED 12
#define ERROR_LED 13
#define WINDVANE_PORT A0
#define MAST_PORT A1

#define GPS_TX  9
#define GPS_RX  10
#define GPS_MAX_VALID_FIXAGE 5000

#define COMPASS_SCALE 1.3
#define BUE_DECLINATION_ANGLE 0.1376

#define WINDVANE_ASSEMBLY_OFFSET 285

#define CHANNEL_A 6
#define CHANNEL_B 7
#define CHANNEL_C 8

SoftwareSerial GPS(GPS_TX,GPS_RX);
TinyGPS gps;
unsigned long fix_age, id=0;
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 ch1=0, ch3=0, ch5=0;

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

  pinMode(STATUS_LED, OUTPUT);
  pinMode(ERROR_LED, OUTPUT);

  Serial.println("********************************************");
  Serial.println("Victoria Project Sailor Firmware v1");
  Serial.println("********************************************");
  
  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(COMPASS_SCALE); // Set the scale of the compass.
  if(error != 0) {// If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));
    digitalWrite(ERROR_LED, HIGH);
    }
  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));
    digitalWrite(ERROR_LED, HIGH);
    }
  
}

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//
  
  verifygps(fix_age);
  
  /*---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);
  
  /*---Main loop ------------------------------------------------------------*/
  /* Ask GPS position at 2Hz */
  while (millis() - start < 500)
    { 
    if (feedgps ()){newdata = true;}
    }

  if (newdata) {gpsdump(gps);}
  
  /*---WindVane--------------------------------------------------------------*/
  wind_vane = analogRead(WINDVANE_PORT);
  mast_pos = analogRead(MAST_PORT);
  
  wind_vane = map(wind_vane, 0, 1023, 0, 360);
  mast_pos = map(mast_pos, 0, 1023, 0, 360);
  
  wind_vane = (wind_vane - WINDVANE_ASSEMBLY_OFFSET);
  if (wind_vane < 0) (wind_vane = wind_vane+360);
  
  //true_wind = mast_pos + wind_vane;
  //if (true_wind > 360) (true_wind = true_wind-360);
  
  /*---Read RC Receiver -----------------------------------------------------*/
  
  ch1 = pulseIn(CHANNEL_A,HIGH);  //Read and store channel 1 winch
  //ch1 = map(ch1,1000,2000,0,100);
  ch3 = pulseIn(CHANNEL_B,HIGH);  //Read and store channel 3 rudder
  //ch3 = map(ch3,1000,2000,-100,100); 
  ch5 = pulseIn(CHANNEL_C,HIGH);  //Read and store channel 5 AUTO/MANUAL
  //ch5 = map(ch5,1000,2000,0,100); 
  
  /*---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("ID:");
  Serial.print(id);
  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(",| SailWinch:");
  Serial.print(ch3);
  Serial.print(", Rudder:");
  Serial.print(ch1);
  Serial.print(", Mode:");
  Serial.print(ch5);
  Serial.print(" | ");
  
  digitalWrite(STATUS_LED, LOW);
  id++;
  
}

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) {
  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/
  // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
  // BUE  is: 7° 53' W  which is 7.883 Degrees, or (which we need) 0.1375901220 radians, I will use 0.1376
  float declinationAngle = BUE_DECLINATION_ANGLE;
  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(ERROR_LED, HIGH);
      return false;
      }
  else 
      if (fix_age > GPS_MAX_VALID_FIXAGE) {
        Serial.println("Warning: Possible Invalid GPS data!");
        digitalWrite(ERROR_LED, HIGH);
        return false;
        }
      else {
        Serial.println("Status: Fixed!");
        digitalWrite(ERROR_LED, LOW);
        digitalWrite(STATUS_LED, 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
}