void encodeTargetData(uint8_t c){
  if (gps.encode(c)){
    float flat, flon;
    unsigned long fix_age;
    gps.f_get_position(&flat, &flon, &fix_age);
    
    if (fix_age == TinyGPS::GPS_INVALID_AGE){
      hasFix = false;
    }
    else if (fix_age > 5000){
      hasFix = false;
    }
    else{
      gps.get_position(&lat,&lon);
      hasFix = true;
    }
    
    if(gps.altitude() != TinyGPS::GPS_INVALID_ALTITUDE) {
      alt = (int16_t)gps.altitude();
      hasAlt = true;
    }else{
      hasAlt = false;
    }
  }
}

void read_gps(long lon, long lat)
{
  while (Serial2.available())
  {
    TinyGPS gps;
    int c = Serial2.read();
    if (gps.encode(c))
    {
      unsigned long fix;
      gps.get_position(&lat,&lon,&fix);
    }
  }
}

//Update the latitude and longitude global variables with GPS data (or place dummy values)
int update_lat_long() {
    unsigned long age;
    gps.f_get_position(&latitude, &longitude, &age); //Updates longitude and latitude
    if (age == TinyGPS::GPS_INVALID_AGE) {
        latitude = -500.0; //These are sentinel values for lat long; if GPS can't track them
        longitude = -500.0; 
        return -1;
    } else {
        return 0;
    }
}

//Update the datetime global variable with GPS data (or place dummy values)
int update_datetime() {
    unsigned long age;
    int year;
    byte month, day, hour, minute, second, hundredths;
    gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
    if (age == TinyGPS::GPS_INVALID_AGE) {
        sprintf(date,"1000 BC");
        return -1;
    } else {
        sprintf(date,"%02d/%02d/%02d %02d:%02d:%02d",month, day, year, hour, minute, second);
        return 0;
    }
}

void encodeTargetData(uint8_t c) {
  if (remoteGps.encode(c)) {
    unsigned long fix_age;
    remoteGps.get_position(&lat, &lon, &fix_age);
    
    if (fix_age == TinyGPS::GPS_INVALID_AGE) {
      HAS_FIX = false;
    } else if (fix_age > 5000) {
      HAS_FIX = false;
    } else {
      //TODO valid data
      lat = lat / 10;
      lon = lon / 10;

      if (remoteGps.altitude() != TinyGPS::GPS_INVALID_ALTITUDE) {
        alt = int16_t(remoteGps.altitude() / 100);
        HAS_ALT = true;
      }
      HAS_FIX = true;
    }
  }
}

void loop() {
	static TinyGPS gps;
	
	static unsigned long nextdisplay = millis();
	
	if(Serial1.available()) {
		gps.encode(Serial1.read());
	}
	
	if(millis() > nextdisplay) {
		long lat, lon;
		
		gps.get_position(&lat,&lon);
		
		Serial.print(lat);
		Serial.print(", ");
		Serial.print(lon);
		Serial.println();
		
		nextdisplay = millis() + 1000;
	}
}

