/**********************************************************************/
//	msa_v4_2.c
/**********************************************************************/

/**********************************************************************/
// Author: Michael A. Petersen
// Copyright (C) 2013 Michael A. Petersen
// Original code date: 5/1/2013
//
// This is the final software for running the MSAv4 on a
// Debian OS installed on a Raspberry Pi (RPi).
// This program is dependent on the bcm 2385 C library, maintained my
// Mike McCauley at http://www.airspayce.com/mikem/bcm2835/
// This program must be compiled on the RPi using the gcc compiler and
// a properly installed version of the bcm3835 1.25 or newer
//
// After installing bcm2835, you can build the MSAv4 program
// with:
// gcc -o msa_v4 msa_v4.c -l bcm2835 -l m -l pthread
// sudo ./msa_v4
//
// This code is written specifically for the RPi, version one, which uses
// the I2C '0' bus, not I2c '1'. The library had to be modified to
// accommodate this. However, RPi V2 and newer are using I2c '1' bus and
// the bcm2835 library will not need to be modified to work correctly.
/**********************************************************************/

/**********************************************************************/
// revision notes
/**********************************************************************/
// Original code: 5/1/2013
// Added tmp112 function : 5/30/13
// Added external temperature sensor: 6/21/13

// Updated particle counter routine: 7/21/13

// Updated with new demo-board: 9/23/13
//	- Moved Particle Counter to channel 2
//	- Moved Pressure Sensor to channel 1

// Added MPU6000 accelerometer/gyroscope routine: 9/27/13

// Added HMC5883L magnetometer and DS3231 rtc routines: 10/1/13

// Replaced Delay function for ADC conversion time with a dynamic read
// function, which reads the mpu6000 and hmc5883 for 150 ms: 10/3/13

// Corrected code for calculating Relative Humidity: 10/13/13

// Modified code with pthread to create paralled processes with a timer
// and a scheduler: 10/21/13

// Included a function to read the battery voltage and initiate a safe
// shutdown in the event that the LIPO battery voltage drops below 6.2 V
// per cell. Also included a function to turn on the buzzer when the
// package gets near the ground on descent: 11/8/2013

// Added Code to Read NO2, O3, and VOC sensors. Also streamlined code by
// creating a generic function to set and read ADC channels. Eliminated
// one "set channel" phase in the scheduler by setting the ADC channels
// for the next channel immediately after reading the current channel.
// This eliminated 0.2 seconds per channel read. Also added a routine to
// open a serial UART channel and read TX. Added a posix thread to read
// data from the CO2 sensor and put it into a separate file: 4/13/2014

// Added code to allow user to initiate shutdown by reinserting the 
// mission pull pin. Also added code to generate an events file to store
// information about errors, which event caused a shutdown, etc: 4/18/14
/**********************************************************************/

/**********************************************************************/
// includes
/**********************************************************************/

#include <bcm2835.h> 	// version 1.25 or newer compile with -l bcm2835
#include <stdio.h>
#include <math.h> 		// compile with -l m
#include <pthread.h> 	// compile with -l pthread
#include <time.h> 		// for nanosleep
#include <semaphore.h> 	// compile with -l pthread
#include <sys/mman.h>  // necessary for real time priority
#include <string.h>     // for memset
#include <unistd.h>     // for real time priority and UART
#include <fcntl.h>		// for UART
#include <termios.h>	// for UART

/**********************************************************************/
// definitions and constants
/**********************************************************************/
#define PIN_7 			RPI_GPIO_P1_07		// assign bcm pin GPIO 04
#define STATUS_LED 		RPI_GPIO_P1_11		// assign bcm pin GPIO 17
#define BUTTON 			RPI_GPIO_P1_12		// assign bcm pin GPIO 18
#define EXTERNAL_LED 	RPI_GPIO_P1_13		// assign bcm pin GPIO 21
#define PULL_PIN 		RPI_GPIO_P1_15		// assign bcm pin GPIO 22
#define DUST_LED 		RPI_GPIO_P1_16		// assign bcm pin GPIO 23
#define BUZZER 			RPI_GPIO_P1_18		// assign bcm pin GPIO 24
#define PIN_22 			RPI_GPIO_P1_22		// assign bcm pin GPIO 25
#define PIN_19 			RPI_GPIO_P1_19		// assign bcm pin GPIO
// normally SPI MOSI

#define PIN_21 			RPI_GPIO_P1_21		// assign bcm pin GPIO
// normally SPI MISO

#define PIN_23 			RPI_GPIO_P1_23		// assign bcm pin GPIO
// normally SPI CLK

#define PIN_24 			RPI_GPIO_P1_24		// assign bcm pin GPIO
// normally SPI CEO

#define PIN_26 			RPI_GPIO_P1_26		// assign bcm pin GPIO
// normally SPI CE1

/**********************************************************************/
// global variables
/**********************************************************************/
unsigned char LTC2495 	= 0x14;	// LTC2495 slave address
unsigned char HIH5030	= 0x14;	// HIH5030 same slave address as 2495
unsigned char MPXM2102A = 0x14; // MPXM2102A same slave address as 2495
unsigned char TMP112	= 0x48; // TMP112 slave address
unsigned char TMP112_X	= 0x49; // TMP112 external sensor slave address
unsigned char DS3231    = 0x68; // DS3231 RTC slave address
unsigned char MPU6000   = 0x69; // MPU6000 accel/gyro slave address
unsigned char HMC5883	= 0x1e; // HMC5883 magnetometer slave address
unsigned char UART_MOD 	= 0x35; // UART to I2C Converter (GPS)

unsigned char tmp_ch0[] = {0xa0, 0xc0};
// 0xa0c0 selects the adc on-board temp sensor configuration
unsigned char chan_0[]  = {0xb0, 0x80};
// 0xb080 selects adc channel 0, single ended input, +IN, gain X 1
unsigned char chan_1[]  = {0xb8, 0x80};
// 0xb880 selects adc channel 1, single ended input, +IN, gain X 1
unsigned char chan_2[]  = {0xb1, 0x80};
// 0xb180 selects adc channel 2, single ended input, +IN, gain X 1
unsigned char chan_3[]	= {0xb9, 0x80};
// 0xb980 selects adc channel 3, single ended input, +IN, gain X 1
unsigned char chan_4[]	= {0xb2, 0x80};
// 0xb280 selects adc channel 4, single ended input, +IN, gain X 1
unsigned char chan_5[]	= {0xba, 0x80};
// 0xba80 selects adc channel 5, single ended input, +IN, gain X 1
unsigned char chan_6[] = {0xb3, 0x80};
// 0xb380 selects adc channel 6, single ended input, +IN, gain X 1
unsigned char chan_7[] = {0xbb, 0x80};
// 0xbb80 selects adc channel 7, single ended input, +IN, gain X 1
unsigned char chan_8[] = {0xb4, 0x80};
// 0xb480 selects adc channel 8, single ended input, +IN, gain X 1
unsigned char chan_9[] = {0xbc, 0x80};
// 0xbc80 selects adc channel 9, single ended input, +IN, gain X 1
unsigned char chan_10[] = {0xb5, 0x80};
// 0xb580 selects adc channel 10, single ended input, +IN, gain X 1
unsigned char chan_11[] = {0xbd, 0x80};
// 0xbd80 selects adc channel 11, single ended input, +IN, gain X 1
unsigned char chan_12[] = {0xb6, 0x80};
// 0xb680 selects adc channel 12, single ended input, +IN, gain X 1
unsigned char chan_13[] = {0xbe, 0x80};
// 0xbe80 selects adc channel 13, single ended input, +IN, gain X 1
unsigned char chan_14[] = {0xb7, 0x80};
// 0xb780 selects adc channel 14, single ended input, +IN, gain X 1
unsigned char chan_15[] = {0xbf, 0x80};
// 0xbf80 selects adc channel 15, single ended input, +IN, gain X 1

