Exemple #1
0
bool calibration_enter(void)
{
	// If not flying
	if (!sys_state_is_flying())
	{
		calibration_prev_state = sys_get_state();
		calibration_prev_mode = sys_get_mode();
		// Lock vehicle during calibration
		sys_set_mode((uint8_t)MAV_MODE_LOCKED);
		sys_set_state((uint8_t)MAV_STATE_CALIBRATING);
		debug_message_buffer("Starting calibration.");

		mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
				global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
				global_data.motor_block, communication_get_uart_drop_rate());
		mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
				global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
				global_data.motor_block, communication_get_uart_drop_rate());
		debug_message_send_one();
		debug_message_send_one();
		return true;
	}
	else
	{
		//Can't calibrate during flight
		debug_message_buffer("Can't calibrate during flight!!!");
		return false;
	}

}
void execute_action(uint8_t action)
{
	switch (action)
	{
	case MAV_ACTION_LAUNCH:
		if (global_data.state.mav_mode > (uint8_t)MAV_MODE_LOCKED)
		{
			global_data.state.status = (uint8_t)MAV_STATE_ACTIVE;
		}
		break;
	case MAV_ACTION_MOTORS_START:
		if (global_data.state.mav_mode > (uint8_t)MAV_MODE_LOCKED)
		{
			global_data.state.status = (uint8_t)MAV_STATE_ACTIVE;
		}
		break;
	case MAV_ACTION_MOTORS_STOP:
		global_data.state.status = (uint8_t)MAV_STATE_STANDBY;
		break;
	case MAV_ACTION_EMCY_KILL:
		global_data.state.status = (uint8_t)MAV_STATE_EMERGENCY;
		break;
	case MAV_ACTION_STORAGE_READ:
		param_read_all();
		debug_message_buffer("Started reading params from eeprom");
		break;
	case MAV_ACTION_STORAGE_WRITE:
		debug_message_buffer("Started writing params to eeprom");
		param_write_all();
		break;
	case MAV_ACTION_CALIBRATE_GYRO:
		start_gyro_calibration();
		break;
	case MAV_ACTION_CALIBRATE_RC:
		start_rc_calibration();
		break;
	case MAV_ACTION_CALIBRATE_MAG:
		start_mag_calibration();
		break;
	case MAV_ACTION_CALIBRATE_PRESSURE:
		start_pressure_calibration();
		break;
	case MAV_ACTION_SET_ORIGIN:
		// If not flying
		if (!sys_state_is_flying())
		{
			gps_set_local_origin();
			altitude_set_local_origin();
		}
		break;
	default:
		// Should never be reached, ignore unknown commands
		debug_message_buffer_sprintf("Rejected unknown action Number: %u", action);
		break;
	}
}
void handle_eeprom_write_request(void)
{
	//GET UPDATES FROM PARAMS
	//testing eeprom params

	if (global_data.state.status == MAV_STATE_STANDBY)
	{
		switch ((uint8_t) global_data.param[PARAM_IMU_RESET])
		{
		case 2:
			debug_message_buffer("start writing params to eeprom");
			param_write_all();
			global_data.param[PARAM_IMU_RESET] = 0;
			break;
		case 3:
			param_read_all();
			debug_message_buffer("start reading params from eeprom");
			global_data.param[PARAM_IMU_RESET] = 0;
			break;
		case 4:
			global_data.param[PARAM_IMU_RESET] = 0;
			debug_message_buffer("params reset to defaults");
			global_data_reset_param_defaults();
			break;
		case 5:
			global_data.param[PARAM_IMU_RESET] = 0;
			if (param_size_check())
			{
				debug_message_buffer("check true");
			}
			else
			{
				debug_message_buffer("check false");
			}

			break;
		case 6:
			global_data.param[PARAM_IMU_RESET] = 0;
			eeprom_check_start();
			break;
		default:
			break;
		}
	}
}
void gps_set_local_origin(void)
{
	gps_local_origin.x = gps_lat / 1e7f;
	gps_local_origin.y = gps_lon / 1e7f;
	gps_local_origin.z = gps_alt / 100e0f;
	gps_cos_origin_lat = cos(gps_local_origin.x * 3.1415 / 180);

	gps_local_origin_init = true;

	debug_message_buffer("GPS Local Origin saved");
}
Exemple #5
0
/**
 * @brief Set the current system state
 * @param state the new system state
 */
bool sys_set_state(uint8_t state)
{
	if (state == MAV_STATE_ACTIVE)
	{
		global_data.state.status = MAV_STATE_ACTIVE;
		return true;
	}
	else if (state == MAV_STATE_BOOT)
	{
		global_data.state.status = MAV_STATE_BOOT;
		return true;
	}
	else if (state == MAV_STATE_CALIBRATING)
	{
		global_data.state.status = MAV_STATE_CALIBRATING;
		return true;
	}
	else if (state == MAV_STATE_CRITICAL)
	{
		global_data.state.status = MAV_STATE_CRITICAL;
		return true;
	}
	else if (state == MAV_STATE_EMERGENCY)
	{
		global_data.state.status = MAV_STATE_EMERGENCY;
		return true;
	}
	else if (state == MAV_STATE_POWEROFF)
	{
		global_data.state.status = MAV_STATE_POWEROFF;
		return true;
	}
	else if (state == MAV_STATE_STANDBY)
	{
		global_data.state.status = MAV_STATE_STANDBY;
		return true;
	}
	else if (state == MAV_STATE_UNINIT)
	{
		global_data.state.status = MAV_STATE_STANDBY;
		return true;
	}
	else
	{
		// UNINIT or invalid state, ignore value and return false
		debug_message_buffer("WARNING: Attempted to set invalid state");
		return false;
	}

	// FIXME Remove this once new interface is existent
	global_data.state.status = state;

}
Exemple #6
0
/**
 * @note Buffer one debug message with a integer variable in it
 * @param string
 * */
uint8_t debug_message_buffer_sprintf(const char* string, const uint32_t num)
{
	if (debug_message_buffer(string))
	{
		// Could write, save integer to buffer
		m_debug_buf_int[m_debug_index_write] = num;
		return 1;
	}
	else
	{
		// Could not write, do nothing
		return 0;
	}
}
/**
 * @brief Add a string debug message to the send buffer
 *
 * @param string Message
 * @return 1 if successfully added, 0 else
 */
uint8_t debug_string_message_buffer(const char* string)
{
	if (debug_message_buffer(string))
	{
		/* Could write, save message to buffer */
		m_debug_buf_type[m_debug_index_write] = DEBUG_STRING;
		return 1;
	}
	else
	{
		/* Could not write, do nothing */
		return 0;
	}
}
Exemple #8
0
void start_rc_calibration(void)
{
	if (calibration_enter())
	{


		// Calibration routine: Read out remote control and wait for all channels.
		// Read initial values.
		const uint8_t chan_count = 9;
		uint32_t chan_initial[chan_count];
		uint32_t chan_min[chan_count];
		uint32_t chan_max[chan_count];
		for (int i = 0; i < chan_count; i++)
		{
			chan_initial[i] = ppm_get_channel(i + 1);
			chan_min[i]=2000;
			chan_max[i]=1000;
		}

		bool abort_rc_cal = 0;
		uint64_t timeout = sys_time_clock_get_time_usec() + 30 * 1000000;
		while (!abort_rc_cal)
		{
			// Now save the min and max values for each channel
			for (int i = 0; i < chan_count; i++)
			{
				uint32_t ppm_value = (uint32_t) ppm_get_channel(i + 1);
				chan_min[i] = min(chan_min[i], ppm_value);
				chan_min[i] = min(chan_min[i], ppm_value);
			}
			// until motors stop conditions are met.
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG))
			{
				abort_rc_cal = 1;
			}
			if(sys_time_clock_get_time_usec()>timeout){
				abort_rc_cal = 1;
				debug_message_buffer("RC calibration abborted after 30 seconds");
			}
		}




		calibration_exit();
	}
}
/**
 * @brief Add a string-float debug message to the send buffers
 *
 * @param string Message
 * @return 1 if successfully added, 0 else
 */
uint8_t debug_float_message_buffer(const char* string, float num)
{
	if (debug_message_buffer(string))
	{
		/* Could write, save float to buffer */
		m_debug_buf_float[m_debug_index_write] = num;
		m_debug_buf_type[m_debug_index_write] = DEBUG_FLOAT;
		return 1;
	}
	else
	{
		/* Could not write, do nothing */
		return 0;
	}
}
/**
 * @brief Add a string-integer debug message to the send buffers
 *
 * @param string Message
 * @return 1 if successfully added, 0 else
 */