void getgps(TinyGPS &gps) {
	// To get all of the data into varialbes that you can use in your code,
	// all you need to do is define variables and query the object for the
	// data. To see the complete list of functions see keywords.txt file in
	// the TinyGPS and NewSoftSerial libs.

	// Define the variables that will be used
	float latitude, longitude;
	// Then call this function
	gps.f_get_position(&latitude, &longitude);
	// You can now print variables latitude and longitude
	Serial.print("Lat/Long: ");
	Serial.print(latitude, 5);
	Serial.print(", ");
	Serial.println(longitude, 5);

	// Same goes for date and time
	int year;
	byte month, day, hour, minute, second, hundredths;
	gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths);
	// Print data and time
	Serial.print("Date: ");
	Serial.print(month, DEC);
	Serial.print("/");
	Serial.print(day, DEC);
	Serial.print("/");
	Serial.print(year);
	Serial.print("  Time: ");
	Serial.print(hour, DEC);
	Serial.print(":");
	Serial.print(minute, DEC);
	Serial.print(":");
	Serial.print(second, DEC);
	Serial.print(".");
	Serial.println(hundredths, DEC);
	//Since month, day, hour, minute, second, and hundr

	// Here you can print the altitude and course values directly since
	// there is only one value for the function
	Serial.print("Altitude (meters): ");
	Serial.println(gps.f_altitude());
	// Same goes for course
	Serial.print("Course (degrees): ");
	Serial.println(gps.f_course());
	// And same goes for speed
	Serial.print("Speed(kmph): ");
	Serial.println(gps.f_speed_kmph());
	Serial.println();

	// Here you can print statistics on the sentences.
	unsigned long chars;
	unsigned short sentences, failed_checksum;
	gps.stats(&chars, &sentences, &failed_checksum);
	//Serial.print("Failed Checksums: ");Serial.print(failed_checksum);
	//Serial.println(); Serial.println();
}

char check_gps()
{
  newData=false;
  unsigned long chars;
  unsigned short sentences, failed;

  // For one second we parse GPS data and report some key values
  for (unsigned long start = millis(); millis() - start < 1000;)
  {
    while (Serial2.available())
    {
      char c = Serial2.read();
      // Serial.write(c); // uncomment this line if you want to see the GPS data flowing
      if (gps.encode(c)) // Did a new valid sentence come in?
        newData = true;
    }
  }

  if (newData)
  {
    float flat, flon;
    unsigned long age;
    int _year;
    byte _month, _day,_hour,_minute,_second,_hundredths;        
    gps.f_get_position(&flat, &flon, &age);
        gps.crack_datetime(&_year,&_month,&_day,&_hour,&_minute,&_second,&_hundredths,&age);
        flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat, 6;
        flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon, 6;
    dtostrf(flat, 11, 6, gps_lat); 
    dtostrf(flon, 10, 6, gps_lon); 
        
        strcpy(gps_sms,"lat:");
        strcat(gps_sms,gps_lat);
        strcat(gps_sms,"\n");
        strcat(gps_sms,"lon:");
        strcat(gps_sms,gps_lon);
        strcat(gps_sms,"\n");
        strcat(gps_sms,"time:");
        
    itoa(_year,gps_year,10);
    strcat(gps_sms,gps_year);
        
    itoa(_month,gps_mon,10);
    if(strlen(gps_mon)==1)
      strcat(gps_sms,"0");
    strcat(gps_sms,gps_mon);
        
    itoa(_day,gps_day,10);
        if(strlen(gps_day)==1)
      strcat(gps_sms,"0");
    strcat(gps_sms,gps_day);
        
    itoa(_hour,gps_hour,10);
        if(strlen(gps_hour)==1)
      strcat(gps_sms,"0");
    strcat(gps_sms,gps_hour);
        
    itoa(_minute,gps_min,10);
        if(strlen(gps_min)==1)
      strcat(gps_sms,"0");
    strcat(gps_sms,gps_min);
        
    itoa(_second,gps_sec,10);
        if(strlen(gps_sec)==1)
      strcat(gps_sms,"0");
    strcat(gps_sms,gps_sec);        
        
    Serial.println(gps_sms);


  }
  



}

//Uses all the sensors to update all of the global variables that have to do with logging.
int update_data() {
    p_sensor.Barometer_MS5803(); //Gathers data from external temp/pressure sensor 
    //Updates temperatures, pressure, and power 
    pressure = p_sensor.MS5803_Pressure();
    external_temp = p_sensor.MS5803_Temperature();
    internal_temp = temperature.read();
    power = ain.read();
    //Data gathered from GPS 
    bool gps_ready = gps_readable();
    int max_gps_requests = 4;
    int gps_counter = 0;
    while (!gps_ready && gps_counter < max_gps_requests) {
        gps_ready = gps_readable();
        gps_counter++;
        //printf("Waiting!\n");
    }
    if (gps_ready) {
        update_lat_long();
        altitude = gps.f_altitude();
        precision = gps.hdop();
        gps.stats(&encoded_chars, &good_sentences, &failed_checksums);
        update_datetime();
    } else {
        //Place DUMMY GPS VALUES 
        latitude = -1000.0;
        longitude = -1000.0; 
        altitude = -1000.0; 
        precision = -1000;
        sprintf(date,"20000 BC");
        encoded_chars = -1;
        good_sentences = -1;
        failed_checksums = -1;
    }
    return 0; //Data update was a success! 
}