float h_temp_comp; 	 
// temp from LTC2495 for calibrating humidity sensor
float h_x_temp_comp; 
// temp from external tmp112 for calibrating the humidity sensor

// Global File Variables
unsigned int file_num = 0; // for counting number of file names
char ENV_PREFIX[] = "/home/pi/env_";
char DYN_PREFIX[] = "/home/pi/dyn_";
char GPS_PREFIX[] = "/home/pi/gps_";
char GAS_PREFIX[] = "/home/pi/gas_";
char CO2_PREFIX[] = "/home/pi/co2_";
char EVENT_PREFIX[] = "/home/pi/event_";
char POSTFIX[] = ".csv";
char EVENT_POSTFIX[] = ".txt";

// max characters in linux file name is 255
char file_name_environmental_data[255];  
char file_name_flight_dynamics_data[255];
char file_name_gps_data[255];
char file_name_gas_data[255];
char file_name_co2_data[255];
char file_name_event_data[255];

// Global File pointers
FILE *env_fp;	// file pointer for environmental data
FILE *dyn_fp;	// file pointer for flight dynamics data
FILE *gps_fp;	// file pointer for GPS data
FILE *gas_fp;	// file pointer for gas data
FILE *co2_fp;	// file pointer for co2 sensor data
FILE *event_fp;	// file pointer for error and event data
int uart0_filestream = -1; // variable for UART 0 file stream

// Global semaphore and pthread variables
sem_t sem;				// semaphore for scheduling threads
int pshared;			// shared between the threads of a process
int ret;				// return value for semaphore
unsigned int value;		// initial value for semaphore 

// Global mission time variables
double mission_time = 0;
int mission_timer = 0;
int time_up  = 14400; // Ends mission after 14400 seconds 4.0 hours
				// Current batter is 1100mAh
				// RPi draws 190mA continuously
				// 1100mAh/190ma = 5.7 hours
int save_time = 30000;	// 3000 milliseconds = 5 minutes
			// closes data-log file and reopens to save data
unsigned int button_status = 1;
unsigned int pin_status = 0;
unsigned int shut_dwn_evnt = 0;
unsigned int led_status;
unsigned int buzz_flag = 0;
float battery_voltage;
unsigned int batt_count_down = 60; 
// 60 seconds to determine if battery is critically low

float sens_volt;
double altitude = 0; // Pressure derived altitude in [meters]
double pressure; // Measured Pressure in [torr]
double bias_pressure; // Weather corrected pressure in [torr]

// GPS variables

unsigned char read_buf[1];
unsigned char write_buf[2];
unsigned int rx_ready, over_run, parity_error, framing_error, rx_break, thr_empty, thr_tsr_empty, data_error;

unsigned char LCR = 0x18;	// Line Control Register
unsigned char RHR = 0x00;	// Receive Holding Register when LCR[7] = 0
unsigned char THR = 0x00; 	// Transmit Holding Register when LCR[7] = 0
unsigned char LSR = 0x28;	// Line Status Register when LCR != 0xBF
unsigned char SFR = 0x30; 	// Special Function Register when LCR != 0xBF

/**********************************************************************/
// prototypes
/**********************************************************************/

void read_tmp112(FILE *);
float read_tmp112_ext(FILE *);
void read_ds3231(FILE *);
void read_mpu6000(FILE *);
void read_hmc5883(FILE *);
void read_dynamics(FILE *);
void init_gps(void);
void read_gps(FILE *);

// Set LTC2495 ADC Channels
void set_channel(unsigned char gen_arr[]);

// Clear LTC2495 ADC Channel
void empty_current_channel(void);

// Read LTC2495 ADC Channels
float read_adc(FILE *, unsigned char);
float read_channel_itemp(FILE *);
double calc_pressure(FILE *, float);
void read_channel_humid(FILE *, float);
void calc_humid(FILE *, float, float);
void read_channel_prtcl(FILE *);
float calc_battery(FILE *, float);
void get_date(FILE *);

// Calculate Altitude
double calc_alt(double, double, double);

/**********************************************************************/
// POSIX threads
/**********************************************************************/

void *run_pump(void *arg)
{
	
} // end run_pump


void *schedule_timer(void *arg)
{
	int milisec = 10; // length of time to sleep, in milliseconds
	struct timespec req = {0};
	req.tv_sec = 0;
	req.tv_nsec = milisec * 1000000L; // set delay to 100 milliseconds
	
	while(1)
	{
		//nanosleep(&req, (struct timespec *)NULL); 
		// sleep 100 milliseconds
		nanosleep(&req, NULL);
		mission_time = mission_time + 0.01;
		sem_post(&sem); // make semaphore available
	} // end while loop
	
} // end schedule_timer

void *read_uart(void *arg)
{
	while(1)
	{
	//----- CHECK FOR ANY RX BYTES -----
	if (uart0_filestream != -1)
	{
		// Read up to 255 characters from the port if they are there
		unsigned char rx_buffer[256];
		int rx_length = read(uart0_filestream, (void*)rx_buffer, 255);		
		//Filestream, buffer to store in, number of bytes to read (max)
		if (rx_length < 0)
		{
			//An error occured (will occur if there are no bytes)
		}
		else if (rx_length == 0)
		{
			//No data waiting
		}
		else
		{
			//Bytes received
			rx_buffer[rx_length] = '\0';
			fprintf(co2_fp,"%s",rx_buffer);
		}
	} // end if
	
	bcm2835_delay(10);
	
	} // end while loop
	
} // end read_uart