uint8_t debug_int_message_buffer(const char* string, int32_t num)
{
	if (debug_message_buffer(string))
	{
		/* Could write, save integer to buffer */
		m_debug_buf_int[m_debug_index_write] = num;
		m_debug_buf_type[m_debug_index_write] = DEBUG_INT;
		return 1;
	}
	else
	{
		/* Could not write, do nothing */
		return 0;
	}
}
Exemple #11
0
void calibration_exit(void)
{
	// Go back to old state
	sys_set_mode(calibration_prev_mode);
	sys_set_state(calibration_prev_state);

	// Clear debug message buffers
	for (int i = 0; i < DEBUG_COUNT; i++)
	{
		debug_message_send_one();
	}

	// Clear UART buffers
	while (uart0_char_available())
	{uart0_get_char();}
	while (uart1_char_available())
	{uart1_get_char();}

	debug_message_buffer("Calibration finished. UART buffers cleared.");
}
void attitude_tobi_laurens(void)
{
	//Transform accelerometer used in all directions
	//	float_vect3 acc_nav;
	//body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav);

	// Kalman Filter

	//Calculate new linearized A matrix
	attitude_tobi_laurens_update_a();

	kalman_predict(&attitude_tobi_laurens_kal);

	//correction update

	m_elem measurement[9] =
	{ };
	m_elem mask[9] =
	{ 1.0f, 1.0f, 1.0f, 0, 0, 0, 1.0f, 1.0f, 1.0f };

	float_vect3 acc;
	float_vect3 mag;
	float_vect3 gyro;

	//	acc.x = global_data.accel_raw.x * 9.81f /690;
	//	acc.y = global_data.accel_raw.y * 9.81f / 690;
	//	acc.z = global_data.accel_raw.z * 9.81f / 690;

#ifdef IMU_PIXHAWK_V250

	acc.x = global_data.accel_raw.x * (650.0f/900.0f);
	acc.y = global_data.accel_raw.y * (650.0f/900.0f);
	acc.z = global_data.accel_raw.z * (650.0f/900.0f);

#else

	acc.x = global_data.accel_raw.x;
	acc.y = global_data.accel_raw.y;
	acc.z = global_data.accel_raw.z;

#endif

	float acc_norm = sqrt(global_data.accel_raw.x * global_data.accel_raw.x + global_data.accel_raw.y * global_data.accel_raw.y + global_data.accel_raw.z * global_data.accel_raw.z);
	static float acc_norm_filt = SCA3100_COUNTS_PER_G;
	float acc_norm_lp = 0.05f;
	acc_norm_filt = (1.0f - acc_norm_lp) * acc_norm_filt + acc_norm_lp
			* acc_norm;

	//	static float acc_norm_filtz = SCA3100_COUNTS_PER_G;
	//	float acc_norm_lpz = 0.05;
	//	acc_norm_filtz = (1.0f - acc_norm_lpz) * acc_norm_filtz + acc_norm_lpz * -acc.z;

	float acc_diff = fabs(acc_norm_filt - SCA3100_COUNTS_PER_G);
	if (acc_diff > 200.0f)
	{
		//Don't correct when acc high
		mask[0] = 0;
		mask[1] = 0;
		mask[2] = 0;

	}
	else if (acc_diff > 100.0f)
	{
		//fade linearely out between 100 and 200
		float mask_lin = (200.0f - acc_diff) / 100.0f;
		mask[0] = mask_lin;
		mask[1] = mask_lin;
		mask[2] = mask_lin;
	}
	//else mask stays 1

//	static uint8_t i = 0;
//	if (i++ > 10)
//	{
//		i = 0;
//		float_vect3 debug;
//		debug.x = mask[0];
//		debug.y = acc_norm;
//		debug.z = acc_norm_filt;
//		debug_vect("acc_norm", debug);
//	}

	//	mag.x = (global_data.magnet_corrected.x ) * 1.f / 510.f;
	//	mag.y = (global_data.magnet_corrected.y) * 1.f / 510.f;
	//	mag.z = (global_data.magnet_corrected.z) * 1.f / 510.f;
	mag.x = (global_data.magnet_corrected.x ) ;
	mag.y = (global_data.magnet_corrected.y) ;
	mag.z = (global_data.magnet_corrected.z) ;


//	gyro.x = -(global_data.gyros_raw.x-global_data.param[PARAM_GYRO_OFFSET_X]) * 0.000955;
//	gyro.y = (global_data.gyros_raw.y-global_data.param[PARAM_GYRO_OFFSET_Y]) * 0.000955;
//	gyro.z = -(global_data.gyros_raw.z-global_data.param[PARAM_GYRO_OFFSET_Z]) * 0.001010;

	gyro.x = -(global_data.gyros_raw.x-global_data.param[PARAM_GYRO_OFFSET_X]) * 0.001008f;
	gyro.y = (global_data.gyros_raw.y-global_data.param[PARAM_GYRO_OFFSET_Y]) * 0.001008f;
	gyro.z = -(global_data.gyros_raw.z-global_data.param[PARAM_GYRO_OFFSET_Z]) * 0.001010f;



	measurement[0] = acc.x;
	measurement[1] = acc.y;
	measurement[2] = acc.z;

	if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VISION)
	{
		measurement[3] = global_data.vision_magnetometer_replacement.x;
		measurement[4] = global_data.vision_magnetometer_replacement.y;
		measurement[5] = global_data.vision_magnetometer_replacement.z;
	}
	else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
	{
//		measurement[3] = global_data.vision_global_magnetometer_replacement.x;
//		measurement[4] = global_data.vision_global_magnetometer_replacement.y;
//		measurement[5] = global_data.vision_global_magnetometer_replacement.z;
	}
	else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VICON)
	{
		measurement[3] = global_data.vicon_magnetometer_replacement.x;
		measurement[4] = global_data.vicon_magnetometer_replacement.y;
		measurement[5] = global_data.vicon_magnetometer_replacement.z;
	}
	else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
	{
		// Do nothing
		// KEEP THIS IN HERE
	}
	else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_MAGNETOMETER) // YAW_ESTIMATION_MODE_MAGNETOMETER
	{
		measurement[3] = mag.x;
		measurement[4] = mag.y;
		measurement[5] = mag.z;
//		debug_vect("mag_f", mag);
	}
	else
	{
		static uint8_t errcount = 0;
		if (errcount == 0) debug_message_buffer("ATT EST. ERROR: No valid yaw estimation mode set!");
		errcount++;
		global_data.state.status = MAV_STATE_CRITICAL;
	}

	measurement[6] = gyro.x;
	measurement[7] = gyro.y;
	measurement[8] = gyro.z;

	//Put measurements into filter


	static int j = 0;

	// MASK
	if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
	{
		// Do nothing, pure integration
	}
	else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VICON)
	{
		// If our iteration count is right and new vicon data is available, update measurement
		if (j >= 3 && global_data.state.vicon_attitude_new_data == 1)
		{
			j = 0;

			mask[3]=1;
			mask[4]=1;
			mask[5]=1;
		}
		else
		{
			j++;
		}
		global_data.state.vicon_attitude_new_data = 0;
	}
	else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VISION)
	{
		// If the iteration count is right and new vision data is available, update measurement
		if (j >= 3 && global_data.state.vision_attitude_new_data == 1)
		{
			j = 0;

			mask[3]=1;
			mask[4]=1;
			mask[5]=1;
		}
		else
		{
			j++;
		}
		global_data.state.vision_attitude_new_data = 0;
	}
	else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
		{
//			// If the iteration count is right and new vision data is available, update measurement
//			if (j >= 3 && global_data.state.global_vision_attitude_new_data == 1)
//			{
//				j = 0;
//
//				mask[3]=1;
//				mask[4]=1;
//				mask[5]=1;
//				j=0;
//			}
//			else
//			{
//				j++;
//			}
//			global_data.state.global_vision_attitude_new_data = 0;
		}
	else
	{
		if (j >= 3 && global_data.state.magnet_ok)
		{
			j = 0;

			mask[3]=1;
			mask[4]=1;
			mask[5]=1;
		}
		else
		{
			j++;
		}
	}

	kalman_correct(&attitude_tobi_laurens_kal, measurement, mask);


	//debug

	// save outputs
	float_vect3 kal_acc, kal_mag, kal_w0, kal_w;

	kal_acc.x = kalman_get_state(&attitude_tobi_laurens_kal, 0);
	kal_acc.y = kalman_get_state(&attitude_tobi_laurens_kal, 1);
	kal_acc.z = kalman_get_state(&attitude_tobi_laurens_kal, 2);

	kal_mag.x = kalman_get_state(&attitude_tobi_laurens_kal, 3);
	kal_mag.y = kalman_get_state(&attitude_tobi_laurens_kal, 4);
	kal_mag.z = kalman_get_state(&attitude_tobi_laurens_kal, 5);

	kal_w0.x = kalman_get_state(&attitude_tobi_laurens_kal, 6);
	kal_w0.y = kalman_get_state(&attitude_tobi_laurens_kal, 7);
	kal_w0.z = kalman_get_state(&attitude_tobi_laurens_kal, 8);

	kal_w.x = kalman_get_state(&attitude_tobi_laurens_kal, 9);
	kal_w.y = kalman_get_state(&attitude_tobi_laurens_kal, 10);
	kal_w.z = kalman_get_state(&attitude_tobi_laurens_kal, 11);

	float_vect3 x_n_b, y_n_b, z_n_b;
	z_n_b.x = -kal_acc.x;
	z_n_b.y = -kal_acc.y;
	z_n_b.z = -kal_acc.z;
	vect_norm(&z_n_b);
	vect_cross_product(&z_n_b, &kal_mag, &y_n_b);
	vect_norm(&y_n_b);

	vect_cross_product(&y_n_b, &z_n_b, &x_n_b);

	//save euler angles
	global_data.attitude.x = atan2f(z_n_b.y, z_n_b.z);
	global_data.attitude.y = -asinf(z_n_b.x);

	if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
	{
		global_data.attitude.z += 0.005f * global_data.gyros_si.z;
	}
	else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
	{
		global_data.attitude.z += 0.0049f * global_data.gyros_si.z;
		if (global_data.state.global_vision_attitude_new_data == 1)
		{
			global_data.attitude.z = 0.995f*global_data.attitude.z + 0.005f*global_data.vision_data_global.ang.z;

			// Reset new data flag at roughly 1 Hz, detecting a vision timeout
			static uint8_t new_data_reset = 0;

			if (new_data_reset == 0)
			{
				global_data.state.global_vision_attitude_new_data = 0;
			}
			new_data_reset++;
		}
	}
	else
	{
//		static hackMagLowpass = 0.0f;
		global_data.attitude.z = global_data.attitude.z*0.9f+0.1f*atan2f(y_n_b.x, x_n_b.x);
	}

	//save rates
	global_data.attitude_rate.x = kal_w.x;
	global_data.attitude_rate.y = kal_w.y;
	global_data.attitude_rate.z = kal_w.z;

	global_data.yaw_lowpass = 0.99f * global_data.yaw_lowpass + 0.01f * global_data.attitude_rate.z;
}
/**
* @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)

}
void update_system_statemachine(uint64_t loop_start_time)
{
	// Update state machine, enable and disable controllers
	switch (global_data.state.mav_mode)
	{
	case MAV_MODE_MANUAL:
		global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0;
		global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0;
		global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0;
		global_data.param[PARAM_MIX_OFFSET_WEIGHT] = 1;
		global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 1;
		break;
	case MAV_MODE_AUTO:
		// Same as guided mode, NO BREAK
	case MAV_MODE_TEST1:
		// Same as guided mode, NO BREAK
	case MAV_MODE_GUIDED:
		if (global_data.state.position_fix)
		{
			if (global_data.state.status == MAV_STATE_CRITICAL)
			{
				//get active again if we are in critical
				//if we are in Emergency we don't want to start again!!!!
				global_data.state.status = MAV_STATE_ACTIVE;
			}
		}
		else
		{
			if (global_data.state.status == MAV_STATE_ACTIVE)
			{
				global_data.state.status = MAV_STATE_CRITICAL;
				global_data.entry_critical = loop_start_time;
				debug_message_buffer("CRITICAL! POSITION LOST");
			}
			else if (global_data.state.status == MAV_STATE_CRITICAL && (loop_start_time - global_data.entry_critical
					> 3 * (uint32_t) global_data.param[PARAM_POSITION_TIMEOUT]))
			{
				//wait 3 times as long as waited for critical

				global_data.state.status = MAV_STATE_EMERGENCY;
				//Initiate Landing even if we didn't reach 0.7m
				global_data.state.fly=FLY_LANDING;
				debug_message_buffer("EMERGENCY! MAYDAY! MAYDAY! LANDING!");
			}
		}
		switch (global_data.state.status)
		{
		case MAV_STATE_ACTIVE:
			global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1;
			global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1;
			global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1;
			break;

		case MAV_STATE_CRITICAL:
			//global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1; // estimate path without vision
			global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0; // try to hover on place
			global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1;
			global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1;
			break;

		case MAV_STATE_EMERGENCY:
			global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0;
			global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1;
			global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1;
			break;
		default:
			global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0;
			global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0;
			global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0;
		}
		global_data.param[PARAM_MIX_OFFSET_WEIGHT] = 1;
		global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 1;
		break;
		case MAV_MODE_TEST2:
			//allow other mix params to be set by hand
			global_data.param[PARAM_MIX_OFFSET_WEIGHT] = 1;
			global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 1;
			break;
		case MAV_MODE_TEST3:
			//				break;

		case MAV_MODE_LOCKED:
			//				break;

		case MAV_MODE_READY:
			//				break;
		case MAV_MODE_UNINIT:
			//				break;
		default:
			global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0;
			global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0;
			global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0;
			global_data.param[PARAM_MIX_OFFSET_WEIGHT] = 0;
			global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 0;
	}
	if (global_data.state.remote_ok == 0)
	{
		global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 0;
	}
}
Exemple #15
0
/* Interrupt handler for I2C0 interrupt */
static void I2C0_ISR(void)
{

	//sprintf((char*)buffer, "h ");
	//message_send_debug(COMM_1, buffer);
	//uint8_t buffer[200];	// string buffer for debug messages
	ISR_ENTRY();

	if (error_counter0 > I2C_PERMANENT_ERROR_LIMIT)
	{
		//uint8_t buffer[50];
		//sprintf(buffer, "i2c error limit %d \n", 1);
		//  sprintf((char*)buffer, package_buffer0[i2c_package_buffer0_current_idx].slave_address) ;
		//message_send_debug(COMM_1, buffer);
		//		debug_message_buffer("i2c error limit reached");
		debug_message_buffer_sprintf("i2c0 error limit reached. Dest: %i",
				package_buffer0[i2c_package_buffer0_current_idx].slave_address);

		if (i2c0_permanent_error_count++ % 256 == 0)
		{
			debug_message_buffer_sprintf(
					"i2c0 error limit reached. Total: %i errors.",
					i2c0_permanent_error_count);
		}

		package_buffer0[i2c_package_buffer0_current_idx].i2c_error_code
				= I2C_CODE_ERROR;//set error code to error
		//TODO set this to ok if everzthing was ok

		if (package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler
				!= NULL)
		{ // check if there is a package handler registered
			package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler(
					&(package_buffer0[i2c_package_buffer0_current_idx])); // call package handler
		}

		i2c_package_buffer0_insert_idx = (i2c_package_buffer0_insert_idx + 1)
				% I2C_PACKAGE_BUFFER_SIZE; // increment package insertion pointer
		error_counter0 = 0;
		if (i2c_package_buffer0_current_idx == i2c_package_buffer0_insert_idx)
		{ // no unhandled packages
			I2C0CONCLR = 1 << STAC;
			I2C0CONSET = 1 << STO; // generate I2C stop condition on the bus without restart
			I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
			i2c0_busy = 0; // release I2C resource
		}
		else
		{ // unhandled packages in package buffer
			if (!(package_buffer0[i2c_package_buffer0_current_idx].write_read
					== 1))
			{
				I2C0CONSET = 1 << STO; // generate stop condition on the bus if no repeated start is necessary
			}
			I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10
			I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
		}
	}
	if (i2c0_busy == 1)
	{ //Return if Package error
		//message_debug_send(MAVLINK_COMM_1, 30, I2C0STAT);
		switch (I2C0STAT)
		{ // check current I2C0 state
		case 0x08: // I2C start condition has been generated on the bus -> transmit slave address and read/write bit -> next state: 0x18 or 0x40
			I2C0DAT
					= package_buffer0[i2c_package_buffer0_current_idx].slave_address
							| package_buffer0[i2c_package_buffer0_current_idx].direction;
			I2C0CONCLR = 1 << STAC; // do not restart (continue normal I2C operation)
			I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
			break;
		case 0x10: // "repeated start" condition has been generated on the bus -> transmit slave address and read/write bit -> next state: 0x18 or 0x40
			I2C0DAT
					= package_buffer0[i2c_package_buffer0_current_idx].slave_address
							| package_buffer0[i2c_package_buffer0_current_idx].direction;
			I2C0CONCLR = 1 << STAC;
			I2C0CONCLR = 1 << SIC;
			break;
		case 0x18: // slave address and write bit has been transmitted -> transmit first data byte -> next state: 0x28
			current_data_byte_i2c0 = 0;
			I2C0CONCLR = 1 << STAC;
			I2C0DAT
					= package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0];
			current_data_byte_i2c0++;
			I2C0CONCLR = 1 << SIC;
			break;
		case 0x28: // data byte has been transmitted
			// if there's another byte to be transmitted, do it
			if (current_data_byte_i2c0
					< package_buffer0[i2c_package_buffer0_current_idx].length)
			{
				I2C0CONCLR = 1 << STAC;
				I2C0DAT
						= package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0];
				current_data_byte_i2c0++;
				I2C0CONCLR = 1 << SIC;
			}
			// it the last byte has been transmitted, check if there are unhandled packages in the package buffer
			else
			{
				i2c_package_buffer0_current_idx
						= (i2c_package_buffer0_current_idx + 1)
								% I2C_PACKAGE_BUFFER_SIZE; // increment pointer to package in processing
				error_counter0 = 0;
				if (i2c_package_buffer0_current_idx
						== i2c_package_buffer0_insert_idx)
				{ // no unhandled packages
					I2C0CONCLR = 1 << STAC;
					I2C0CONSET = 1 << STO; // generate I2C stop condition on the bus without restart
					I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
					i2c0_busy = 0; // release I2C resource
				}
				else
				{ // unhandled packages in package buffer
					if (!(package_buffer0[i2c_package_buffer0_current_idx].write_read
							== 1))
					{
						I2C0CONSET = 1 << STO; // generate stop condition on the bus if no repeated start is necessary
					}
					I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10
					I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
				}
			}
			break;
		case 0x20: // I2C error state detection
			I2C0CONSET = 1 << STO;
			I2C0CONSET = 1 << STA; // restart I2C state machine with current package
			error_counter0++;
			//debug_message_buffer("I2C error: slave address not acknowledged (write)\n");
			debug_message_buffer_sprintf(
					"I2C0 error: slave address not acknowledged (write). Dest: %i",
					package_buffer0[i2c_package_buffer0_current_idx].slave_address);

			//message_send_debug(COMM_1, buffer);
			I2C0CONCLR = 1 << SIC;
			break;
		case 0x30: // I2C error state detection
			I2C0CONSET = 1 << STO;
			I2C0CONSET = 1 << STA; // restart I2C state machine with current package
			error_counter0++;
			//		debug_message_buffer("I2C error: data not acknowledged\n");
			debug_message_buffer_sprintf(
					"I2C0 error: data not acknowledged. Dest: %i",
					package_buffer0[i2c_package_buffer0_current_idx].slave_address);

			//message_send_debug(COMM_1, buffer);
			I2C0CONCLR = 1 << SIC;
			break;
		case 0x38: // I2C error state detection
			I2C0CONSET = 1 << STA; // restart I2C state machine with current package
			error_counter0++;
			//		debug_message_buffer("I2C error: arbitration lost\n");
			debug_message_buffer_sprintf(
					"I2C0 error: arbitration lost. Dest: %i",
					package_buffer0[i2c_package_buffer0_current_idx].slave_address);

			//message_send_debug(COMM_1, buffer);
			I2C0CONCLR = 1 << SIC;
			break;
		case 0x40: // slave address and read bit has been transmitted -> clear interrupt and wait for first data byte -> next state: 0x50 or 0x58
			current_data_byte_i2c0 = 0;
			if (package_buffer0[i2c_package_buffer0_current_idx].length > 1)
				I2C0CONSET = 1 << AA; // if there's more than one byte to be received -> next state: 0x50
			else
				I2C0CONCLR = 1 << AAC; // if there's only one byte to be received -> next state: 0x58
			I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
			break;
		case 0x50: // data byte has been received
			package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]
					= I2C0DAT; // copy data byte to data array in I2C package
			current_data_byte_i2c0++; // increment data byte
			if ((current_data_byte_i2c0 + 1)
					< package_buffer0[i2c_package_buffer0_current_idx].length)
			{ // there's more than one byte left to be received
				I2C0CONSET = 1 << AA; // acknowledge next data byte -> next state: 0x50
			}
			else
			{ // there's only one byte left to be received
				I2C0CONCLR = 1 << AAC; // do not acknowledge next data byte -> next state: 0x58
			}
			I2C0CONCLR = 1 << SIC;
			break;
		case 0x58: // last data byte has been received
			package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]
					= I2C0DAT; // copy data byte to data array in I2C package
			I2C0CONSET = 1 << STO; // generate STOP condition on the I2C bus
			//uart0_transmit(package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]);
			if (package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler
					!= NULL)
			{ // check if there is a package handler registered
				package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler(
						&(package_buffer0[i2c_package_buffer0_current_idx])); // call package handler
			}
			i2c_package_buffer0_current_idx = (i2c_package_buffer0_current_idx
					+ 1) % I2C_PACKAGE_BUFFER_SIZE; // increment pointer to package in processing
			error_counter0 = 0;
			// check if there are unhandled packages in the package buffer
			if (i2c_package_buffer0_current_idx
					== i2c_package_buffer0_insert_idx)
			{ // no unhandled packages
				I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
				i2c0_busy = 0; // release I2C resource
			}
			else
			{ // unhandled packages in package buffer
				I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10
				I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag
			}
			break;
		case 0x48: // I2C error state detection
			I2C0CONSET = 1 << STO;
			I2C0CONSET = 1 << STA; // restart I2C state machine with current package
			error_counter0++;
			debug_message_buffer(
					"I2C0 error: slave address not acknowledged (read)\n");
			//message_send_debug(COMM_1, buffer);
			I2C0CONCLR = 1 << SIC;
			break;

		default: // I2C error state detection
			I2C0CONSET = 1 << STO;
			I2C0CONSET = 1 << STA; // restart I2C state machine with current package
			error_counter0++;
			debug_message_buffer_sprintf("I2C0 error: undefined I2C state: %i",I2C0STAT);
			debug_message_buffer_sprintf("I2C0 error: prior state: %X",i2c0stat_prior_state);
			//		message_send_debug(COMM_1, buffer);
			I2C0CONCLR = 1 << SIC;
		}
	}

	// Sum up errors
	global_data.i2c0_err_count += error_counter0;

	VICVectAddr = 0x00000000; // clear this interrupt from the VIC
	ISR_EXIT();
	// exit ISR
}
Exemple #16
0
/**
 * @brief Set the current mode of operation
 * @param mode the new mode
 */
