/*
 * SimpleTesting.c
 *
 * Created: 09.04.2013 09:50:30
 *  Author: snfu
 */ 



//------------------------------------------------------
// DEFINE SECTION
//#define F_CPU	12000000UL



//------------------------------------------------------

//------------------------------------------------------
// INCLUDE SECTION
#include "GeneralSettings.h"
#include <avr/io.h>
#include <avr/delay.h>
#include "uart.h"
#include "ServoControl.h"
#include "port.h"
#include <avr/interrupt.h>
#include <string.h>
//------------------------------------------------------

//------------------------------------------------------
// Local declarations and variables

// temperature
int8_t	temperature	= 0;
uint8_t temperature_0	= 50;
// gyro reading
int8_t	gyro_x_h	= 0;
uint8_t gyro_x_l	= 0;
int16_t gyro_x		= 0;
int16_t	gyro_p		= 0;

int8_t	gyro_y_h	= 0;
uint8_t gyro_y_l	= 0;
int16_t gyro_y		= 0;
int16_t	gyro_q		= 0;

int8_t	gyro_z_h	= 0;
uint8_t gyro_z_l	= 0;
int16_t gyro_z		= 0;
float	gyro_r		= 0;
//------------------------------------------------------


void start_gyro(void)
{
	i2c_start();
	i2c_send_address_write(L3GD20_ADDRESS_WRITE);
	i2c_send_registry(L3GD20_CTRL_REG1);
	i2c_send_data(0x0F);
	i2c_stop();
	write_string_ln("gyro started");
}

void temp_read(void)
{
	i2c_start();
	i2c_send_address_write(L3GD20_ADDRESS_WRITE);
	i2c_send_registry(L3GD20_OUT_TEMP);
	i2c_repeated_start();
	i2c_send_address_read(L3GD20_ADDRESS_READ);
	temperature = i2c_read_data_nmak();
	i2c_stop();
	temperature = (temperature);
	write_var_ln(temperature);
}

void read_gyro_registry(char registry)
{
	int data;
	i2c_start();
	i2c_send_address_write(L3GD20_ADDRESS_WRITE);
	i2c_send_registry(registry);
	i2c_repeated_start();
	i2c_send_address_read(L3GD20_ADDRESS_READ);
	data = i2c_read_data_nmak();
	i2c_stop();
	write_var_ln(data);
}

void write_gyro_registry(char registry, char data)
{
	i2c_start();
	i2c_send_address_write(L3GD20_ADDRESS_WRITE);
	i2c_send_registry(registry);
	i2c_send_data(data);
	i2c_stop();
	write_string_ln("data wrote");
}

void gyro_read(void)
{
	i2c_start();
	i2c_send_address_write(L3GD20_ADDRESS_WRITE);
	i2c_send_registry(L3GD20_OUT_X_L+0b10000000);		// add MSB to activate auto increment
	i2c_repeated_start();
	i2c_send_address_read(L3GD20_ADDRESS_READ);
	gyro_x_l = i2c_read_data_mak();
	gyro_x_h = i2c_read_data_mak();
	gyro_y_l = i2c_read_data_mak();
	gyro_y_h = i2c_read_data_mak();
	gyro_z_l = i2c_read_data_mak();
	gyro_z_h = i2c_read_data_nmak();					// add the end we give no master ack to finish communication
	i2c_stop();
		
	gyro_x = (gyro_x_h<<4) | (gyro_x_l);
	gyro_y = (gyro_y_h<<4) | (gyro_y_l);
	gyro_z = (gyro_z_h<<4) | (gyro_z_l);
		
	gyro_p = gyro_x * 0.00875;
	/*write_string("gyro_x reg:");write_var(gyro_x);write_string("  p: ");*/write_var(gyro_p);write_string(" deg/s \r\n ");
}

int main(void)
{	
	
	// Initializing
	init_uart();
	servo_start_signal();
	i2c_initialize();
	
	
	//activate internal PullUps for I2C?
	//Data and clock lines are high as soon as the Controller is powered
	
	
	write_string_ln("Init Completed");
	
	start_gyro();
	
	_delay_ms(2000);
int i;
	
	while(1)
    {
		// Program Code
		//_delay_ms(100);
		gyro_read();

    }
}