void *schedule_tasks(void *arg)
{
	unsigned int conversion = 0;
	unsigned int dyn_time = 0;
	unsigned int counter = 0;
	unsigned int channel = 0;
	init_gps();
	
	// schedule sensor reading
	while(1)
	{
		sem_wait(&sem);	// wait for semaphore to become available
			
			
			conversion ++;
			dyn_time ++;
			if(conversion == 20) // 200ms
			{	
		    switch(counter)
		    {
				case 0 :
					counter ++;
					switch(channel)
					{
					 case 0:

						read_ds3231(env_fp);
						read_ds3231(gas_fp);	
						// read time from RTC for environmental data
						
						fprintf(gas_fp,"%.1f,", mission_time);
					    fprintf(env_fp,"%.1f,", mission_time);
						// read mission time for env and gas data
						
						read_tmp112(env_fp);	
						// read PCB board temperature
					    h_x_temp_comp = read_tmp112_ext(env_fp); 
					    // read external temperature
					    set_channel(tmp_ch0);
						// set adc to internal temp sensor
						break;
					 case 1:
						set_channel(chan_0);
						// set adc to pressure sensor channel
						break;
					 case 2:
						set_channel(chan_1);
						// set adc to humidity sensor channel	
						break;
					 case 3:
						set_channel(chan_3);
						// set adc to external humidity sensor channel
						break;
					 case 4:
						set_channel(chan_6);
						// set adc to gas temperature sensor channel
						break;
					 case 5:
						set_channel(chan_7);
						// set adc to gas humidity sensor channel						
						break;
					 case 6:
						set_channel(chan_8);
						// set adc to NO2 gas sensor channel
					    break;
					 case 7:
						set_channel(chan_9);
						// set adc to O3 gas sensor channel
					    break;
					 case 8:
						set_channel(chan_10);
						// set adc to VOC gas sensor channel
						break;
					 default:
						set_channel(chan_5);
						// set adc to Battery Voltage channel
					} // end switch
					break;
				case 1 :
					counter ++;
					empty_current_channel();	
					// clear garbage read out of adc
					break;
				case 2 :
					counter = 0;
				    switch(channel) 
				    {
					 case 0: // READ INTERNAL TEMPERATURE
						h_temp_comp = read_channel_itemp(env_fp);
						channel ++;
						set_channel(chan_0);
						break;
					 case 1: // READ PRESSURE SENSOR
						sens_volt = read_adc(env_fp, LTC2495);
						pressure = calc_pressure(env_fp, sens_volt);
						altitude = calc_alt(altitude, pressure, bias_pressure);
						fprintf(env_fp, "%.2f,", altitude);
						channel ++;
						set_channel(chan_1);
						break;
					 case 2: // READ INTERNAL HUMIDITY SENSOR
						sens_volt = read_adc(env_fp, LTC2495);
						calc_humid(env_fp, sens_volt, h_temp_comp);
						channel ++;
						set_channel(chan_3);
						break;
					 case 3: // READ EXTERNAL HUMIDITY SENSOR
						sens_volt = read_adc(env_fp, LTC2495);
						calc_humid(env_fp, sens_volt, h_x_temp_comp);
						channel ++;
						set_channel(chan_6);
						break;
					 case 4: // READ GAS TEMPERATURE SENSOR
						sens_volt = read_adc(gas_fp, LTC2495);
					    channel ++;
					    set_channel(chan_7);
					    break;
					 case 5: // READ GAS HUMIDITY SENSOR
						sens_volt = read_adc(gas_fp, LTC2495);
						channel ++;
						set_channel(chan_8);
						break;
					 case 6: // READ NO2 SENSOR
						sens_volt = read_adc(gas_fp, LTC2495);
						channel ++;
						set_channel(chan_9);
						break;
					 case 7: // READ O3 SENSOR
						sens_volt = read_adc(gas_fp, LTC2495);
						channel ++;
						set_channel(chan_10);
						break;
					 case 8: // READ VOC SENSOR
						sens_volt = read_adc(gas_fp, LTC2495);
						channel ++;
						set_channel(chan_5);
						break;
					 default: // READ BATTERY VOLTAGE
						sens_volt = read_adc(env_fp, LTC2495);
						battery_voltage = calc_battery(env_fp, sens_volt);
						fprintf(env_fp,"\n");
						fprintf(gas_fp,"\n"); 
						// newline for environmental data
						channel = 0;
						set_channel(tmp_ch0);
					}// end switch
					break;
			    default :
					break;
			         	    
			} // end switch
			conversion = 0;
			} // end if - conversion
		
		if(dyn_time==10)
		{					
		read_dynamics(dyn_fp);	// READ FLIGHT DYNAMICS SENSORS
		dyn_time=0;
		}
		
		read_gps(gps_fp); // READ GPS DATA
	
		// close and save files every 5 minutes
		if(save_time > 0)
		{
			save_time = save_time - 1;
		} // end if
		else
		{
			fclose(env_fp);	// close environmental data file
			fclose(dyn_fp);	// close dynamic flight data file
			fclose(gps_fp); // close gps file
			fclose(gas_fp); // close gas file
			fclose(co2_fp);	// close CO2 data file
						
			env_fp = fopen(file_name_environmental_data, "a");
			// file for environmental data
			dyn_fp = fopen(file_name_flight_dynamics_data, "a");
			// file for dynamic data
			gps_fp = fopen(file_name_gps_data, "a");
			// file for gps tracking data
			gas_fp = fopen(file_name_gas_data, "a");
			// file for atmospheric gas data
			co2_fp = fopen(file_name_co2_data, "a");
			// file for carbon dioxide gas data
			
			save_time = 30000; // reset save time for another 5 minutes.
		} // end else
	} // end while-loop	
} // end schedule_tasks

/**********************************************************************/
// main thread
/**********************************************************************/