bool sys_set_mode(uint8_t mode)
{
	if (mode == MAV_MODE_AUTO)
	{
		global_data.state.mav_mode = MAV_MODE_AUTO;
		return true;
	}
	else if (mode == MAV_MODE_GUIDED)
	{
		global_data.state.mav_mode = MAV_MODE_GUIDED;
		return true;
	}
	else if (mode == MAV_MODE_LOCKED)
	{
		global_data.state.mav_mode = MAV_MODE_LOCKED;
		return true;
	}
	else if (mode == MAV_MODE_MANUAL)
	{
		global_data.state.mav_mode = MAV_MODE_MANUAL;
		return true;
	}
	else if (mode == MAV_MODE_READY)
	{
		global_data.state.mav_mode = MAV_MODE_READY;
		return true;
	}
	else if (mode == MAV_MODE_TEST1)
	{
		global_data.state.mav_mode = MAV_MODE_TEST1;
		return true;
	}
	else if (mode == MAV_MODE_TEST2)
	{
		global_data.state.mav_mode = MAV_MODE_TEST2;
		return true;
	}
	else if (mode == MAV_MODE_TEST3)
	{
		global_data.state.mav_mode = MAV_MODE_TEST3;
		return true;
	}
	else if (mode == MAV_MODE_RC_TRAINING)
	{
		// Only go into RC training if not flying
		if (! sys_state_is_flying())
		{
			global_data.state.mav_mode = MAV_MODE_RC_TRAINING;
			return true;
		}
		else
		{
			debug_message_buffer("WARNING: SYSTEM IS IN FLIGHT! Denied to switch to RC mode");
			return false;
		}
	}
	// UNINIT is not a mode that should be actively set
	// it will thus be rejected like any other invalid mode
	else
	{
		// No valid mode
		debug_message_buffer("WARNING: Attempted to set invalid mode");
		return false;
	}
}
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 Send one of the buffered messages
 * @param pos data from vision
 */