void loop() {
	bool newData = false;
	unsigned long start = millis();
	while (millis() - start < 1000) {
		while (nss.available()) {
			if (gps.encode(nss.read()))
				newData = true;
		}
	}

	if (newData) {
		if (gps.hdop() < MAX_HDOP)
			signal_fixed_status();
		else
			signal_instable_status();

		if (!gps_filename) {
			gps_filename = create_gps_file();
		}

		String line = create_gps_line();
		gps_filename.println(line);
		gps_filename.flush();

		Serial.println(line);
	} else {
		signal_unavailable_status();
	}
}

void MXS2201::get_info(float *latitude, float *longitude, float *altitude,float *speed_mps,int *year, byte *month, byte *day,
		byte *hour, byte *minute, byte *second,byte *hundredths,unsigned long *fix_age)
{
	bool newData = false;


	// For one second we parse GPS data and report some key values
	for (unsigned long start = millis(); millis() - start < 1500;)
	{
		while (_mySerial.available())
		{
			char c = _mySerial.read();
			if (gps.encode(c)) // Did a new valid sentence come in?
				newData = true;
		}
	}

	if (newData)
	{
		gps.f_get_position(latitude,longitude,fix_age);
		gps.crack_datetime(year, month, day,
				hour, minute, second,hundredths,fix_age );
		*altitude=gps.f_altitude();
		*speed_mps=gps.f_speed_mps();
		return;
	}
}

/* GPS listen thread */
void gps_spinOnce(void) {
   if(rx_ready(gps_port)) {
      gps_input = rx_byte(gps_port);

      if(gps.encode(gps_input)) {
         gps.get_position(&lat, &lon);

         if( gps_pub.reset() ) {
            gps_pub.append(lat);
            gps_pub.append(lon);
            // TODO: fill in rest of GPS message
            gps_pub.append(gps.altitude());
            gps_pub.append(gps.hdop());
            gps_pub.finish();
         }

         if( gps_pub2.reset() ) {
            gps_pub2.append(lat);
            gps_pub2.append(lon);
            // TODO: fill in rest of GPS message
            gps_pub2.finish();
         }
      } 
   }
}

char gps_get_data() {
    float lat, lon = 0.0;
    unsigned long fixAge = 0;
    char uart_buffer[512] = {};
    int ctr = 0;

    uart_init(9600);// The default baud rate for this GPS device is 9600

    uart_read(uart_buffer, sizeof(uart_buffer));
    while (uart_buffer[ctr] != NULL){
        char charRead = uart_buffer[ctr];
        GPSDevice.encode(charRead);
        if ((charRead == ',') && (GPSDevice._term_number == 1)) {
            itsAGPRMCMessage = !GPSDevice.gpsstrcmp(GPSDevice._term, _GPRMC_TERM);
        }

        if ((charRead == 'A' || charRead == 'V') && itsAGPRMCMessage) {
            dataStatus = charRead;
            itsAGPRMCMessage = 0;
        }
        ctr++;
    }
    GPSDevice.f_get_position(&lat, &lon, &fixAge);
    printf("lat: %.4f\t", lat);
    printf("lon: %.4f\t", lon);
    printf("alt: %.1f\t", GPSDevice.f_altitude());
    printf("sta: %c\n", dataStatus);

    return dataStatus;
}