int main(void)
{
	// Initialize file names for checking
	sprintf(file_name_event_data, "%s%d%s",EVENT_PREFIX,file_num,EVENT_POSTFIX);
	
	// Check if files already exists by attempting to read them
	
	// generate file name for event data
	while((event_fp = fopen(file_name_event_data, "r")) != NULL) 
	// while the file name already exists
	{
	fclose(event_fp);
	file_num ++;
	sprintf(file_name_event_data, "%s%d%s",EVENT_PREFIX,file_num,EVENT_POSTFIX);
	}// stop when a file name has been found that doesn't already exist
	
	event_fp = fopen(file_name_event_data, "a"); // file for event data
	
	file_num = 0;
	
	fprintf(event_fp,"Error and Event Data\n");
    fprintf(event_fp,"HARBOR: Weber State University\n\n");
	
	// Following code provide by Arjan to make the timing in this program
	// a little more reliable.
	//fprintf(event_fp,"Init realtime environment\n");

	if (geteuid() == 0) 
	{
		struct sched_param sp;
		memset(&sp, 0, sizeof(sp));
		sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
		sched_setscheduler(0, SCHED_FIFO, &sp);
		mlockall(MCL_CURRENT | MCL_FUTURE);
		fprintf(stderr, "Running with realtime priority!\n");
		fprintf(event_fp, "Running with realtime priority!\n");
   }
	else
	{
		fprintf(stderr, "Not running with realtime priority.\n");
		fprintf(event_fp, "Not running with realtime priority.\n");
	}
	
		//-------------------------
	//----- SETUP USART 0 -----
	//-------------------------
	//At bootup, pins 8 and 10 are already set to UART0_TXD,
	// UART0_RXD (ie the alt0 function) respectively
	
	
	//OPEN THE UART
	//The flags (defined in fcntl.h):
	//	Access modes (use 1 of these):
	//		O_RDONLY - Open for reading only.
	//		O_RDWR - Open for reading and writing.
	//		O_WRONLY - Open for writing only.
	//
	//	O_NDELAY / O_NONBLOCK (same function) - Enables nonblocking 
	//  mode. When set read requests on the file can return immediately 
	//  with a failure status if there is no input immediately available
	//  (instead of blocking). Likewise, write requests can also return
	//	immediately with a failure status if the output can't be written
	//  immediately.
	//
	//	O_NOCTTY - When set and path identifies a terminal device, 
	//  open() shall not cause the terminal device to become the 
	//  controlling terminal for the process.
	uart0_filestream = open("/dev/ttyAMA0", O_RDWR | O_NOCTTY | O_NDELAY);		
	//Open in non blocking read/write mode
	if (uart0_filestream == -1)
	{
		//ERROR - CAN'T OPEN SERIAL PORT
		fprintf(event_fp, "Error - Unable to open UART.  Ensure it is not in use by another application\n");
	}
	
	//CONFIGURE THE UART
	//The flags (defined in /usr/include/termios.h - see 
	//http://pubs.opengroup.org/onlinepubs/007908799/xsh/termios.h.html):
	//	Baud rate:- B1200, B2400, B4800, B9600, B19200, B38400, B57600, 
	//  B115200, B230400, B460800, B500000, B576000, B921600, B1000000, 
	//  B1152000, B1500000, B2000000, B2500000, B3000000, B3500000, 
	//  B4000000
	//	CSIZE:- CS5, CS6, CS7, CS8
	//	CLOCAL - Ignore modem status lines
	//	CREAD - Enable receiver
	//	IGNPAR = Ignore characters with parity errors
	//	ICRNL - Map CR to NL on input (Use for ASCII comms where you 
	//  want to auto correct end of line characters - don't use for 
	//  bianry comms!)
	//	PARENB - Parity enable
	//	PARODD - Odd parity (else even)
	struct termios options;
	tcgetattr(uart0_filestream, &options);
	options.c_cflag = B9600 | CS8 | CLOCAL | CREAD;		//<Set baud rate
	options.c_iflag = IGNPAR;
	options.c_oflag = 0;
	options.c_lflag = 0;
	tcflush(uart0_filestream, TCIFLUSH);
	tcsetattr(uart0_filestream, TCSANOW, &options);
	
	// declare variables
	
	pthread_t clk_thread; // thread to create .1 second reads
	pthread_t sch_thread; // thread with data logging schedule
	pthread_t read_uart_thread; // thread to read uart
	
	//int second = 1000; // length of time to sleep, in milliseconds
	//int tenthsecond = 100; // 100 milliseconds
	//int millisecond = 1; //	1 milliseconds
	struct timespec req = {0};
	req.tv_sec = 1;
	req.tv_nsec = 0;
	
	unsigned char i;
	
	// If you call this, it will not actually access the GPIO
	// Use for testing
	// bcm2835_set_debug(1);

	if (!bcm2835_init())
	return 1;
	bcm2835_i2c_begin();	// The default
	bcm2835_i2c_setClockDivider(BCM2835_I2C_CLOCK_DIVIDER_626);
		// Divides the 250Mhz clock
		// to a 400Khz Clock
		// The max speed for i2c sensors
		// is 400Khz
	bcm2835_gpio_fsel(STATUS_LED, BCM2835_GPIO_FSEL_OUTP); 	
	// On-Board Status LED
	bcm2835_gpio_fsel(EXTERNAL_LED, BCM2835_GPIO_FSEL_OUTP);
	// External Status LED
	bcm2835_gpio_fsel(DUST_LED, BCM2835_GPIO_FSEL_OUTP);	
	// Dust Sensor LED
	bcm2835_gpio_fsel(BUTTON, BCM2835_GPIO_FSEL_INPT); 		
	// Stop Button
	bcm2835_gpio_fsel(PULL_PIN, BCM2835_GPIO_FSEL_INPT); 	
	// Pull-Pin
	bcm2835_gpio_fsel(BUZZER, BCM2835_GPIO_FSEL_OUTP);		
	// Buzzer

	// turn on status LED
	bcm2835_gpio_write(STATUS_LED, LOW);
	bcm2835_gpio_write(EXTERNAL_LED, LOW);
	
	// Initialize file names for checking
	sprintf(file_name_environmental_data, "%s%d%s",ENV_PREFIX,file_num,POSTFIX);
	sprintf(file_name_flight_dynamics_data, "%s%d%s",DYN_PREFIX,file_num,POSTFIX);
	sprintf(file_name_gps_data, "%s%d%s",GPS_PREFIX,file_num,POSTFIX);
	sprintf(file_name_gas_data, "%s%d%s",GAS_PREFIX,file_num,POSTFIX);
	sprintf(file_name_co2_data, "%s%d%s",CO2_PREFIX,file_num,POSTFIX);

	
	// Check if files already exists by attempting to read them
	
	// generate file name for environmental data
	while((env_fp = fopen(file_name_environmental_data, "r")) != NULL) 
	// while the file name already exists
	{
	fclose(env_fp);
	file_num ++;
	sprintf(file_name_environmental_data, "%s%d%s",ENV_PREFIX,file_num,POSTFIX);
	}// stop when a file name has been found that doesn't already exist

	env_fp = fopen(file_name_environmental_data, "a"); // file for environmental data
	
	file_num = 0;
	
	// generate file name for flight dynamics data
	while((dyn_fp = fopen(file_name_flight_dynamics_data, "r")) != NULL) 
	// while the file name already exists
	{
	fclose(dyn_fp);
	file_num ++;
	sprintf(file_name_flight_dynamics_data, "%s%d%s",DYN_PREFIX,file_num,POSTFIX);
	}// stop when a file name has been found that doesn't already exist

	dyn_fp = fopen(file_name_flight_dynamics_data, "a"); 
	// file for dynamics data
	
	file_num = 0;
	
	// generate file name for gps data
	while((gps_fp = fopen(file_name_gps_data, "r")) != NULL) 
	// while the file name already exists
	{
	fclose(gps_fp);
	file_num ++;
	sprintf(file_name_gps_data, "%s%d%s",GPS_PREFIX,file_num,POSTFIX);
	}// stop when a file name has been found that doesn't already exist

	gps_fp = fopen(file_name_gps_data, "a"); // file for gps data
	
	file_num = 0;
	
	// generate file name for co2 data
	while((co2_fp = fopen(file_name_co2_data, "r")) != NULL) 
	// while the file name already exists
	{
	fclose(co2_fp);
	file_num ++;
	sprintf(file_name_co2_data, "%s%d%s",CO2_PREFIX,file_num,POSTFIX);
	}// stop when a file name has been found that doesn't already exist

	co2_fp = fopen(file_name_co2_data, "a"); 
	// file for co2 data
	
	file_num = 0;
	
	// generate file name for gas data
	while((gas_fp = fopen(file_name_gas_data, "r")) != NULL) 
	// while the file name already exists
	{
	fclose(gas_fp);
	file_num ++;
	sprintf(file_name_gas_data, "%s%d%s",GAS_PREFIX,file_num,POSTFIX);
	}// stop when a file name has been found that doesn't already exist

	gas_fp = fopen(file_name_gas_data, "a"); // file for gas data
	// file for gas data
	
	file_num = 0;
	
	
	// insert current date header at the top of all data files
	get_date(env_fp);
	get_date(dyn_fp);
	get_date(gps_fp);
	get_date(gas_fp);

	// install data headers at the top of all data files
	fprintf(env_fp,"Environmental Data\n");
    fprintf(env_fp,"HARBOR: Weber State University\n");	
    // Quickly sample air pressure to remove weather effect at ground level
    set_channel(chan_0);
    bcm2835_delay(200);
    empty_current_channel();
    bcm2835_delay(200);
    sens_volt = read_adc(env_fp, LTC2495);
    fprintf(env_fp,"\n");
	pressure = calc_pressure(env_fp, sens_volt);
	fprintf(env_fp,"\n");
    bias_pressure = pressure - 612.8; // 612.8 torr should be equivalent to
    // 1778 meters above sea level (the elevation of Duchesne Airport)
    fprintf(env_fp,"Bias Pressure at Duchesne Airport 1778 meters (ASL): %.2f \n", bias_pressure);
	fprintf(env_fp,"time,mission_time,reg_temp,xtemp,itemp,ipressure_raw,pressure,altitude,ihumidity_raw,ihumidity,xhumidity_raw,xhumidity,batt_voltage_raw,batt_voltage,\n"); 

	fprintf(dyn_fp,"Flight Dynamics Data\n");
    fprintf(dyn_fp,"HARBOR: Weber State University\n");
	fprintf(dyn_fp,"time,mission_time,accel_x_2g,accel_y_2g,accel_z_2g,gyro_x_250,gyro_y_250,gyro_z_250,accel_x_8g,accel_y_8g,accel_z_8g,gyro_x_1000,gyro_y_1000,gyro_z_1000,mag_x,mag_y,mag_z,\n");
    
    fprintf(gas_fp,"Gas Sensor Data\n");
    fprintf(gas_fp,"HARBOR: Weber State University\n");
	fprintf(gas_fp,"time,mission_time, temperature, humidity, nitrate, ozone , VOC,\n");

	fprintf(co2_fp,"CO2 Sensor Data\n");
    fprintf(co2_fp,"HARBOR: Weber State University\n");
	fprintf(co2_fp,"Collected from COZIR CO2 sensor\n");
	fprintf(co2_fp,"time, CO2, temperature, humidity,\n");
	
    fprintf(event_fp,"Mission Pin Pulled at: ");


	while(pin_status==0) // wait for mission pin to be pulled
	{
		pin_status = bcm2835_gpio_lev(PULL_PIN);
		
		if(pin_status == 1) 
		// Debounce -- wait half a second and then check the pin status
		// again to make sure it has been pulled for real
		{
			bcm2835_delay(500);
			pin_status = bcm2835_gpio_lev(PULL_PIN);
		} // end if
		
		bcm2835_gpio_write(STATUS_LED, LOW);
		bcm2835_gpio_write(EXTERNAL_LED, LOW);
		bcm2835_delay(50);
		bcm2835_gpio_write(STATUS_LED, HIGH);
		bcm2835_gpio_write(EXTERNAL_LED, HIGH);
		bcm2835_delay(500);
	} // end while-loop

	// acknowledge pin was pulled
	for(i=0; i<10; i++)
	{
		bcm2835_gpio_write(STATUS_LED, LOW);
		bcm2835_gpio_write(EXTERNAL_LED, LOW);
		bcm2835_gpio_write(BUZZER, HIGH);
		bcm2835_delay(20);
		bcm2835_gpio_write(STATUS_LED, HIGH);
		bcm2835_gpio_write(EXTERNAL_LED, HIGH);
		bcm2835_gpio_write(BUZZER, LOW);
		bcm2835_delay(100);
	}// end for loop	
	
	read_ds3231(event_fp); // time that mission pin was pulled
	fprintf(event_fp,"\n\n");
	
	
	// initialize POSIX threads and semaphores
	pshared = 0;	// initial value of 0 -> share between threads
	value 	= 1;	// initial value of 1 -> do schedule
	sem_init(&sem,pshared,value); // initialize semaphore
	
	// start 100 millisecond task timer
	pthread_create(&clk_thread, NULL, &schedule_timer, NULL);
	
	// start data logging scheduler
	pthread_create(&sch_thread, NULL, &schedule_tasks, NULL);
	
	 // start UART channel
 pthread_create(&read_uart_thread, NULL, &read_uart, NULL);
	
	system("/opt/vc/bin/tvservice -off");  
	// shut off PAL/HDMI outputs to save power
	system("sh -c \"echo 1 > /sys/devices/platform/bcm2708_usb/bussuspend\" "); 
	// shut off USB chip to save power
		
	// **** START MAIN LOOP ****
	while((shut_dwn_evnt == 0)) 
	// while stop button not pressed and max time not exceeded
	{
	button_status = bcm2835_gpio_lev(BUTTON);
	pin_status = bcm2835_gpio_lev(PULL_PIN);
	led_status = bcm2835_gpio_lev(STATUS_LED);
	
	// MONITOR SHUTDOWN BUTTON
	if(button_status == 0) 
		// Debounce -- wait half a second and then check the button status
		// again to make sure it has been pressed for real
		{
			bcm2835_delay(500);
			button_status = bcm2835_gpio_lev(BUTTON);
			
			if(button_status == 0)
				{
					shut_dwn_evnt = 1;
					fprintf(event_fp, "Shutdown button pressed.\n");
				}
		} // end if
	
	// MONITOR PULL PIN
	if(pin_status == 0)
		// Debounce -- wait half a second and then check the pin status
		// again to make sure it has been reinserted for real
		{
			bcm2835_delay(500);
			pin_status = bcm2835_gpio_lev(PULL_PIN);
			
			if(pin_status == 0)
				{
					shut_dwn_evnt = 1;
					fprintf(event_fp, "Pull pin reinserted.\n");
				}
		} // end if
		
	// MONITOR BATTERY VOLTAGE
	if(battery_voltage < 6.4) // 6.4V or 3.2 V per Cell;
	{
		batt_count_down --;
	} // end if
	else
	{
		batt_count_down = 60; 
		// only count 1 consecutive minute of low voltage
	}
	
	if(batt_count_down == 0) // Battery is critically low; SHUTDOWN!!!
	{
		shut_dwn_evnt = 1; // Initiate shutdown
		fprintf(event_fp, "Battery voltage critically low!!!\n");
	} // end if
	
	// INVERT STATUS LED
	if(led_status == 0)
	{
		bcm2835_gpio_write(STATUS_LED, HIGH);	// turn off led
		bcm2835_gpio_write(EXTERNAL_LED, HIGH);	// turn off led
	} // end if
	else
	{
		bcm2835_gpio_write(STATUS_LED, LOW); 	// turn on led
		bcm2835_gpio_write(EXTERNAL_LED, LOW); 	// turn on led
	} // end else

	// sleep 1 second
	nanosleep(&req, NULL);
	
	mission_timer ++; // increment mission timer in one second intervals
	
	if(mission_timer > time_up)
	{
		shut_dwn_evnt = 1;
		fprintf(event_fp, "Mission timer expired!!!\n");
	}// end if

	} // **** END MAIN LOOP ****


	fprintf(event_fp, "Initiating Shutdown!\n");
	fprintf(event_fp, "Mission Elapsed Time: %.1f seconds\n", mission_time);
	// acknowledge shutdown initiated
	for(i=0; i<10; i++)
	{
	bcm2835_gpio_write(STATUS_LED, LOW);
	bcm2835_gpio_write(EXTERNAL_LED, LOW);
	bcm2835_gpio_write(BUZZER, HIGH);
	bcm2835_delay(20);
	bcm2835_gpio_write(STATUS_LED, HIGH);
	bcm2835_gpio_write(EXTERNAL_LED, HIGH);
	bcm2835_gpio_write(BUZZER, LOW);
	bcm2835_delay(100);
	}// end for-loop

	fprintf(event_fp, "Closing files\n");
	fclose(env_fp); 	// close environmental data file
	fclose(dyn_fp);		// close dynamics data file
	fclose(gps_fp);		// close gps data file
	fclose(gas_fp);		// close gas data file
	fclose(co2_fp);		// close dynamics data file

	
	fprintf(event_fp, "Resetting BCM2835 ports\n");
	bcm2835_i2c_end(); 	// close i2c port
	bcm2835_close();	// close all other bcm2835 ports

	fprintf(event_fp, "Closing UART filestream\n");
  	//----- CLOSE THE UART -----
	close(uart0_filestream);

	fprintf(event_fp, "Done\n");
	fclose(event_fp);	// close event data file
	system("shutdown -h -P now");  // shutdown the pi now

	return 0; // end msa_v4_0
} // end main