void vision_buffer_handle_data(mavlink_vision_position_estimate_t* pos)
{
	if (vision_buffer_index_write == vision_buffer_index_read)
	{
		//buffer empty
		return;

	}
	vision_buffer_index_read = (vision_buffer_index_read + 1)
			% VISION_BUFFER_COUNT;

	//TODO: find data and process it
	uint8_t for_count = 0;
	uint8_t i = vision_buffer_index_read;
	for (; (vision_buffer[i].time_captured < pos->usec)
			&& (vision_buffer_index_write - i != 1); i = (i + 1)
			% VISION_BUFFER_COUNT)
	{

		if (++for_count > VISION_BUFFER_COUNT)
		{
			debug_message_buffer("vision_buffer PREVENTED HANG");
			break;
		}
	}
	if (vision_buffer[i].time_captured == pos->usec)
	{

		//we found the right data
		if (!isnumber(pos->x) || !isnumber(pos->y) || !isnumber(pos->z)
				|| !isnumber(pos->roll) || !isnumber(pos->pitch) || !isnumber(pos->yaw)
				|| pos->x == 0.0 || pos->y == 0.0 || pos->z == 0.0)
		{
			//reject invalid data
			debug_message_buffer("vision_buffer invalid data (inf,nan,0) rejected");
		}
		else if (fabs(vision_buffer[i].ang.x - pos->roll)
				< global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD]
				&& fabs(vision_buffer[i].ang.y - pos->pitch)
						< global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD])
		{
			// Update validity time
			global_data.pos_last_valid = sys_time_clock_get_time_usec();

			//Pack new vision_data package
			global_data.vision_data.time_captured
					= vision_buffer[i].time_captured;
			global_data.vision_data.comp_end = sys_time_clock_get_unix_time();

			//Set data from Vision directly
			global_data.vision_data.ang.x = pos->roll;
			global_data.vision_data.ang.y = pos->pitch;
			global_data.vision_data.ang.z = pos->yaw;

			global_data.vision_data.pos.x = pos->x;
			global_data.vision_data.pos.y = pos->y;
			global_data.vision_data.pos.z = pos->z;

			// If yaw input from vision is enabled, feed vision
			// directly into state estimator
			global_data.vision_magnetometer_replacement.x = 200.0f*lookup_cos(pos->yaw);
			global_data.vision_magnetometer_replacement.y = -200.0f*lookup_sin(pos->yaw);
			global_data.vision_magnetometer_replacement.z = 0.f;

			//If yaw goes to infinity (no idea why) set it to setpoint, next time will be better
			if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559)
			{
				global_data.attitude.z = global_data.yaw_pos_setpoint;
				debug_message_buffer("vision_buffer CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
			}

			global_data.vision_data.new_data = 1;

			//TODO correct also all buffer data needed if we are going to have overlapping vision data
		}
		else
		{
			//rejected outlayer
			if (vision_buffer_reject_count++ % 16 == 0)
			{
				debug_message_buffer_sprintf("vision_buffer rejected outlier #%u",
						vision_buffer_reject_count);
			}
		}
		if (global_data.param[PARAM_SEND_SLOT_DEBUG_1] == 1)
		{

			//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 202, global_data.attitude.z);

			mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 210, pos->x);
			mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 211, pos->y);
			mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 212, pos->z);
			mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 215, pos->yaw);
			//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 212, pos.z);
			//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 203, pos.r1);
			//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 204, pos.confidence);
		}
		if (for_count)
		{
			debug_message_buffer_sprintf(
					"vision_buffer data found skipped %i data sets", for_count);
		}
	}
	else
	{
		//we didn't find it
		//		debug_message_buffer("vision_buffer data NOT found");
		if (for_count)
		{
			debug_message_buffer_sprintf(
					"vision_buffer data NOT found skipped %i data sets",
					for_count);
		}
	}
	vision_buffer_index_read = i;//skip all images that are older;
	//	if (for_count)
	//	{
	//		debug_message_buffer_sprintf("vision_buffer skipped %i data sets",
	//				for_count);
	//	}
}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_ground_car(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();

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


		///////////////////////////////////////////////////////////////////////////
		/// 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);


			// TODO READ OUT MOUSE SENSOR

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


		///////////////////////////////////////////////////////////////////////////
		/// 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();
			// 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))
		{
			// 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_remote_control();

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


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 20 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(50000, COUNTER7, loop_start_time))
		{
			led_toggle(LED_YELLOW);
			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)

}
Exemple #20
0
inline void remote_control(void)
{
	static uint32_t lossCounter = 0;
	if (global_data.state.mav_mode & (uint8_t) MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)
	{
		if (radio_control_status() == RADIO_CONTROL_ON)
		{
			global_data.state.remote_ok=1;
			if (lossCounter > 0)
			{
				debug_message_buffer("REGAINED REMOTE SIGNAL AFTER LOSS!");
			}
			lossCounter = 0;
			//get remote controll values
			float gas_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
					global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) - PPM_OFFSET);
			//	message_debug_send(MAVLINK_COMM_1, 12, gas_remote);
			float yaw_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) - PPM_CENTRE);
			//	message_debug_send(MAVLINK_COMM_1, 13, yaw_remote);
			float nick_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
					global_data.param[PARAM_PPM_NICK_CHANNEL]) - PPM_CENTRE);
			//	message_debug_send(MAVLINK_COMM_1, 12, gas_remote);
			float roll_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
					global_data.param[PARAM_PPM_ROLL_CHANNEL]) - PPM_CENTRE);
			//	message_debug_send(MAVLINK_COMM_1, 13, yaw_remote);
			//	if(radio_control_status()==RADIO_CONTROL_OFF){
			//		gas_remote=0.3;
			//		yaw_remote=0;
			//		nick_remote=0;
			//		roll_remote=0;
			//	}
			//MANUAL ATTITUDE CONTROL
			global_data.attitude_setpoint_remote.x = -roll_remote;
			global_data.attitude_setpoint_remote.y = -nick_remote;
			global_data.attitude_setpoint_remote.z = -5 * yaw_remote;// used as yaw speed!!! yaw offset1
			global_data.gas_remote = gas_remote;

			//MANUAL POSITION CONTROL
			//TODO


			//		Switch on MAV_STATE_ACTIVE
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) < PPM_LOW_TRIG))
			{


				//				//Reset YAW integration(only needed if no vision)
				//				global_data.attitude.z = 0;
				//				global_data.yaw_pos_setpoint = 0;

				if (global_data.state.status != MAV_STATE_ACTIVE)
				{
					debug_message_buffer("MAV_STATE_ACTIVE Motors started");

					//debug_message_buffer("CALIBRATING GYROS");
					//start_gyro_calibration();
				}
				//switch on motors
				global_data.state.status = MAV_STATE_ACTIVE;
				global_data.state.fly = FLY_WAIT_MOTORS;
				global_data.state.mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
				//this will be done by setpoint
				if (global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED)
				{
					global_data.state.fly = FLY_FLYING;
				}
			}

			//		Switch off MAV_STATE_STANDBY
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG))
			{
				if (global_data.state.status != MAV_STATE_STANDBY)
				{
					debug_message_buffer("MAV_STATE_STANDBY Motors off");
				}
				//switch off motors
				global_data.state.status = MAV_STATE_STANDBY;
				global_data.state.fly = FLY_GROUNDED;
				global_data.state.mav_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;

			}

			// Start Gyro calibration left stick: left down. right stick right down.
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG)
					&& (ppm_get_channel(
							global_data.param[PARAM_PPM_NICK_CHANNEL])
							< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG))
			{
				start_gyro_calibration();
			}

			// Start capture: left down. right stick right up.
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG)
					&& (ppm_get_channel(
							global_data.param[PARAM_PPM_NICK_CHANNEL])
							> PPM_HIGH_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG))
			{
				// FIXME LORENZ CAPTURE START
				//should actually go to capturer not IMU
			}
			// Stop capture: left down. right stick right up.
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG)
					&& (ppm_get_channel(
							global_data.param[PARAM_PPM_NICK_CHANNEL])
							> PPM_HIGH_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_ROLL_CHANNEL]) > PPM_HIGH_TRIG))
			{
				// FIXME LORENZ CAPTURE END
				//should actually go to capturer not IMU
			}

			if (global_data.state.mav_mode & MAV_MODE_FLAG_GUIDED_ENABLED)
			{
				// Reset position to 0,0, mostly useful for optical flow with setpoint at 0,0
				if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE4_CHANNEL])
						< PPM_LOW_TRIG)
				{
					global_data.position.x = 0;
					global_data.position.y = 0;
				}
			}

			if (global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED)
			{

				if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE1_CHANNEL])
						< PPM_LOW_TRIG)
				{
					//z-controller disabled
					global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0;
				}
				else
				{
					//z-controller enabled
					global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1;
				}

				if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE4_CHANNEL])
						< PPM_LOW_TRIG)
				{
					//low - Position Hold off
					global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0;
					global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0;

					global_data.position.x = 0;
					global_data.position.y = 0;
				}
				else if (ppm_get_channel(
						global_data.param[PARAM_PPM_TUNE4_CHANNEL])
						> PPM_HIGH_TRIG)
				{
					//high - Position Hold with setpoint movement TODO
					global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1;
					global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1;
				}
				else
				{
					//center - Position Hold
					global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1;
					global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1;
				}

			}

			//			//Integrate YAW setpoint
			//			if (fabs(
			//					(ppm_get_channel(global_data.param[PARAM_PPM_GIER_CHANNEL])
			//							- PPM_CENTRE)) > 10)
			//			{
			//				global_data.yaw_pos_setpoint -= 0.02 * 0.03
			//						* (ppm_get_channel(
			//								global_data.param[PARAM_PPM_GIER_CHANNEL])
			//								- PPM_CENTRE);
			//			}
			//		Set PID VALUES

			float tune2 = 1 * (ppm_get_channel(
					global_data.param[PARAM_PPM_TUNE2_CHANNEL]) - 1109);
			float tune3 = 1 * (ppm_get_channel(
					global_data.param[PARAM_PPM_TUNE3_CHANNEL]) - 1109);
			if (tune2 < 0)
			{
				tune2 = 0;
			}
			if (tune2 > 1000)
			{
				tune2 = 1000;
			}

			if (tune3 < 0)
			{
				tune3 = 0;
			}
			if (tune3 > 1000)
			{
				tune3 = 1000;
			}

			//TUNING FOR TOBIS REMOTE CONTROL

			if (global_data.param[PARAM_TRIMCHAN] == 1)
			{
								global_data.param[PARAM_PID_ATT_P] = 0.1 * tune2;
//								global_data.param[PARAM_PID_ATT_I] = 0;
								global_data.param[PARAM_PID_ATT_D] = 0.1 * tune3;
			}
			else if (global_data.param[PARAM_TRIMCHAN] == 2)
			{
								global_data.param[PARAM_PID_POS_P] = 0.01 * tune2;
//								global_data.param[PARAM_PID_POS_I] = 0;
								global_data.param[PARAM_PID_POS_D] = 0.01 * tune3;
			}
			else if (global_data.param[PARAM_TRIMCHAN] == 3)
			{
								global_data.param[PARAM_PID_POS_Z_P] = 0.01 * tune2;
//								global_data.param[PARAM_PID_POS_Z_I] = 0;
								global_data.param[PARAM_PID_POS_Z_D] = 0.01 * tune3;
			}
			else if (global_data.param[PARAM_TRIMCHAN] == 4)
			{
								global_data.param[PARAM_PID_YAWSPEED_P] = 0.1 * tune2;
//								global_data.param[PARAM_PID_YAWSPEED_I] = 0;
								global_data.param[PARAM_PID_YAWSPEED_D] = 0.1 * tune3;
			}
			else if (global_data.param[PARAM_TRIMCHAN] == 5)
			{
								global_data.param[PARAM_PID_YAWPOS_P] = 0.1 * tune2;
//								global_data.param[PARAM_PID_YAWPOS_I] = 0;
								global_data.param[PARAM_PID_YAWPOS_D] = 0.1 * tune3;
			}
			//this is done at 10 Hz
			//			pid_set_parameters(&nick_controller,
			//					global_data.param[PARAM_PID_ATT_P],
			//					global_data.param[PARAM_PID_ATT_I],
			//					global_data.param[PARAM_PID_ATT_D],
			//					global_data.param[PARAM_PID_ATT_AWU]);
			//			pid_set_parameters(&roll_controller,
			//					global_data.param[PARAM_PID_ATT_P],
			//					global_data.param[PARAM_PID_ATT_I],
			//					global_data.param[PARAM_PID_ATT_D],
			//					global_data.param[PARAM_PID_ATT_AWU]);
			//
			//			pid_set_parameters(&x_axis_controller,
			//					global_data.param[PARAM_PID_POS_P],
			//					global_data.param[PARAM_PID_POS_I],
			//					global_data.param[PARAM_PID_POS_D],
			//					global_data.param[PARAM_PID_POS_AWU]);
			//			pid_set_parameters(&y_axis_controller,
			//					global_data.param[PARAM_PID_POS_P],
			//					global_data.param[PARAM_PID_POS_I],
			//					global_data.param[PARAM_PID_POS_D],
			//					global_data.param[PARAM_PID_POS_AWU]);

			//global_data.param_name[PARAM_MIX_REMOTE_WEIGHT] = 1;// add position only keep - tune3;
			//global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1;
			if (global_data.param[PARAM_PPM_TUNE1_CHANNEL] != -1)
			{
				global_data.param[PARAM_CAM_ANGLE_Y_OFFSET]
						= ((float) (ppm_get_channel(
								global_data.param[PARAM_PPM_TUNE1_CHANNEL]))
								- 1000.0f) / 1000.0f;
			}
		}
		else
		{
			//No Remote signal
			lossCounter++;

			if (global_data.state.remote_ok == 1)
			{
				//Wait one round and start sinking
				global_data.state.remote_ok = 0;
				debug_message_buffer("No remote signal (1st time)");
			}
			else
			{
				static uint16_t countdown;
				//already the second time
				// Emergency Landing
				if (global_data.state.fly != FLY_GROUNDED
						&& global_data.state.fly != FLY_RAMP_DOWN && global_data.state.fly != FLY_LANDING)
				{
					debug_message_buffer_sprintf("global_data.state.fly=%i",global_data.state.fly);
					sys_set_state(MAV_STATE_EMERGENCY);
					global_data.state.fly = FLY_LANDING;//start landing
					debug_message_buffer(
							"EMERGENCY LANDING STARTED. No remote signal");

					countdown = 50 * 5; // 5 seconds
				}
				else
				{
					if (global_data.state.fly == FLY_GROUNDED || global_data.state.fly == FLY_WAIT_MOTORS)
					{
						if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED)
						{

							if (lossCounter < 5)
							{
								debug_message_buffer(
										"EMERGENCY LANDING FINISHED. No remote signal");
								debug_message_buffer(
										"EMERGENCY LANDING NOW LOCKED");
							}
						}

						// Set to disarmed
						sys_set_mode(global_data.state.mav_mode & ~MAV_MODE_FLAG_SAFETY_ARMED);
						sys_set_state(MAV_STATE_STANDBY);
					}
					else
					{
						//won't come here anymore if once in locked mode
						debug_message_buffer(
								"EMERGENCY LANDING. No remote signal");

					}
				}

				if((global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED) && global_data.state.fly != FLY_GROUNDED){

					//z-controller enabled
					global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1;
					global_data.position_setpoint.z = global_data.position.z
							+ 0.01;
					global_data.param[PARAM_POSITION_SETPOINT_Z]
							= global_data.position.z + 0.01;
					if (countdown-- <= 1)
					{
						global_data.state.fly = FLY_GROUNDED;
						//global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0;
						debug_message_buffer(
								"EMERGENCY LANDING Z control finished");
					}
				}
			}
		}

	}

}
/**
 * @brief Send one of the buffered messages
 * @param pos data from vision
 */