File create_gps_file() {
	int year;
	unsigned long age;
	byte month, day, hour, minute, second, hundredth;
	gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredth, &age);

	String filename;
	filename.concat(GPS_DIR);
	filename.concat('/');
	filename.concat(year);
	filename.concat('/');
	filename.concat(month);
	filename.concat('/');
	filename.concat(day);
	filename.concat('/');
	filename.concat(hour);
	filename.concat(minute);
	filename.concat(second);
	filename.concat(".GPS");

	char buf[filename.length() + 1];
	filename.toCharArray(buf, sizeof(buf));

	Serial.print("Criando arquivo... ");
	Serial.println(buf);
	return create_file(buf);
}

String create_gps_line() {
	float lat, lon;
	unsigned long age;
	gps.f_get_position(&lat, &lon, &age);

	short satellites = gps.satellites();

	String line;
	line.concat(line_number++);
	line.concat(',');

	char buf[12];
	line.concat(dtostrf(lat, 4, 6, buf));
	line.concat(',');

	line.concat(dtostrf(lon, 4, 6, buf));
	line.concat(',');

	int year;
	byte month, day, hour, minute, second, hundredth;
	gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredth, &age);

	line.concat(day);
	line.concat('/');
	line.concat(month);
	line.concat('/');
	line.concat(year);
	line.concat(" ");
	line.concat(hour);
	line.concat(":");
	line.concat(minute);
	line.concat(":");
	line.concat(second);
	line.concat(',');
	line.concat(satellites);
	line.concat(',');
	line.concat(dtostrf(gps.f_altitude(), 4, 2, buf));
	line.concat(',');
	line.concat(gps.hdop());
	line.concat(',');
	line.concat(age);

	return line;
}

void gps_update(float *latitude, float *longitude, unsigned long *time) {
	static const int cum_days[] = {
		0, 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334
	};

	int year;
	unsigned long age;
	byte month, day, hour, minute, second, centisecond;

	while(Serial1.available())
		gps.encode(Serial1.read());

	gps.f_get_position(latitude,longitude);
	gps.crack_datetime(&year,&month,&day,&hour,&minute,&second,
		&centisecond,&age);

	*time = ((((year - 1970)*365 + cum_days[month])*24 + hour)*60
		+ minute)*60 + second + (10*centisecond + age)/1000;
}

//Check if we can read new GPS data (keep check for about 0.5 seconds) and return true or false.
bool gps_readable() {
    bool new_data = false;
    for (unsigned long start = time(NULL); time(NULL) - start < 0.5;) {
        while (gps_ser.readable()) {
            char c = gps_ser.getc();
            //printf("%c",c);
            if (gps.encode(c)) new_data = true;
        }
    }
    return new_data;
}

static char* print_gps_time(char *buffer)
{
    int year;
    byte month, day, hour, minute, second, hundredths;

    gps.crack_datetime(
        &year, &month, &day,
        &hour, &minute, &second, &hundredths);

    sprintf(buffer, "%d-%02d-%02d %02d:%02d:%02d.%02d",
        year, month, day,
        hour, minute, second, hundredths);
}

void loop() {
	int delayMillis = 10000;
	for (int i = 0; i < baudRates.length(); i++) {
		uart_gps.begin(baudRates[i]);
		int startMillis = millis();
		while ((millis() - startMillis) < delayMillis) {
			while (uart_gps.available()) // While there is data on the RX pin...
			{
				int c = uart_gps.read(); // load the data into a variable...
				Serial.print("baud = ");
				Serial.print(baudRates[i], DEC);
				Serial.print(" ");
				Serial.print(c);
				Serial.println();
				if (gps.encode(c)) // if there is a new valid sentence...
				{
					getgps(gps); // then grab the data.
				}
			}
		}
	}
}