/**********************************************************************/
// function definitions
/**********************************************************************/

// The LTC2495 has an average conversion time of 165ms in the 1x speed mode.
// minimum conversion time 144 mS and typical is 150 mS

void set_channel(unsigned char gen_arr[])
{
	bcm2835_i2c_setSlaveAddress(LTC2495);
	bcm2835_i2c_write(gen_arr, sizeof(gen_arr));
	// set ADC to read selected channel
} // end set_channel


void empty_current_channel(void)
{
	unsigned char junk2[3];
	bcm2835_i2c_setSlaveAddress(LTC2495);
	bcm2835_i2c_read(junk2, sizeof(junk2)); // junk data

}// end empty_current_channel

float read_channel_itemp(FILE *gen_fp)
{
	bcm2835_i2c_setSlaveAddress(LTC2495);

	// on-board temperature sensor variables
	int temp,sign;  // DATAOUT16 HEX Value for Temperature
	float temp2;	// Kelvin
	//float tslope = 3.712121212;// slope = VRef(+)/12.25 per datasheet
	float tslope = 0.269387755;  // slope = VRef(+)/12.25 per datasheet
							   // VRef(+) = 3.3 Volts
	float temp3;	// Celsius
	float temp4;	// Fahrenheit

	unsigned char t_buf[3];	// buffers for capturing the temperature
/**********************************************************************/
// read on-board temperature sensor
/**********************************************************************/

	bcm2835_i2c_read(t_buf, sizeof(t_buf));
	// read the data register from the LTC2495
	sign = t_buf[0] & 0x40;	// capture sign bit
	t_buf[0] = t_buf[0] & 0x7f;  
	// mask the sign bit for number conversion
	// Concatenate the two temp bytes into a single 16bit value
	temp = (t_buf[0]*0x10000) + (t_buf[1]*0x100) + (t_buf[2]);
	// write temperature data in hex
    temp = temp/0x40; // right justify data bits
	temp2 = ((float)temp*tslope); 
	// Convert the 16-bit hex value into Kelvin
	temp3 = temp2 - 273.0; 
	// Convert the Kelvin value into degrees Celsius
	if(sign == 0x40)
	{
		temp3 = temp3*(-1);
	} // end if statement

	fprintf(gen_fp, "%.2f,",temp3);

	return temp3;

}// end read_channel_itemp


double calc_pressure(FILE *gen_fp, float v_press)
{
	
	// pressure sensor variables
	double c_press;  
	// Unit calibrated value [Torr]  
	double log_press;
	// variable used when sensor is operating in logarithmic region
 
	if(v_press > 0.319) // sensor operating in linear region above 150 torr
	{
	c_press = 500 * v_press + 37.75; 
	// convert sensor voltage to units [Torr]
	}
	else
	{
	log_press = (v_press-0.3089)/0.002;
	c_press = exp(log_press);
	}
	fprintf(gen_fp, "%.2f,",c_press);
	
	return c_press;

}// end calc_pressure