void vision_buffer_handle_global_data(mavlink_global_vision_position_estimate_t* pos)
{
	//#define PROJECT_GLOBAL_DATA_FORWARD

	#ifdef PROJECT_GLOBAL_DATA_FORWARD

	if (vision_buffer_index_write == vision_buffer_index_read)
	{
		//buffer empty
		debug_message_buffer("ERROR VIS BUF: buffer empty");
		return;
	}
	vision_buffer_index_read = (vision_buffer_index_read + 1) % VISION_BUFFER_COUNT;

	//TODO: find data and process it
	uint8_t for_count = 0;
	uint8_t i = vision_buffer_index_read;
	for (; (vision_buffer[i].time_captured < pos->usec)
			&& (vision_buffer_index_write - i != 1); i = (i + 1)
			% VISION_BUFFER_COUNT)
	{
		if (++for_count > VISION_BUFFER_COUNT) break;
	}

	if (vision_buffer[i].time_captured == pos->usec)
	{

		//we found the right data
		if (!isnumber(pos->x) || !isnumber(pos->y) || !isnumber(pos->z)
				|| !isnumber(pos->roll) || !isnumber(pos->pitch) || !isnumber(pos->yaw)
				|| pos->x == 0.0 || pos->y == 0.0 || pos->z == 0.0)
		{
			//reject invalid data
			debug_message_buffer("ERROR VIS BUF: invalid data (inf,nan,0) rejected");
		}
		else if (fabs(vision_buffer[i].ang.x - pos->roll) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD]
				&& fabs(vision_buffer[i].ang.y - pos->pitch) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD])
		{
			// Update validity time
			global_data.pos_last_valid = sys_time_clock_get_time_usec();

			//Pack new vision_data package
			global_data.vision_data_global.time_captured
					= vision_buffer[i].time_captured;
			global_data.vision_data_global.comp_end = sys_time_clock_get_unix_time();

			// FIXME currently visodo is not allowed to run in parallel, else race condititions!

			// Project position measurement
			global_data.vision_data_global.pos.x = pos->x + (global_data.position.x - vision_buffer[i].pos.x);
			global_data.vision_data_global.pos.y = pos->y + (global_data.position.y - vision_buffer[i].pos.y);
			global_data.vision_data_global.pos.z = pos->z + (global_data.position.z - vision_buffer[i].pos.z);

			// Set roll and pitch absolutely
			global_data.vision_data_global.ang.x = pos->roll;
			global_data.vision_data_global.ang.y = pos->pitch;
			global_data.vision_data_global.ang.z = pos->yaw;

			// Project yaw
			//global_data.vision_data_global.ang.z = pos->yaw-vision_buffer[i].ang.z;

			for (uint8_t j = (i+1) % VISION_BUFFER_COUNT; j != i; j = (j + 1) % VISION_BUFFER_COUNT)
			{
				vision_buffer[j].pos.x = vision_buffer[j].pos.x + (pos->x - vision_buffer[i].pos.x);
				vision_buffer[j].pos.y = vision_buffer[j].pos.y + (pos->y - vision_buffer[i].pos.y);
				vision_buffer[j].pos.z = vision_buffer[j].pos.z + (pos->z - vision_buffer[i].pos.z);
			}


			// If yaw input from vision is enabled, feed vision
			// directly into state estimator
			float lpYaw = pos->yaw*0.5f+global_data.attitude.z*0.5f;
			global_data.vision_global_magnetometer_replacement.x = 200.0f*lookup_cos(lpYaw);
			global_data.vision_global_magnetometer_replacement.y = -200.0f*lookup_sin(lpYaw);
			global_data.vision_global_magnetometer_replacement.z = 0.f;

			//If yaw goes to infinity (no idea why) set it to setpoint, next time will be better
			if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559)
			{
				global_data.attitude.z = global_data.yaw_pos_setpoint;
				debug_message_buffer("vision_buffer CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
			}

			global_data.vision_data_global.new_data = 1;
			global_data.state.global_vision_attitude_new_data = 1;
			debug_message_buffer_sprintf("vision_buffer data found skipped %i data sets", for_count);
			//TODO correct also all buffer data needed if we are going to have overlapping vision data

			if (global_data.param[PARAM_SEND_SLOT_DEBUG_1] == 1)
			{

				//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 202, global_data.attitude.z);

				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 220, pos->x);
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 221, pos->y);
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 222, pos->z);
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 225, pos->yaw);
				//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 212, pos.z);
				//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 203, pos.r1);
				//mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 204, pos.confidence);
			}
		}
		else
		{
			//rejected outlier
			//if (vision_buffer_reject_count++ % 16 == 0)
			{
				debug_message_buffer_sprintf("vision_buffer rejected outlier #%u",
						vision_buffer_reject_count);
			}
		}
	}
	else
	{
		//we didn't find it
		debug_message_buffer_sprintf("vision_buffer data NOT found skipped %i data sets", for_count);
	}
	vision_buffer_index_read = i;//skip all images that are older;
