void main_loop_quadrotor(void)
{
	/**
	 * @brief Initialize the whole system
	 *
	 * All functions that need to be called before the first mainloop iteration
	 * should be placed here.
	 */
	main_init_generic();
	control_quadrotor_position_init();
	control_quadrotor_attitude_init();
	attitude_tobi_laurens_init();

	// FIXME XXX Make proper mode switching

//	outdoor_position_kalman_init();
	//vision_position_kalman_init();

	// Default filters, allow Vision, Vicon and optical flow inputs
	vicon_position_kalman_init();
	optflow_speed_kalman_init();

	/**
	 * @brief This is the main loop
	 *
	 * It will be executed at maximum MCU speed (60 Mhz)
	 */
	// Executiontime debugging
	time_debug.x = 0;
	time_debug.y = 0;
	time_debug.z = 0;

	last_mainloop_idle = sys_time_clock_get_time_usec();
	debug_message_buffer("Starting main loop");

	led_off(LED_GREEN);
	led_off(LED_RED);
	while (1)
	{
		// Time Measurement
		uint64_t loop_start_time = sys_time_clock_set_loop_start_time(); // loop_start_time should not be used anymore

		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL 200 Hz functions
		///////////////////////////////////////////////////////////////////////////
		if (us_run_every(5000, COUNTER2, loop_start_time))
		{
			// Kalman Attitude filter, used on all systems
			gyro_read();
			sensors_read_acc();

			sensors_pressure_bmp085_read_out();

			// Read out magnetometer at its default 50 Hz rate
			static uint8_t mag_count = 0;

			if (mag_count == 3)
			{
				sensors_read_mag();
				//attitude_observer_correct_magnet(global_data.magnet_corrected);
				mag_count = 0;
			}else if(mag_count==1){

				hmc5843_start_read();
				mag_count++;
			}
			else
			{
				mag_count++;
			}

			// Correction step of observer filter
			attitude_tobi_laurens();

			if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VICON_ONLY ||
				global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VISION_VICON_BACKUP)
			{
				vicon_position_kalman();
			}
			else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_ONLY)
			{
				outdoor_position_kalman();
			}

			control_quadrotor_attitude();

			//debug counting number of executions
			count++;
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// Camera Shutter - This takes 50 usecs!!!
		///////////////////////////////////////////////////////////////////////////
		// Set camera shutter with 2.5ms resolution
		else if (us_run_every(5000, COUNTER1, loop_start_time)) //was 2500 !!!
		{
			camera_shutter_handling(loop_start_time);

			// Measure time for debugging
			time_debug.x = max(time_debug.x, sys_time_clock_get_time_usec()
					- loop_start_time);

		}

		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL FAST 50 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(20000, COUNTER3, loop_start_time))
		{
			// Read infrared sensor
			//adc_read();

			// Control the quadrotor position
			control_quadrotor_position();
			// Read remote control
			remote_control();

			control_camera_angle();

//			//float_vect3 opt;
//			static float_vect3 opt_int;
//			uint8_t valid = optical_flow_get_dxy(80, &global_data.optflow.x, &global_data.optflow.y, &global_data.optflow.z);
//			if (valid)
//			{
//				opt_int.x += global_data.optflow.x;
//				opt_int.y += global_data.optflow.y;
//
//			}
//
//			uint8_t supersampling = 10;
//			for (int i = 0; i < supersampling; ++i)
//			{
//				global_data.sonar_distance += sonar_distance_get(ADC_5_CHANNEL);
//			}
//
//			global_data.sonar_distance /= supersampling;
//
//			opt_int.z = valid;
//			static unsigned int i = 0;
//			if (i == 10)
//			{
//				mavlink_msg_optical_flow_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_loop_start_time(), 0, global_data.optflow.x, global_data.optflow.y, global_data.optflow.z, global_data.sonar_distance_filtered);
//
//				i = 0;
//			}
//			i++;
			//optical_flow_debug_vect_send();
			//debug_vect("opt_int", opt_int);
//			optical_flow_start_read(80);

			if (global_data.state.position_estimation_mode
					== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_INTEGRATING
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_NON_INTEGRATING
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VICON_AS_OFFSET
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VISION_AS_OFFSET
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ODOMETRY_ADD_VISION_AS_OFFSET
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VICON
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_GPS_OPTICAL_FLOW
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_GLOBAL_VISION
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VISUAL_ODOMETRY_GLOBAL_VISION)
			{
				optflow_speed_kalman();
			}

			// Send the raw sensor/ADC values
			communication_send_raw_data(loop_start_time);

			float_vect3 yy; yy.x = global_data.yaw_lowpass; yy.y = 0.f; yy.z = 0.f;
			debug_vect("yaw_low", yy);
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL FAST 20 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(50000, COUNTER4, loop_start_time))
		{
			//*** this happens in handle_controller_timeouts already!!!!! ***
			//			//update global_data.state
			//			if (global_data.param[PARAM_VICON_MODE] == 1)
			//			{
			//				//VICON_MODE 1 only accepts vicon position
			//				global_data.state.position_fix = global_data.state.vicon_ok;
			//			}
			//			else
			//			{
			//				//VICON_MODEs 0, 2, 3 accepts vision additionally, so check vision
			//				global_data.state.position_fix = global_data.state.vision_ok;
			//			}

			update_system_statemachine(loop_start_time);
			update_controller_setpoints();

			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(
					global_data.param[PARAM_SEND_DEBUGCHAN],
					sys_time_clock_get_loop_start_time_boot_ms(),
					global_data.attitude_setpoint.x,
					global_data.attitude_setpoint.y,
					global_data.position_yaw_control_output,
					global_data.thrust_control_output);

			//STARTING AND LANDING
			quadrotor_start_land_handler(loop_start_time);
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 100 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(5000, COUNTER6, loop_start_time))
		{

			if (global_data.param[PARAM_SEND_SLOT_DEBUG_6])
			{
				debug_vect("att_ctrl_o", global_data.attitude_control_output);
			}
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// UNCRITICAL SLOW 5 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(200000, COUNTER8, loop_start_time))
		{
			// The onboard controllers go into failsafe mode once
			// position data is missing
			handle_controller_timeouts(loop_start_time);
			// Send buffered data such as debug text messages
			// Empty one message out of the buffer
			debug_message_send_one();

			// Toggle status led
			led_toggle(LED_RED);

			// Toggle active mode led
			if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED)
			{
				led_on(LED_GREEN);
			}
			else
			{
				led_off(LED_GREEN);
			}

			handle_eeprom_write_request();
			handle_reset_request();

			update_controller_parameters();

			communication_send_controller_feedback();

			communication_send_remote_control();

			// Pressure sensor driver works, but not tested regarding stability
			//			sensors_pressure_bmp085_read_out();

			if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
			{
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
						90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
						91, global_data.yaw_pos_setpoint);
			}
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 1 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(1000000, COUNTER9, loop_start_time))
		{
			// Send system state, mode, battery voltage, etc.
			send_system_state();

			// Send position setpoint offset
			//debug_vect("pos offs", global_data.position_setpoint_offset);

			// Send current onboard time
			mavlink_msg_system_time_send(MAVLINK_COMM_1, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());
			mavlink_msg_system_time_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());

			//update state from received parameters
			sync_state_parameters();

			//debug number of execution
			count = 0;

			if (global_data.param[PARAM_GPS_MODE] >= 10)
			{
				//Send GPS information
				float_vect3 gps;
				gps.x = gps_utm_north / 100.0f;//m
				gps.y = gps_utm_east / 100.0f;//m
				gps.z = gps_utm_zone;// gps_week;
				debug_vect("GPS", gps);

			}
			else if (global_data.param[PARAM_GPS_MODE] == 9
					|| global_data.param[PARAM_GPS_MODE] == 8)
			{

				if (global_data.param[PARAM_GPS_MODE] == 8)
				{
					gps_set_local_origin();
					//					gps_local_home_init = false;
				}
				if (gps_lat == 0)
				{
					debug_message_buffer("GPS Signal Lost");
				}
				else
				{
					float_vect3 gps_local, gps_local_velocity;
					gps_get_local_position(&gps_local);
					debug_vect("GPS local", gps_local);
					gps_get_local_velocity(&gps_local_velocity);
					debug_vect("GPS loc velocity", gps_local_velocity);
				}
			}
			if (global_data.state.gps_mode)
			{
				gps_send_local_origin();
			}
			beep_on_low_voltage();

		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 20 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(50000, COUNTER7, loop_start_time))
		{
			//led_toggle(LED_YELLOW);

			if (global_data.param[PARAM_GPS_MODE] >= 10)
			{
				//get thru all gps messages
				debug_message_send_one();
			}

			communication_send_attitude_position(loop_start_time);

			// Send parameter
			communication_queued_send();

//			//infrared distance
//			float_vect3 infra;
//			infra.x = global_data.ground_distance;
//			infra.y = global_data.ground_distance_unfiltered;
//			infra.z = global_data.state.ground_distance_ok;
//			debug_vect("infrared", infra);
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 200 Hz functions                                     //
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(5000, COUNTER5, loop_start_time))
		{
			if (global_data.state.status == MAV_STATE_STANDBY)
			{
				//Check if parameters should be written or read
				param_handler();
			}
		}
		///////////////////////////////////////////////////////////////////////////

		else
		{
			// All Tasks are fine and we have no starvation
			last_mainloop_idle = loop_start_time;
		}

		// Read out comm at max rate - takes only a few microseconds in worst case
		communication_receive();

		// MCU load measurement
		uint64_t loop_stop_time = sys_time_clock_get_time_usec();
		global_data.cpu_usage = measure_avg_cpu_load(loop_start_time,
				loop_stop_time, min_mainloop_time);
		global_data.cpu_peak = measure_peak_cpu_load(loop_start_time,
				loop_stop_time, min_mainloop_time);
		time_debug.y = max(time_debug.y, global_data.cpu_usage);
		time_debug.z = max(time_debug.z, global_data.cpu_peak);
		if (loop_start_time - last_mainloop_idle >= 20000)
		{
			debug_message_buffer(
					"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
			last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
		}
		if (global_data.cpu_usage > 800)
		{
			// CPU load higher than 80%
			debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
		}
	} // End while(1)

}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_fixed_wing(void)
{
	last_mainloop_idle = sys_time_clock_get_time_usec();
	debug_message_buffer("Starting main loop");
	while (1)
	{
		// Time Measurement
		uint64_t loop_start_time = sys_time_clock_get_time_usec();

		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// Camera Shutter
		///////////////////////////////////////////////////////////////////////////
		// Set camera shutter with 2.5ms resolution
		if (us_run_every(2500, COUNTER1, loop_start_time))
		{
			camera_shutter_handling(loop_start_time);
		}

		if (global_data.state.mav_mode == MAV_MODE_RC_TRAINING)
		{
			///////////////////////////////////////////////////////////////////////////
			/// RC INTERFACE HACK AT 50 Hz
			///////////////////////////////////////////////////////////////////////////
			if (us_run_every(20000, COUNTER8, loop_start_time))
			{
				// Write start byte
				uart1_transmit(0xFF);

				// Write channels 1-6
				for (int i = 1; i < 7; i++)
				{
					uart1_transmit((radio_control_get_channel(1)+1)*127);
				}
			}
			led_toggle(LED_GREEN);
			led_toggle(LED_RED);
			// Do not execute any of the functions below
			continue;
		}

		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL 200 Hz functions
		///////////////////////////////////////////////////////////////////////////
		if (us_run_every(5000, COUNTER2, loop_start_time))
		{
			// Kalman Attitude filter, used on all systems
			gyro_read();
			sensors_read_acc();

			// Read out magnetometer at its default 50 Hz rate
			static uint8_t mag_count = 0;
			if (mag_count == 3)
			{
				sensors_read_mag();
				attitude_observer_correct_magnet(global_data.magnet_corrected);
				mag_count = 0;
			}
			else
			{
				mag_count++;
			}

			// Correction step of observer filter
			attitude_observer_correct_accel(global_data.accel_raw);

			// Write in roll and pitch
			static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			attitude_observer_get_angles(&att);
			global_data.attitude.x = att.x;
			global_data.attitude.y = att.y;
			if (global_data.param[PARAM_ATT_KAL_IYAW])
			{
				global_data.attitude.z += 0.005 * global_data.gyros_si.z;
			}
			else
			{
				global_data.attitude.z = att.z;
			}
			// Prediction step of observer
			attitude_observer_predict(global_data.gyros_si);

			control_fixed_wing_attitude();

		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL FAST 50 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(20000, COUNTER3, loop_start_time))
		{
			// Read Analog-to-Digital converter
			adc_read();
			// Read remote control
			remote_control();
		}
		///////////////////////////////////////////////////////////////////////////



		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 100 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(10000, COUNTER6, loop_start_time))
		{
			// Send the raw sensor/ADC values
			communication_send_raw_data(loop_start_time);
		}
		///////////////////////////////////////////////////////////////////////////



		///////////////////////////////////////////////////////////////////////////
		/// UNCRITICAL SLOW 5 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(200000, COUNTER8, loop_start_time))
		{
			// The onboard controllers go into failsafe mode once
			// position data is missing
			handle_controller_timeouts(loop_start_time);
			// Send buffered data such as debug text messages
			communication_queued_send();
			// Empty one message out of the buffer
			debug_message_send_one();

			// Toggle status led
			//led_toggle(LED_YELLOW);
			led_toggle(LED_RED); // just for green LED on alpha at the moment

			// Toggle active mode led
			if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
					== MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO)
			{
				led_on(LED_GREEN);
			}
			else
			{
				led_off(LED_GREEN);
			}

			handle_eeprom_write_request();
			handle_reset_request();

			communication_send_controller_feedback();

			communication_send_remote_control();

			// Pressure sensor driver works, but not tested regarding stability
			sensors_pressure_bmp085_read_out();

			if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
			{
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
						90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
						91, global_data.yaw_pos_setpoint);
			}

		}
		///////////////////////////////////////////////////////////////////////////



		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 1 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(1000000, COUNTER9, loop_start_time))
		{
			// Send system state, mode, battery voltage, etc.
			send_system_state();

			if (global_data.param[PARAM_GPS_MODE] >= 10)
			{
				//Send GPS information
				float_vect3 gps;
				gps.x = gps_utm_north / 100.0f;//m
				gps.y = gps_utm_east / 100.0f;//m
				gps.z = gps_utm_zone;// gps_week;
				debug_vect("GPS", gps);
			}

			beep_on_low_voltage();

		}
		///////////////////////////////////////////////////////////////////////////




		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 20 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(50000, COUNTER7, loop_start_time))
		{
			led_toggle(LED_YELLOW);

			if (global_data.param[PARAM_GPS_MODE] >= 10)
			{
				//get thru all gps messages
				debug_message_send_one();
			}

			communication_send_attitude_position(loop_start_time);
		}
		///////////////////////////////////////////////////////////////////////////




		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 200 Hz functions                                     //
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(5000, COUNTER5, loop_start_time))
		{
			if (global_data.state.status == MAV_STATE_STANDBY)
			{
				//Check if parameters should be written or read
				param_handler();
			}
		}
		///////////////////////////////////////////////////////////////////////////

		else {
			// All Tasks are fine and we have no starvation
			last_mainloop_idle = loop_start_time;
		}

		// Read out comm at max rate - takes only a few microseconds in worst case
		communication_receive();

		// MCU load measurement
		uint64_t loop_stop_time = sys_time_clock_get_time_usec();
		global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
		global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
		if (loop_start_time - last_mainloop_idle >= 5000)
		{
			debug_message_buffer(
					"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
			last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
		}
		if (global_data.cpu_usage > 800)
		{
			// CPU load higher than 80%
			debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
		}
	} // End while(1)

}
/**
* @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);
	}
}