void calc_humid(FILE *gen_fp, float v_humid, float temp_comp)
{

	// humidity sensor variables
	float humidx2;	// Relative Humidity (1st Order Curve)
	float t_humid_x;	// temperature corrected humidity

	// V(out)=V(source)[(0.00636)(RH)+0.1515]
	// V(out) is being divided so that max doesn't exceed ADC input range
	// The datasheet doesn't provide a realistic value compared to measurement
	// Thus:
	// V(out) = 80.806*V(out-to adc)-22.168
	// This accounts for the min (0.274 V) and max (1.51 V) for this sensor
	// as seen by the adc due to the 68K 56K voltage divider.
	humidx2 = (80.806*(v_humid) - 22.168); 	// convert humidty to RH %
	// The following is suggested by the data sheet
	t_humid_x = humidx2/(1.0546-.00216*temp_comp); // correct for temperature
	
	fprintf(gen_fp, "%.2f,", t_humid_x);  
	// save humidty value in file

}// end read_channel_humid


float calc_battery(FILE *gen_fp, float v_batt)
{

	float  a_batt;	// adjusted battery voltage

	// The battery voltage is divided down to meet the ADC input requirements using a
	// 0.175438596 ratio voltage divider with a 1M ohm and 4.7M ohm resistor pair
	a_batt = v_batt / 0.175; 
	// re-scale voltage back up to actual battery volage
	
	fprintf(gen_fp, "%.2f,",a_batt);	
	// log battery value (voltage) in datalog file
	
	return a_batt;
	
} // end calc_battery

void read_tmp112(FILE *gen_fp)
{

	bcm2835_i2c_setSlaveAddress(TMP112);

	int reg_temp,sign,i;
	int conv_time = 35; // conversion time for tmp112 is 35 mS max
	float reg_temp2;
	float reg_temp3;
	float reg_temp4;
	unsigned char bufreg_temp[] = {0x00, 0x00};

	for(i=0; i<2; i++)
	{
	bcm2835_i2c_read(bufreg_temp, sizeof(bufreg_temp));
	sign = bufreg_temp[0] & 0x40;	// capture sign bit
	if(sign == 0x40)
	{
	bufreg_temp[0] = ~bufreg_temp[0];
	bufreg_temp[1] = ~bufreg_temp[1];
	reg_temp = (bufreg_temp[1]) + (bufreg_temp[0]*0x100);
	reg_temp2 = (float)reg_temp / (float)0x10; // left justify 12 bits
	reg_temp2 = reg_temp2 + 1;	// two's compliment
	reg_temp3 = reg_temp2 * (-1) * 0.0625; 	// convert to celsius
	} // end if
	else
	{
	reg_temp = (bufreg_temp[1]) + (bufreg_temp[0]*0x100);
	reg_temp2 = (float)reg_temp / (float)0x10; // left justify 12 bits
	reg_temp3 = reg_temp2 * 0.0625;	// convert to celsius
	} // end else

	bcm2835_delay(conv_time);
	} // end for loop

	fprintf(gen_fp, "%.2f,",reg_temp3); 
	// save external temperature in data file

} // end read_tmp112

float read_tmp112_ext(FILE *gen_fp)
{

	bcm2835_i2c_setSlaveAddress(TMP112_X);

	int ext_temp,x_sign,x_i;
	int x_conv_time = 35; // conversion time for tmp112 is 35 mS max
	float ext_temp2;
	float ext_temp3;
	float ext_temp4;
	unsigned char bufext_temp[] = {0x00, 0x00};

	for(x_i=0; x_i<2; x_i++)
	{
	bcm2835_i2c_read(bufext_temp, sizeof(bufext_temp));
	x_sign = bufext_temp[0] & 0x40;	// capture sign bit
	if(x_sign == 0x40)
	{
	bufext_temp[0] = ~bufext_temp[0];
	bufext_temp[1] = ~bufext_temp[1];
	ext_temp = (bufext_temp[1]) + (bufext_temp[0]*0x100);
	ext_temp2 = (float)ext_temp / (float)0x10; // left justify 12 bits
	ext_temp2 = ext_temp2 + 1;	// two's compliment
	ext_temp3 = ext_temp2 * (-1) * 0.0625; 	// convert to celsius
	} // end if
	else
	{
	ext_temp = (bufext_temp[1]) + (bufext_temp[0]*0x100);
	ext_temp2 = (float)ext_temp / (float)0x10; // left justify 12 bits
	ext_temp3 = ext_temp2 * 0.0625;	// convert to celsius
	} // end else

	bcm2835_delay(x_conv_time);
	} // end for loop

	fprintf(gen_fp, "%.2f,",ext_temp3); 
	// save external temperature in data file
	
	return ext_temp3;

} // end read_tmp112


