void adc_read(void)
{
	//Read out distance to ground from infrared sensor
	global_data.ground_distance_unfiltered = infrared_distance_get()-0.10;//TODO: offset calculation in multitracker

	// Read out system status
	global_data.battery_voltage = battery_get_value();

	float ground_distance_filter_weight = 0.15;
	global_data.ground_distance = (1 - ground_distance_filter_weight)
			* global_data.ground_distance
			+ ground_distance_filter_weight * global_data.ground_distance_unfiltered;

	if (global_data.ground_distance < 1 && global_data.ground_distance
			> 0)
	{
		global_data.state.ground_distance_ok = 1;
	}

	else
	{
		global_data.state.ground_distance_ok = 0;
	}

	//GET NEWEST DATA
	//Switch to Grounddistance sensor for z if we dont have vision
//	if (global_data.state.vision_ok == 0)
//	{
//		if (global_data.state.ground_distance_ok == 1)
//		{
//			global_data.position.z = -global_data.ground_distance;
//		}
//		else
//		{
//			global_data.position.z = -1;
//		}
//		global_data.velocity.z = 0;
//	}
}
/**
* @brief Initialize the whole system
*
* All functions that need to be called before the first mainloop iteration
* should be placed here.
*/
void main_init_generic(void)
{

	// Reset to safe values
	global_data_reset();

	// Load default eeprom parameters as fallback
	global_data_reset_param_defaults();

	// LOWLEVEL INIT, ONLY VERY BASIC SYSTEM FUNCTIONS
	hw_init();
	enableIRQ();
	led_init();
	led_on(LED_GREEN);
	buzzer_init();
	sys_time_init();
	sys_time_periodic_init();
	sys_time_clock_init();
	ppm_init();
	pwm_init();

	// Lowlevel periphel support init
	adc_init();
	// FIXME SDCARD
//	MMC_IO_Init();
	spi_init();
	i2c_init();

	// Sensor init
	sensors_init();
	debug_message_buffer("Sensor initialized");

	// Shutter init
	shutter_init();
	shutter_control(0);

	// Debug output init
	debug_message_init();
	debug_message_buffer("Text message buffer initialized");

	// MEDIUM LEVEL INIT, INITIALIZE I2C, EEPROM, WAIT FOR MOTOR CONTROLLERS
	// Try to reach the EEPROM
	eeprom_check_start();

	// WAIT FOR 2 SECONDS FOR THE USER TO NOT TOUCH THE UNIT
	while (sys_time_clock_get_time_usec() < 2000000)
	{
	}

	// Do the auto-gyro calibration for 1 second
	// Get current temperature
	led_on(LED_RED);
	gyro_init();

//	uint8_t timeout = 3;
//	// Check for SD card
//	while (sys_time_clock_get_time_usec() < 2000000)
//	{
//		while (GetDriveInformation() != F_OK && timeout--)
//		  {
//		   debug_message_buffer("MMC/SD-Card not found ! retrying..");
//		  }
//	}
//
//	if (GetDriveInformation() == F_OK)
//	{
//		debug_message_buffer("MMC/SD-Card SUCCESS: FOUND");
//	}
//	else
//	{
//		debug_message_buffer("MMC/SD-Card FAILURE: NOT FOUND");
//	}
	//FIXME redo init because of SD driver decreasing speed
	//spi_init();
	led_off(LED_RED);

	// Stop trying to reach the EEPROM - if it has not been found by now, assume
	// there is no EEPROM mounted
	if (eeprom_check_ok())
	{
		param_read_all();
		debug_message_buffer("EEPROM detected - reading parameters from EEPROM");

		for (int i = 0; i < ONBOARD_PARAM_COUNT * 2 + 20; i++)
		{
			param_handler();
			//sleep 1 ms
			sys_time_wait(1000);
		}
	}
	else
	{
		debug_message_buffer("NO EEPROM - reading onboard parameters from FLASH");
	}

	// Set mavlink system
	mavlink_system.compid = MAV_COMP_ID_IMU;
	mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID];

	//Magnet sensor
	hmc5843_init();
	acc_init();

	// Comm parameter init
	mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID]; // System ID, 1-255
	mavlink_system.compid = global_data.param[PARAM_COMPONENT_ID]; // Component/Subsystem ID, 1-255

	// Comm init has to be
	// AFTER PARAM INIT
	comm_init(MAVLINK_COMM_0);
	comm_init(MAVLINK_COMM_1);

	// UART initialized, now initialize COMM peripherals
	communication_init();
	gps_init();

	us_run_init();

	servos_init();

	//position_kalman3_init();

	// Calibration starts (this can take a few seconds)
	//	led_on(LED_GREEN);
	//	led_on(LED_RED);

	// Read out first time battery
	global_data.battery_voltage = battery_get_value();

	global_data.state.mav_mode = MAV_MODE_PREFLIGHT;
	global_data.state.status = MAV_STATE_CALIBRATING;

	send_system_state();

	float_vect3 init_state_accel;
	init_state_accel.x = 0.0f;
	init_state_accel.y = 0.0f;
	init_state_accel.z = -1000.0f;
	float_vect3 init_state_magnet;
	init_state_magnet.x = 1.0f;
	init_state_magnet.y = 0.0f;
	init_state_magnet.z = 0.0f;


	//auto_calibration();


	attitude_observer_init(init_state_accel, init_state_magnet);

	debug_message_buffer("Attitude Filter initialized");
	led_on(LED_RED);

	send_system_state();

	debug_message_buffer("System is initialized");

	// Calibration stopped
	led_off(LED_RED);

	global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
	global_data.state.status = MAV_STATE_STANDBY;

	send_system_state();

	debug_message_buffer("Checking if remote control is switched on:");
	// Initialize remote control status
	remote_control();
	remote_control();
	if (radio_control_status() == RADIO_CONTROL_ON && global_data.state.remote_ok)
	{
		global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED;
		debug_message_buffer("RESULT: remote control switched ON");
		debug_message_buffer("Now in MAV_MODE_TEST2 position hold tobi_laurens");
		led_on(LED_GREEN);
	}
	else
	{
		global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
		debug_message_buffer("RESULT: remote control switched OFF");
		led_off(LED_GREEN);
	}
}