#else
	global_data.vision_data_global.pos.x = pos->x;
	global_data.vision_data_global.pos.y = pos->y;
	global_data.vision_data_global.pos.z = pos->z;

	//Set data from Vision directly
	global_data.vision_data_global.ang.x = pos->roll;
	global_data.vision_data_global.ang.y = pos->pitch;
	global_data.vision_data_global.ang.z = pos->yaw;

	global_data.vision_data_global.new_data = 1;
	global_data.state.global_vision_attitude_new_data = 1;
#endif

	mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(), global_data.vision_data_global.pos.x, global_data.vision_data_global.pos.y, global_data.vision_data_global.pos.z, global_data.vision_data_global.ang.x, global_data.vision_data_global.ang.y, global_data.vision_data_global.ang.z);
}
//@{
void handle_mavlink_message(mavlink_channel_t chan,
		mavlink_message_t* msg)
{
	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
	uint32_t len;
	switch (chan)
	{
	case MAVLINK_COMM_0:
	{
		if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE)
		{
			// Copy to COMM 1
			len = mavlink_msg_to_send_buffer(buf, msg);
			for (int i = 0; i < len; i++)
			{
				uart1_transmit(buf[i]);
			}
		}
	}
	break;
	case MAVLINK_COMM_1:
	{
		if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE && msg->msgid != MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE)
		{
			// Copy to COMM 0
			len = mavlink_msg_to_send_buffer(buf, msg);
			for (int i = 0; i < len; i++)
			{
				uart0_transmit(buf[i]);
			}
			break;
		}
	}
	default:
		break;
	}


	switch (msg->msgid)
	{
	case MAVLINK_MSG_ID_SET_MODE:
	{
		mavlink_set_mode_t mode;
		mavlink_msg_set_mode_decode(msg, &mode);
		// Check if this system should change the mode
		if (mode.target == (uint8_t)global_data.param[PARAM_SYSTEM_ID])
		{
			sys_set_mode(mode.mode);

			// Emit current mode
			mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
					global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
					global_data.motor_block, communication_get_uart_drop_rate());
			mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
					global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
					global_data.motor_block, communication_get_uart_drop_rate());

		}
	}
	break;
	case MAVLINK_MSG_ID_ACTION:
	{
		execute_action(mavlink_msg_action_get_action(msg));

		//Forwart actions from Xbee to Onboard Computer and vice versa
		if (chan == MAVLINK_COMM_1)
		{
			mavlink_send_uart(MAVLINK_COMM_0, msg);
		}
		else if (chan == MAVLINK_COMM_0)
		{
			mavlink_send_uart(MAVLINK_COMM_1, msg);
		}
	}
	break;
	case MAVLINK_MSG_ID_SYSTEM_TIME:
	{
		if (!sys_time_clock_get_unix_offset())
		{
			int64_t offset = ((int64_t) mavlink_msg_system_time_get_time_usec(
					msg)) - (int64_t) sys_time_clock_get_time_usec();
			sys_time_clock_set_unix_offset(offset);

			debug_message_buffer("UNIX offset updated");
		}
		else
		{

			//			debug_message_buffer("UNIX offset REFUSED");
		}
	}
	break;
	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
	{
		mavlink_request_data_stream_t stream;
		mavlink_msg_request_data_stream_decode(msg, &stream);
		switch (stream.req_stream_id)
		{
		case 0: // UNIMPLEMENTED
			break;
		case 1: // RAW SENSOR DATA
			global_data.param[PARAM_SEND_SLOT_RAW_IMU] = stream.start_stop;
			break;
		case 2: // EXTENDED SYSTEM STATUS
			global_data.param[PARAM_SEND_SLOT_ATTITUDE] = stream.start_stop;
			break;
		case 3: // REMOTE CONTROL CHANNELS
			global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] = stream.start_stop;
			break;
		case 4: // RAW CONTROLLER
			//global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
			//global_data.param[PARAM_SEND_SLOT_DEBUG_3] = stream.start_stop;
			global_data.param[PARAM_SEND_SLOT_CONTROLLER_OUTPUT] = stream.start_stop;
			break;
		case 5: // SENSOR FUSION

			//LOST IN GROUDNCONTROL
			//			global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
			break;
		case 6: // POSITION
			global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop;
			break;
		case 7: // EXTRA1
			global_data.param[PARAM_SEND_SLOT_DEBUG_2] = stream.start_stop;
			break;
		case 8: // EXTRA2
			global_data.param[PARAM_SEND_SLOT_DEBUG_4] = stream.start_stop;
			break;
		case 9: // EXTRA3
			global_data.param[PARAM_SEND_SLOT_DEBUG_6] = stream.start_stop;
			break;
		default:
			// Do nothing
			break;
		}
	}
	break;
	case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
	{
		mavlink_param_request_read_t set;
		mavlink_msg_param_request_read_decode(msg, &set);

		// Check if this message is for this system
		if ((uint8_t) set.target_system
				== (uint8_t) global_data.param[PARAM_SYSTEM_ID]
				                               && (uint8_t) set.target_component
				                               == (uint8_t) global_data.param[PARAM_COMPONENT_ID])
		{
			char* key = (char*) set.param_id;

			if (set.param_id[0] == '\0')
			{
				// Choose parameter based on index
				if (set.param_index < ONBOARD_PARAM_COUNT)
				{
					// Report back value
					mavlink_msg_param_value_send(chan,
							(int8_t*) global_data.param_name[set.param_index],
							global_data.param[set.param_index], ONBOARD_PARAM_COUNT, set.param_index);
				}
			}
			else
			{
				for (int i = 0; i < ONBOARD_PARAM_COUNT; i++)
				{
					bool match = true;
					for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
					{
						// Compare
						if (((char) (global_data.param_name[i][j]))
								!= (char) (key[j]))
						{
							match = false;
						}

						// End matching if null termination is reached
						if (((char) global_data.param_name[i][j]) == '\0')
						{
							break;
						}
					}

					// Check if matched
					if (match)
					{
						// Report back value
						mavlink_msg_param_value_send(chan,
								(int8_t*) global_data.param_name[i],
								global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
					}
				}
			}
		}
	}
	break;
	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
	{
		// Start sending parameters
		m_parameter_i = 0;
	}
	break;
	case MAVLINK_MSG_ID_PARAM_SET:
	{
		mavlink_param_set_t set;
		mavlink_msg_param_set_decode(msg, &set);

		// Check if this message is for this system
		if ((uint8_t) set.target_system
				== (uint8_t) global_data.param[PARAM_SYSTEM_ID]
				                               && (uint8_t) set.target_component
				                               == (uint8_t) global_data.param[PARAM_COMPONENT_ID])
		{
			char* key = (char*) set.param_id;

			for (int i = 0; i < ONBOARD_PARAM_COUNT; i++)
			{
				bool match = true;
				for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++)
				{
					// Compare
					if (((char) (global_data.param_name[i][j]))
							!= (char) (key[j]))
					{
						match = false;
					}

					// End matching if null termination is reached
					if (((char) global_data.param_name[i][j]) == '\0')
					{
						break;
					}
				}

				// Check if matched
				if (match)
				{
					// Only write and emit changes if there is actually a difference
					// AND only write if new value is NOT "not-a-number"
					// AND is NOT infy
					if (global_data.param[i] != set.param_value
							&& !isnan(set.param_value)
							&& !isinf(set.param_value))
					{
						global_data.param[i] = set.param_value;
						// Report back new value
						mavlink_msg_param_value_send(MAVLINK_COMM_0,
								(int8_t*) global_data.param_name[i],
								global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);
						mavlink_msg_param_value_send(MAVLINK_COMM_1,
								(int8_t*) global_data.param_name[i],
								global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i);

						debug_message_buffer_sprintf("Parameter received param id=%i",i);
					}
				}
			}
		}
	}
	break;
	case MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET:
	{
		mavlink_position_control_setpoint_set_t pos;
		mavlink_msg_position_control_setpoint_set_decode(msg, &pos);
		if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1)
		{
			//			global_data.position_setpoint.x = pos.x;
			//			global_data.position_setpoint.y = pos.y;
			//			global_data.position_setpoint.z = pos.z;
			debug_message_buffer("Position setpoint updated. OLD?\n");
		}
		else
		{
			debug_message_buffer(
					"Position setpoint recieved but not updated. OLD?\n");
		}

		// Send back a message confirming the new position
		mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_0, pos.id,
				pos.x, pos.y, pos.z, pos.yaw);
		mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_1, pos.id,
				pos.x, pos.y, pos.z, pos.yaw);
	}
	break;
	case MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET:
	{
		mavlink_position_control_offset_set_t set;
		mavlink_msg_position_control_offset_set_decode(msg, &set);
		//global_data.attitude_setpoint_pos_body_offset.z = set.yaw;

		//Ball Tracking
		if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1 && global_data.param[PARAM_POSITION_YAW_TRACKING]==1)
		{
			global_data.param[PARAM_POSITION_SETPOINT_YAW]
			                  = global_data.attitude.z + set.yaw;

			mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 92, set.yaw);
		}
	}
	break;
	case MAVLINK_MSG_ID_SET_CAM_SHUTTER:
	{
		// Decode the desired shutter
		mavlink_set_cam_shutter_t cam;
		mavlink_msg_set_cam_shutter_decode(msg, &cam);
		shutter_set(cam.interval, cam.exposure);
		debug_message_buffer_sprintf("set_cam_shutter. interval %i",
				cam.interval);

	}
	break;
	case MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL:
	{
		uint8_t enable = mavlink_msg_image_trigger_control_get_enable(msg);
		shutter_control(enable);
		if (enable)
		{
			debug_message_buffer("CAM: Enabling hardware trigger");
		}
		else
		{
			debug_message_buffer("CAM: Disabling hardware trigger");
		}
	}
	break;
	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
	{
		mavlink_vision_position_estimate_t pos;
		mavlink_msg_vision_position_estimate_decode(msg, &pos);

		vision_buffer_handle_data(&pos);
		// Update validity time is done in vision buffer

	}
	break;
	case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
	{
		mavlink_vicon_position_estimate_t pos;
		mavlink_msg_vicon_position_estimate_decode(msg, &pos);

		global_data.vicon_data.x = pos.x;
		global_data.vicon_data.y = pos.y;
		global_data.vicon_data.z = pos.z;
		global_data.state.vicon_new_data=1;
		// Update validity time
		global_data.vicon_last_valid = sys_time_clock_get_time_usec();
		global_data.state.vicon_ok=1;

//		//Set data from Vicon into vision filter
//		global_data.vision_data.ang.x = pos.roll;
//		global_data.vision_data.ang.y = pos.pitch;
//		global_data.vision_data.ang.z = pos.yaw;
//
//		global_data.vision_data.pos.x = pos.x;
//		global_data.vision_data.pos.y = pos.y;
//		global_data.vision_data.pos.z = pos.z;
//
//		global_data.vision_data.new_data = 1;

		if (!global_data.state.vision_ok)
		{
			//Correct YAW
			global_data.attitude.z = pos.yaw;
			//If yaw goes to infy (no idea why) set it to setpoint, next time will be better
			if (global_data.attitude.z > 18.8495559 || global_data.attitude.z
					< -18.8495559)
			{
				global_data.attitude.z = global_data.yaw_pos_setpoint;
				debug_message_buffer(
						"vicon CRITICAL FAULT yaw was bigger than 6 PI! prevented crash");
			}
		}


		//send the vicon message to UART0 with new timestamp
		mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_0, global_data.vicon_last_valid, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);

	}
	break;
	case MAVLINK_MSG_ID_PING:
	{
		mavlink_ping_t ping;
		mavlink_msg_ping_decode(msg, &ping);
		if (ping.target_system == 0 && ping.target_component == 0)
		{
			// Respond to ping
			uint64_t r_timestamp = sys_time_clock_get_unix_time();
			mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp);
		}
	}
	break;
	case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
	{
		mavlink_local_position_setpoint_set_t sp;
		mavlink_msg_local_position_setpoint_set_decode(msg, &sp);
		if (sp.target_system == global_data.param[PARAM_SYSTEM_ID])
		{
			if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1)
			{
				if (sp.x >= global_data.position_setpoint_min.x && sp.y
						>= global_data.position_setpoint_min.y && sp.z
						>= global_data.position_setpoint_min.z && sp.x
						<= global_data.position_setpoint_max.x && sp.y
						<= global_data.position_setpoint_max.y && sp.z
						<= global_data.position_setpoint_max.z)
				{
					debug_message_buffer("Setpoint accepted and set.");
					global_data.param[PARAM_POSITION_SETPOINT_X] = sp.x;
					global_data.param[PARAM_POSITION_SETPOINT_Y] = sp.y;
					global_data.param[PARAM_POSITION_SETPOINT_Z] = sp.z;

					if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 0)
					{
						// Only update yaw if we are not tracking ball.
						global_data.param[PARAM_POSITION_SETPOINT_YAW] = sp.yaw;
					}

					//check if we want to start or land
					if (global_data.state.status == MAV_STATE_ACTIVE
							|| global_data.state.status == MAV_STATE_CRITICAL)
					{
						if (sp.z > -0.1)
						{
							if (!(global_data.state.fly == FLY_GROUNDED
									|| global_data.state.fly == FLY_SINKING
									|| global_data.state.fly
											== FLY_WAIT_LANDING
									|| global_data.state.fly == FLY_LANDING
									|| global_data.state.fly == FLY_RAMP_DOWN))
							{
								//if setpoint is lower that ground iate landing
								global_data.state.fly = FLY_SINKING;
								global_data.param[PARAM_POSITION_SETPOINT_Z]
										= -0.2;//with lowpass
								debug_message_buffer(
										"Sinking for LANDING. (z-sp lower than 10cm)");
							}
							else if (!(global_data.state.fly == FLY_GROUNDED))
							{
								global_data.param[PARAM_POSITION_SETPOINT_Z]
										= -0.2;//with lowpass
							}
						}
						else if (global_data.state.fly == FLY_GROUNDED && sp.z
								< -0.50)
						{
							//start if we were grounded and get a sp over 0.5m
							global_data.state.fly = FLY_WAIT_MOTORS;
							debug_message_buffer(
									"STARTING wait motors. (z-sp higher than 50cm)");
							//set point changed with lowpass, after 5s it will be ok.
						}
					}

					//SINK TO 0.7m if we are critical or emergency
					if (global_data.state.status == MAV_STATE_EMERGENCY
							|| global_data.state.status == MAV_STATE_CRITICAL)
					{
						global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.7;//with lowpass
					}
				}
				else
				{
					debug_message_buffer("Setpoint refused. Out of range.");
				}
			}
			else
			{
				debug_message_buffer("Setpoint refused. Param setpoint accept=0.");
			}
		}
	}
	break;
	default:
		break;
	}
}
inline void remote_control(void)
{

	if (global_data.state.mav_mode == (uint8_t) MAV_MODE_MANUAL || global_data.state.mav_mode
			== (uint8_t) MAV_MODE_GUIDED || global_data.state.mav_mode
			== (uint8_t) MAV_MODE_AUTO)
	{
		if (radio_control_status() == RADIO_CONTROL_ON)
		{
			global_data.state.remote_ok=1;
			//get remote controll values
			float gas_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
					global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) - PPM_OFFSET);
			//	message_debug_send(MAVLINK_COMM_1, 12, gas_remote);
			float yaw_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) - PPM_CENTRE);
			//	message_debug_send(MAVLINK_COMM_1, 13, yaw_remote);
			float nick_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
					global_data.param[PARAM_PPM_NICK_CHANNEL]) - PPM_CENTRE);
			//	message_debug_send(MAVLINK_COMM_1, 12, gas_remote);
			float roll_remote = PPM_SCALE_FACTOR * (ppm_get_channel(
					global_data.param[PARAM_PPM_ROLL_CHANNEL]) - PPM_CENTRE);
			//	message_debug_send(MAVLINK_COMM_1, 13, yaw_remote);
			//	if(radio_control_status()==RADIO_CONTROL_OFF){
			//		gas_remote=0.3;
			//		yaw_remote=0;
			//		nick_remote=0;
			//		roll_remote=0;
			//	}
			//MANUAL ATTITUDE CONTROL
			global_data.attitude_setpoint_remote.x = -roll_remote;
			global_data.attitude_setpoint_remote.y = -nick_remote;
			global_data.attitude_setpoint_remote.z = -5 * yaw_remote;// used as yaw speed!!! yaw offset1
			global_data.gas_remote = gas_remote;

			//MANUAL POSITION CONTROL
			//TODO


			//		Switch on MAV_STATE_ACTIVE
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) < PPM_LOW_TRIG))
			{


				//				//Reset YAW integration(only needed if no vision)
				//				global_data.attitude.z = 0;
				//				global_data.yaw_pos_setpoint = 0;

				if (global_data.state.status != MAV_STATE_ACTIVE)
				{
					debug_message_buffer("MAV_STATE_ACTIVE Motors started");
				}
				//switch on motors
				global_data.state.status = MAV_STATE_ACTIVE;
//				global_data.state.fly = FLY_WAIT_MOTORS;
//				this will be done by setpoint
			}

			//		Switch off MAV_STATE_STANDBY
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG))
			{
				if (global_data.state.status != MAV_STATE_STANDBY)
				{
					debug_message_buffer("MAV_STATE_STANDBY Motors off");
				}
				//switch off motors
				global_data.state.status = MAV_STATE_STANDBY;


			}

			// Start Gyro calibration left stick: left down. right stick right down.
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG)
					&& (ppm_get_channel(
							global_data.param[PARAM_PPM_NICK_CHANNEL])
							< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG))
			{
				start_gyro_calibration();
			}

			// Start capture: left down. right stick right up.
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG)
					&& (ppm_get_channel(
							global_data.param[PARAM_PPM_NICK_CHANNEL])
							> PPM_HIGH_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG))
			{
				mavlink_msg_action_send(
						global_data.param[PARAM_SEND_DEBUGCHAN],
						global_data.param[PARAM_SYSTEM_ID],MAV_COMP_ID_IMU,
						MAV_ACTION_REC_START);
				//should actually go to capturer not IMU
			}
			// Stop capture: left down. right stick right up.
			if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL])
					< PPM_LOW_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG)
					&& (ppm_get_channel(
							global_data.param[PARAM_PPM_NICK_CHANNEL])
							> PPM_HIGH_TRIG) && (ppm_get_channel(
					global_data.param[PARAM_PPM_ROLL_CHANNEL]) > PPM_HIGH_TRIG))
			{
				mavlink_msg_action_send(
						global_data.param[PARAM_SEND_DEBUGCHAN],
						global_data.param[PARAM_SYSTEM_ID], MAV_COMP_ID_IMU,
						MAV_ACTION_REC_STOP);
				//should actually go to capturer not IMU
			}









			//			//Integrate YAW setpoint
			//			if (fabs(
			//					(ppm_get_channel(global_data.param[PARAM_PPM_GIER_CHANNEL])
			//							- PPM_CENTRE)) > 10)
			//			{
			//				global_data.yaw_pos_setpoint -= 0.02 * 0.03
			//						* (ppm_get_channel(
			//								global_data.param[PARAM_PPM_GIER_CHANNEL])
			//								- PPM_CENTRE);
			//			}
			//		Set PID VALUES

			float tune2 = 1 * (ppm_get_channel(
					global_data.param[PARAM_PPM_TUNE2_CHANNEL]) - 1109);
			float tune3 = 1 * (ppm_get_channel(
					global_data.param[PARAM_PPM_TUNE3_CHANNEL]) - 1109);
			if (tune2 < 0)
			{
				tune2 = 0;
			}
			if (tune2 > 1000)
			{
				tune2 = 1000;
			}

			if (tune3 < 0)
			{
				tune3 = 0;
			}
			if (tune3 > 1000)
			{
				tune3 = 1000;
			}

			//TUNING FOR TOBIS REMOTE CONTROL

			if (global_data.param[PARAM_TRIMCHAN] == 1)
			{
								global_data.param[PARAM_PID_ATT_P] = 0.1 * tune2;
//								global_data.param[PARAM_PID_ATT_I] = 0;
								global_data.param[PARAM_PID_ATT_D] = 0.1 * tune3;
			}
			else if (global_data.param[PARAM_TRIMCHAN] == 2)
			{
								global_data.param[PARAM_PID_POS_P] = 0.01 * tune2;
//								global_data.param[PARAM_PID_POS_I] = 0;
								global_data.param[PARAM_PID_POS_D] = 0.01 * tune3;
			}
			else if (global_data.param[PARAM_TRIMCHAN] == 3)
			{
								global_data.param[PARAM_PID_POS_Z_P] = 0.01 * tune2;
//								global_data.param[PARAM_PID_POS_Z_I] = 0;
								global_data.param[PARAM_PID_POS_Z_D] = 0.01 * tune3;
			}
			else if (global_data.param[PARAM_TRIMCHAN] == 4)
			{
								global_data.param[PARAM_PID_YAWSPEED_P] = 0.1 * tune2;
//								global_data.param[PARAM_PID_YAWSPEED_I] = 0;
								global_data.param[PARAM_PID_YAWSPEED_D] = 0.1 * tune3;
			}
			else if (global_data.param[PARAM_TRIMCHAN] == 5)
			{
								global_data.param[PARAM_PID_YAWPOS_P] = 0.1 * tune2;
//								global_data.param[PARAM_PID_YAWPOS_I] = 0;
								global_data.param[PARAM_PID_YAWPOS_D] = 0.1 * tune3;
			}
			//this is done at 10 Hz
			//			pid_set_parameters(&nick_controller,
			//					global_data.param[PARAM_PID_ATT_P],
			//					global_data.param[PARAM_PID_ATT_I],
			//					global_data.param[PARAM_PID_ATT_D],
			//					global_data.param[PARAM_PID_ATT_AWU]);
			//			pid_set_parameters(&roll_controller,
			//					global_data.param[PARAM_PID_ATT_P],
			//					global_data.param[PARAM_PID_ATT_I],
			//					global_data.param[PARAM_PID_ATT_D],
			//					global_data.param[PARAM_PID_ATT_AWU]);
			//
			//			pid_set_parameters(&x_axis_controller,
			//					global_data.param[PARAM_PID_POS_P],
			//					global_data.param[PARAM_PID_POS_I],
			//					global_data.param[PARAM_PID_POS_D],
			//					global_data.param[PARAM_PID_POS_AWU]);
			//			pid_set_parameters(&y_axis_controller,
			//					global_data.param[PARAM_PID_POS_P],
			//					global_data.param[PARAM_PID_POS_I],
			//					global_data.param[PARAM_PID_POS_D],
			//					global_data.param[PARAM_PID_POS_AWU]);

			//global_data.param_name[PARAM_MIX_REMOTE_WEIGHT] = 1;// add position only keep - tune3;
			//global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1;
			if (global_data.param[PARAM_PPM_TUNE1_CHANNEL] != -1)
			{
				global_data.param[PARAM_CAM_ANGLE_Y_OFFSET]
						= ((float) (ppm_get_channel(
								global_data.param[PARAM_PPM_TUNE1_CHANNEL]))
								- 1000.0f) / 1000.0f;
			}
		}
		else
		{
			//No Remote signal

			if (global_data.state.remote_ok == 1)
			{
				//Wait one round and start sinking
				global_data.state.remote_ok = 0;
				debug_message_buffer("No remote signal (1st time)");
			}
			else
			{
				//already the second time
				// Emergency Landing
				if (global_data.state.fly != FLY_GROUNDED
						&& global_data.state.fly != FLY_RAMP_DOWN)
				{
					sys_set_state(MAV_STATE_EMERGENCY);
					global_data.state.fly = FLY_LANDING;//start landing
					debug_message_buffer(
							"EMERGENCY LANDING STARTED. No remote signal");
				}
				else
				{
					if (global_data.state.fly == FLY_GROUNDED)
					{
						sys_set_mode(MAV_MODE_LOCKED);
						sys_set_state(MAV_STATE_STANDBY);

						debug_message_buffer(
								"EMERGENCY LANDING FINISHED. No remote signal");
						debug_message_buffer(
								"EMERGENCY LANDING NOW LOCKED");
					}
					else
					{
						//won't come here anymore if once in locked mode
						debug_message_buffer(
								"EMERGENCY LANDING. No remote signal");
					}
				}
			}
		}

	}

}
/**
 * @brief Receive communication packets and handle them
 *
 * This function decodes packets on the protocol level and also handles
 * their value by calling the appropriate functions.
 */