void read_mpu6000(FILE *gen_fp)
{
	bcm2835_i2c_setSlaveAddress(MPU6000);

	// Data Registers on MPU6000 chip

	unsigned char accel_xout_high[]	= {0x3b};
	// x-axis accelerometer measurement [15:8] register
	unsigned char accel_xout_low[]	= {0x3c};
	// x-axis accelerometer measurement [7:0] register
	unsigned char accel_yout_high[]	= {0x3d};
	// y-axis accelerometer measurement [15:8] register
	unsigned char accel_yout_low[]	= {0x3e};
	// y-axis accelerometer measurement [7:0] register
	unsigned char accel_zout_high[]	= {0x3f};
	// z-axis accelerometer measurement [15:8] register
	unsigned char accel_zout_low[]	= {0x40};
	// z-axis accelerometer measurement [7:0] register

	unsigned char gyro_xout_high[]	= {0x43}; 
	// x-axis gyroscope measurement [15:8] register
	unsigned char gyro_xout_low[]	= {0x44}; 
	// x-axis gyroscope measurement [7:0] register
	unsigned char gyro_yout_high[]	= {0x45}; 
	// y-axis gyroscope measurement [15:8] register
	unsigned char gyro_yout_low[]	= {0x46}; 
	// y_axis gyroscope measurement [7:0] register
	unsigned char gyro_zout_high[]	= {0x47}; 
	// z-axis gyroscope measurement [15:8] register
	unsigned char gyro_zout_low[]	= {0x48}; 
	// z-axis gyroscope measurement [7:0] register

	unsigned char temp_out_high[]	= {0x41}; 
	// temperature measurement [15:8] register
	unsigned char temp_out_low[]	= {0x42}; 
	// temperature measurement [7:0] register

	// Commands

	unsigned char wake_up[]		= {0x6b, 0x03};	
	// command to restart power management and use z_gyro reference clock
	unsigned char flscl_2g[]	= {0x1c, 0x00}; 
	// command to set accelerometer to +/- 2g full scale
	unsigned char flscl_8g[]	= {0x1c, 0x10}; 
	// command to set accelerometer to +/- 8g full scale
	unsigned char flscl_250[]	= {0x1b, 0x00}; 
	// command to set gyrometer to +/- 250 d/s full scale
	unsigned char flscl_1000[]	= {0x1b, 0x10}; 
	// command set gyrometer to +/- 1000 d/s full scale

	// Data buffers for reading MPU6000 chip

	unsigned char accel_data[6];
	unsigned char gyro_data[6];
	unsigned char temp_data[2];

	unsigned int accel_x_uint32, accel_y_uint32, accel_z_uint32;
	unsigned int gyro_x_uint32, gyro_y_uint32, gyro_z_uint32;
	float accel_x_flt, accel_y_flt, accel_z_flt;
	float gyro_x_flt, gyro_y_flt, gyro_z_flt;

	signed int temp_int;
	float temp_flt;
	int q;


	// restart the mpu6000 and set CLKSEL to 3 to use Z axis gyroscope reference
	bcm2835_i2c_write(wake_up, sizeof(wake_up));

	// set accelerometer configuration to +/- 2g
	bcm2835_i2c_write(flscl_2g, sizeof(flscl_2g));

	// set gyroscope configuration to +/- 250 d/s
	bcm2835_i2c_write(flscl_250, sizeof(flscl_250));

	float acc_sens = 16384;
	float gyr_sens = 131;

	for(q=0;q<2;q++)
	{

	// burst-read accelerometer data starting at 0x3b and ending at 0x40
	bcm2835_i2c_read_register_rs(accel_xout_high, accel_data, sizeof(accel_data));

	// burst-read gyroscope data starting at 0x43 and ending at 0x48
	bcm2835_i2c_read_register_rs(gyro_xout_high, gyro_data, sizeof(gyro_data));

	// burst-read temperature data starting at 0x41 and ending at 0x42
	bcm2835_i2c_read_register_rs(temp_out_high, temp_data, sizeof(temp_data));

	// compute acclerometer reading 16 bit two's compliment values our of register
	accel_x_uint32 = ((unsigned int)accel_data[0] << 8) | (accel_data[1]);
	if (accel_x_uint32 >= 32768)
		accel_x_uint32 -= 65536;
	accel_x_flt = (int)accel_x_uint32 / acc_sens;

	accel_y_uint32 = ((unsigned int)accel_data[2] << 8) | (accel_data[3]);
	if (accel_y_uint32 >= 32768)
		accel_y_uint32 -= 65536;
	accel_y_flt = (int)accel_y_uint32 / acc_sens;

	accel_z_uint32 = ((unsigned int)accel_data[4] << 8) | (accel_data[5]);
	if (accel_z_uint32 >= 32768)
		accel_z_uint32 -= 65536;
	accel_z_flt = (int)accel_z_uint32 / acc_sens;

	// compute gyrometer reading
	gyro_x_uint32 = ((unsigned int)gyro_data[0] << 8) | (gyro_data[1]);
	if (gyro_x_uint32 >= 32768)
		gyro_x_uint32 -= 65536;
	gyro_x_flt = (int)gyro_x_uint32 / gyr_sens;

	gyro_y_uint32 = ((unsigned int)gyro_data[2] << 8) | (gyro_data[3]);
	if (gyro_y_uint32 >= 32768)
		gyro_y_uint32 -= 65536;
	gyro_y_flt = (int)gyro_y_uint32 / gyr_sens;

	gyro_z_uint32 = ((unsigned int)gyro_data[4] << 8) | (gyro_data[5]);
	if (gyro_z_uint32 >= 32768)
		gyro_z_uint32 -= 65536;
	gyro_z_flt = (int)gyro_z_uint32 / gyr_sens;

	fprintf(gen_fp, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,", accel_x_flt, 
	accel_y_flt, accel_z_flt, gyro_x_flt, gyro_y_flt, gyro_z_flt);

	// set acceleromter configureation to +/- 8g
	bcm2835_i2c_write(flscl_8g, sizeof(flscl_8g));

	// set gyroscope configuration to +/- 1000 d/s
	bcm2835_i2c_write(flscl_1000, sizeof(flscl_1000));

	acc_sens = 4096;
	gyr_sens = 32.8;

	} // end for loop

} // end read_mpu6000

void read_hmc5883(FILE *gen_fp)
{
	bcm2835_i2c_setSlaveAddress(HMC5883);

	unsigned char mag_buf[6]; 
	// buffer to hold data from magnetometer registers 0x03 to 0x08
	unsigned char mag_reg[] = {0x03}; 
	// first addresss of magnetomter registers 0x03 to 0x08

	unsigned int mag_x_uint32,mag_y_uint32,mag_z_uint32;
	float mag_x_flt,mag_y_flt,mag_z_flt;

	float mag_sens = 390; 
	// for gain = 5 sensitivity = 390 LSb/Gauss

	// commands
	unsigned char samp_rate_8_15[] = {0x00,0x70}; 
	// 8 averaged samples, 15 Hz, normal measurement
	unsigned char gain_5[]		 = {0x01,0xa0}; 
	// set gain to 5
	unsigned char cont_mode[]	 = {0x02,0x00}; 
	// set to continuous measurement mode
	unsigned char single_mode[]	= {0x02,0x01};	
	// set to single-measurement mode

	// set sample rate
	bcm2835_i2c_write(samp_rate_8_15, sizeof(samp_rate_8_15));

	// set gain
	bcm2835_i2c_write(gain_5, sizeof(gain_5));

	// set measurement mode
	bcm2835_i2c_write(cont_mode, sizeof(single_mode));

	bcm2835_delay(10);

	// read magnetometer
	bcm2835_i2c_read_register_rs(mag_reg, mag_buf, sizeof(mag_buf));

	// compute readings
	mag_x_uint32 = ((unsigned int)mag_buf[0] << 8) | (mag_buf[1]);
	if (mag_x_uint32 >= 32768)
		mag_x_uint32 -= 65536;
	mag_x_flt = (int)mag_x_uint32 / mag_sens;

	mag_y_uint32 = ((unsigned int)mag_buf[2] << 8) | (mag_buf[3]);
	if (mag_y_uint32 >= 32768)
		mag_y_uint32 -= 65536;
	mag_y_flt = (int)mag_y_uint32 / mag_sens;

	mag_z_uint32 = ((unsigned int)mag_buf[4] << 8) | (mag_buf[5]);
	if (mag_z_uint32 >= 32768)
		mag_z_uint32 -= 65536;
	mag_z_flt = (int)mag_z_uint32 / mag_sens;

	fprintf(gen_fp,"%.2f,%.2f,%.2f,",mag_x_flt,mag_y_flt,mag_z_flt);

}// end read_hmc5883

void read_ds3231(FILE *gen_fp)
{
	bcm2835_i2c_setSlaveAddress(DS3231);

	unsigned char time_buf[3]; 
	// buffer to hold data from time registers 0x00 to 0x02
	
	unsigned char time_reg[] = {0x00}; 
	// first address of time registers 0x00 to 0x02
	
	unsigned int bcd_sec, bcd_min, bcd_hr;

	bcm2835_i2c_read_register_rs(time_reg, time_buf, sizeof(time_buf));
	
	bcd_sec = (((time_buf[0] & 0xf0)/0x10) * 10) + (time_buf[0] & 0x0f);
	bcd_min = (((time_buf[1] & 0xf0)/0x10) * 10) + (time_buf[1] & 0x0f);
	bcd_hr	= (((time_buf[2] & 0xf0)/0x10) * 10) + (time_buf[2] & 0x0f);

	fprintf(gen_fp,"%.2d:%.2d:%.2d,", bcd_hr,bcd_min,bcd_sec);

}//end read_ds3231

void get_date(FILE *gen_fp)
{
	bcm2835_i2c_setSlaveAddress(DS3231);
	
	unsigned char date_buf[3]; 
	// buffer to hold data from date registers 0x04 to 0x06
	
	unsigned char date_reg[] = {0x04}; 
	// first address of date registers 0x04 to 0x06
	
	unsigned int bcd_dy, bcd_mnth, bcd_yr;

	bcm2835_i2c_read_register_rs(date_reg, date_buf, sizeof(date_buf));
	
	bcd_dy = (((date_buf[0] & 0xf0)/0x10) * 10) + (date_buf[0] & 0x0f);
	bcd_mnth = (((date_buf[1] & 0xf0)/0x10) * 10) + (date_buf[1] & 0x0f);
	bcd_yr	= (((date_buf[2] & 0xf0)/0x10) * 10) + (date_buf[2] & 0x0f);
	
	fprintf(gen_fp,"Date: %.2d/%.2d/%.2d \n",bcd_mnth,bcd_dy,bcd_yr);	
}