int main(void) {    
    
    Config32MHzClock(); // Setup the 32MHz Clock. Should really be using 2MHz...
    
    // Setup output and input ports.
    LEDPORT.DIRSET = 0xFF; 
    LEDPORT.OUT = 0xFF;
    AD9835_PORT.DIRCLR = 0x40;
    PORTC.DIRSET = 0x04;

    // Start up the timer.
    init_timer();
    
    sensors.begin();
    sensors.requestTemperatures();
    
    // Wait a bit before starting the AD9835.
    // It seems to take a few hundred ms to 'boot up' once power is applied.
    _delay_ms(500); 
    
    // Configure the AD9835, and start in sleep mode.
    AD9835_Setup();
    AD9835_Sleep();
    
    // Setup the AD9835 for our chosen datamode.
    TX_Setup();
    AD9835_Awake();
    
    // Broadcast a bit of carrier.
    _delay_ms(1000);
    
    TXString("Booting up...\n"); // Kind of like debug lines.
    
    // Start up the GPS RX UART.
    init_gps();
    
    // Turn Interrupts on.
    PMIC.CTRL = PMIC_HILVLEN_bm | PMIC_LOLVLEN_bm;
    sei();
    
    sendNMEA("$PUBX,00"); // Poll the UBlox5 Chip for data.
    
    //TXString("GPS Active, Interrupts On.\n");
    
    int found_sensors = sensors.getDeviceCount();
    
    //sprintf(tx_buffer,"Found %u sensors.\n",found_sensors);
   // TXString(tx_buffer);
    unsigned int counter = 0; // Init out TX counter.
    
 
    while(1){
        // Identify every few minutes
        if ((counter%30 == 0)&&(data_mode != FALLBACK)) TXString("DE VK5VZI Project Horus HAB Launch - projecthorus.org \n");
    
	    // Read ADC PortA pin 0, using differential, signed input mode. Negative input comes from pin 1, which is tied to ground. Use VCC/1.6 as ref.
	    uint16_t temp = readADC(); 
	    float bat_voltage = (float)temp * 0.001007572056668* 8.5;
        floatToString(bat_voltage,1,voltString);
   
    
    
        // Collect GPS data
        
        gps.f_get_position(&lat, &lon);
	    sats = gps.sats();
	    if(sats>2){LEDPORT.OUTCLR = 0x80;}
	    speed = gps.f_speed_kmph();
	    altitude = (long)gps.f_altitude();
	    gps.crack_datetime(0, 0, 0, &time[0], &time[1], &time[2]);
	    
	    floatToString(lat, 5, latString);
	    floatToString(lon, 5, longString);
	    
        sensors.requestTemperatures();
        _intTemp = sensors.getTempC(internal);
        _extTemp = sensors.getTempC(external);
        if (_intTemp!=85 && _intTemp!=127 && _intTemp!=-127 && _intTemp!=999) intTemp = _intTemp;
        if (_extTemp!=85 && _extTemp!=127 && _extTemp!=-127 && _extTemp!=999) extTemp = _extTemp;
	    
	    if(data_mode != FALLBACK){
	    
            // Construct our Data String
            sprintf(tx_buffer,"$$DARKSIDE,%u,%02d:%02d:%02d,%s,%s,%ld,%d,%d,%d,%d,%s",counter++,time[0], time[1], time[2],latString,longString,altitude,speed,sats,intTemp,extTemp,voltString);
            
            
            // Calculate the CRC-16 Checksum
            char checksum[10];
            snprintf(checksum, sizeof(checksum), "*%04X\n", gps_CRC16_checksum(tx_buffer));
         
            // And copy the checksum onto the end of the string.
            memcpy(tx_buffer + strlen(tx_buffer), checksum, strlen(checksum) + 1);
        }else{
            // If our battery is really low, we don't want to transmit much data, so limit what we TX to just an identifier, battery voltage, and our position.
            
            sprintf(tx_buffer, "DE VK5VZI HORUS8 %s %s %s %ld", bat_voltage, latString, longString,altitude);
        }
        
        // Blinky blinky...
        LEDPORT.OUTTGL = 0x20;
//.........这里部分代码省略.........