void communication_receive(void)
{
	mavlink_message_t msg;
	mavlink_status_t status =
	{ 0 };
	status.packet_rx_drop_count = 0;

	// COMMUNICATION WITH ONBOARD COMPUTER

	while (uart0_char_available())
	{
		uint8_t c = uart0_get_char();


		if (global_data.state.uart0mode == UART_MODE_MAVLINK)
		{
			// Try to get a new message
			if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
			{
				// Handle message
				handle_mavlink_message(MAVLINK_COMM_0, &msg);
			}
		}
		else if (global_data.state.uart0mode == UART_MODE_BYTE_FORWARD)
		{
			uart1_transmit(c);
		}
		// And get the next one
	}

	// Update global packet drops counter
	global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count;
	global_data.comm.uart0_rx_success_count += status.packet_rx_success_count;
	status.packet_rx_drop_count = 0;

	// COMMUNICATION WITH EXTERNAL COMPUTER

	while (uart1_char_available())
	{
		uint8_t c = uart1_get_char();

		// Check if this link is used for MAVLink or GPS
		if (global_data.state.uart1mode == UART_MODE_MAVLINK)
		{
			//uart0_transmit((unsigned char)c);
			// Try to get a new message
			if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status))
			{
				// Handle message
				handle_mavlink_message(MAVLINK_COMM_1, &msg);
			}
		}
		else if (global_data.state.uart1mode == UART_MODE_GPS)
		{
			if (global_data.state.gps_mode == 10)
			{
				static uint8_t gps_i = 0;
				static char gps_chars[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
				if (c == '$' || gps_i == MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN
						- 1)
				{
					gps_i = 0;
					char gps_chars_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
					strncpy(gps_chars_buf, gps_chars,
							MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
					debug_message_buffer(gps_chars_buf);

				}
				gps_chars[gps_i++] = c;
			}
			if (gps_parse(c))
			{
				// New GPS data received
				//debug_message_buffer("RECEIVED NEW GPS DATA");
				parse_gps_msg();

				if (gps_lat == 0)
				{
					global_data.state.gps_ok = 0;
					//debug_message_buffer("GPS Signal Lost");
				}
				else
				{
					global_data.state.gps_ok = 1;

					mavlink_msg_gps_raw_send(
							global_data.param[PARAM_SEND_DEBUGCHAN],
							sys_time_clock_get_unix_time(), gps_mode, gps_lat
									/ 1e7f, gps_lon / 1e7f, gps_alt / 100.0f,
							0.0f, 0.0f, gps_gspeed / 100.0f, gps_course / 10.0f);
				}
				//				// Output satellite info
				//				for (int i = 0; i < gps_nb_channels; i++)
				//				{
				//					mavlink_msg_gps_status_send(global_data.param[PARAM_SEND_DEBUGCHAN], gps_numSV, gps_svinfos[i].svid, gps_satellite_used(gps_svinfos[i].qi), gps_svinfos[i].elev, ((gps_svinfos[i].azim/360.0f)*255.0f), gps_svinfos[i].cno);
				//				}
			}

		}
		else if (global_data.state.uart1mode == UART_MODE_BYTE_FORWARD)
		{
			uart0_transmit(c);
			led_toggle(LED_YELLOW);
		}
		// And get the next one
	}

	// Update global packet drops counter
	global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count;
	global_data.comm.uart0_rx_success_count += status.packet_rx_success_count;
	status.packet_rx_drop_count = 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);
	}
}