void read_dynamics(FILE *gen_fp)
{
 
	read_ds3231(gen_fp);	// read time from RTC for dynamics data
	fprintf(gen_fp,"%.1f,", mission_time);
	read_mpu6000(gen_fp);	// read accelerometer/gyrometer
	read_hmc5883(gen_fp);	// read magnetometer
	fprintf(gen_fp,"\n");


}// end read_dynamics

// READ_ADC()
// This function is designed to read 3 bytes of data from the LTC2495 
// ADC via an I2C buss. It then calculates the voltage sensed by the ADC
// according to the manufacturer's datasheet.
// INPUTS: FILE pointer, unsigned char SLAVE_ADDRESS
// OUTPUTS: float ADC_VOLTAGE
float read_adc(FILE *gen_fp, unsigned char slv_add)
{
	bcm2835_i2c_setSlaveAddress(slv_add);
	
	unsigned char v_buf[3];
	
	// pressure sensor variables
	int   adc_out;	// DATAOUT16 HEX Value
	float v_out;	// ADC voltage on channel
	
	bcm2835_i2c_read(v_buf, sizeof(v_buf));
	// read the data register from the LTC2495

	v_buf[0] = v_buf[0] & 0x7f;	// mask the first bit (unused)
	adc_out = (v_buf[0] * 0x10000) + (v_buf[1] * 0x100) + (v_buf[2]);
	adc_out = adc_out/0x40;
	v_out = adc_out * 0.000025177; 
	// multiply by step size to get voltage

	fprintf(gen_fp, "%.3f,",v_out); // print voltage to raw data file
	
	return v_out; // return the voltage sensed by the ADC
	
}// end read_adc



double calc_alt(double gen_alt, double Pr_torr, double Pr_bias)
// This function derives the altitdue from pressure values and the U.S.
// Standard Atmosphere Model 1976. The the only inputs are the current
// pressure reading and the last calculated altitude. In this sense, the
// derivation is iterative. The function then returns the new altitude.
// INPUTS: double LAST_ALTITUDE, double CURRENT_PRESSURE
// OUTPUTS: double NEW_ALTITUDE
// NOTES: Pressure is in units of (pascals) and altitude in (meters ASL)
{

double new_alt; // Returned Value
float R=8.31432; // Universal Gas Constant (N*m)/(mol*k)
float G=9.80665; // Gravitational Acceleration (m/s^2)
float M=0.0289644; // Molar Mass (Kg/mol) of Air
float PRESS_CONV = 133.322368; // Conversion (pascals/torr)
double Pr;

Pr_torr = Pr_torr - Pr_bias;

Pr = Pr_torr*PRESS_CONV;

unsigned int b;
// Index for US Standard Atmosphere Table

int H[]={0,11000,20000,32000,47000,51000,71000};
// Altitude (meters) ASL from US Standard Atmosphere

float P[]={101325.00,22632.10,5474.89,868.02,110.91,66.94,3.96};
// Static Pressure (pascals) from US Standard Atmosphere 

float T[]={288.15, 216.65, 216.65, 228.65,228.65,270.65,270.65,214.65};
// Standard Temperature (kelvin) from US Standard Atmosphere

float L[]={-0.0065,0,0.001,0.0028,0,-0.0028,-0.002};
// Standard Lapse Rate from US Standard Atmosphere

if((gen_alt<11000)&(gen_alt>=0))
{
	b = 0;
	new_alt = (T[b]*(pow((Pr/P[b]),(-R*L[b]/(G*M)))-1)/L[b])+H[b];
}
else if((gen_alt<20000)&(gen_alt>=11000))
{
	b = 1;
	new_alt = (R*T[b]*log(Pr/P[b])/(-G*M))+H[b]; // Lapse Rate = 0;
}
else if((gen_alt<32000)&(gen_alt>=20000))
{
	b = 2;
	new_alt = (T[b]*(pow((Pr/P[b]),(-R*L[b]/(G*M)))-1)/L[b])+H[b];
}
else if((gen_alt<47000)&(gen_alt>=32000))
{
	b = 3;
	new_alt = (T[b]*(pow((Pr/P[b]),(-R*L[b]/(G*M)))-1)/L[b])+H[b];
}
else if((gen_alt<51000)&(gen_alt>=47000))
{
	b = 4;
	new_alt = (R*T[b]*log(Pr/P[b])/(-G*M))+H[b]; // Lapse Rate = 0
}
else if((gen_alt<71000)&(gen_alt>=51000))
{
	b = 5;
	new_alt = (T[b]*(pow((Pr/P[b]),(-R*L[b]/(G*M)))-1)/L[b])+H[b];
}
else
{
	b = 6;
	new_alt = (T[b]*(pow((Pr/P[b]),(-R*L[b]/(G*M)))-1)/L[b])+H[b];
}

return new_alt;

}//end calc_alt

void init_gps(void)
{
	unsigned char setLCR[] 		= {0x18,0x03}; 	// one stop bit with set 8-bit words
	unsigned char clearFIFO[] 	= {0x10,0x07}; 	// reset Tx, Rx, FIFOS
	unsigned char setFCR[] 		= {0x10,0x01}; 	// enable FIFOs
	unsigned char setLCR_7[] 	= {0x18, 0x83}; // set LCR[7] = 1 to set divisor
	unsigned char setDLL[] 		= {0x00,0x34}; 	// Set the Divisor LSB register to 52 = 0x34
	
	bcm2835_i2c_setSlaveAddress(UART_MOD);
	
	// Set LCR[7] = 1 for changing divisor
	bcm2835_i2c_write(setLCR_7, sizeof(setLCR_7));
	
	// Set divisor LSB to 52
	bcm2835_i2c_write(setDLL, sizeof(setDLL));
		
	// Set LCR[7] = 0 for writing to FCR and LSR
	bcm2835_i2c_write(setLCR, sizeof(setLCR));
	
	// Clear and enable FIFOs
	bcm2835_i2c_write(clearFIFO, sizeof(clearFIFO));
	
	bcm2835_i2c_read_register_rs(&SFR, read_buf, sizeof(read_buf));

	bcm2835_i2c_read_register_rs(&LSR, read_buf, sizeof(read_buf));
	
	rx_ready = (read_buf[0] & 0x01);
	thr_empty = (read_buf[0] & 0x20) >> 5;
}

void read_gps(FILE *gen_fp)
{
	bcm2835_i2c_setSlaveAddress(UART_MOD);
	
	while(rx_ready == 0) // wait for receive byte to be ready
	{
	bcm2835_i2c_read_register_rs(&LSR, read_buf, sizeof(read_buf));
	rx_ready = (read_buf[0] & 0x01);	
	}
	
	bcm2835_i2c_read_register_rs(&LSR, read_buf, sizeof(read_buf));
    data_error = (read_buf[0] & 0x80) >> 7;
	thr_tsr_empty = (read_buf[0] & 0x40) >> 6;
	thr_empty = (read_buf[0] & 0x20) >> 5;
	rx_break = (read_buf[0] & 0x10) >> 4;
	framing_error = (read_buf[0] & 0x08) >> 3;
	parity_error = (read_buf[0] & 0x04) >> 2;
	over_run = (read_buf[0] & 0x02) >> 1;
	rx_ready = (read_buf[0] & 0x01);
	
	
	if(data_error == 1)
	fprintf(gen_fp,"data_error!\n");
		
	if(rx_break == 1)
	fprintf(gen_fp,"break error!\n");
	
	if(framing_error == 1)
	fprintf(gen_fp,"framing error!\n");
	
	if(parity_error ==1)
	fprintf(gen_fp,"parity error!\n");
	
	if(over_run == 1)
	fprintf(gen_fp,"over run error!\n");

	// read from RHR register	
	bcm2835_i2c_read_register_rs(&RHR, read_buf, sizeof(read_buf));
	
	fprintf(gen_fp,"%c", read_buf[0]); // print received byte to file
	
	bcm2835_i2c_read_register_rs(&LSR, read_buf, sizeof(read_buf));
	rx_ready = (read_buf[0] & 0x01);
	
}