Exemplo n.º 1
0
void FPGACamToolApp::run() {
	log("FPGACamToolApp running", Logger::LOGLEVEL_DEBUG);

	Array2D array(Core::argc, Core::argv);

	while( !interrupted() ) {
		if ( Array2D::status == Array2D::CLEAR || Array2D::status == Array2D::COMPLETE ) {
			Array2D::status = Array2D::PENDING;
			Lock tx_lock(tx_mav_mutex);
			mavlink_msg_data_transmission_handshake_pack(system_id(), component_id, &tx_mav_msg, DATA_TYPE_RAW_IMAGE, 0, 0, 0, 0, 0, 0);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
		}
			
		Array2D::display();
		usleep(10000);
	}

	{
		Lock tx_lock(tx_mav_mutex);
		mavlink_msg_data_transmission_handshake_pack(system_id(), component_id, &tx_mav_msg, 0, 0, 0, 0, 0, 0, 0);
		AppLayer<mavlink_message_t>::send(tx_mav_msg);
	}

	log("FPGACamToolApp stop running", Logger::LOGLEVEL_DEBUG);
}
Exemplo n.º 2
0
void MAVLinkMKApp::handle_input(const mk_message_t& msg) {
	uint64_t tmp64 = message_time;
	message_time = get_time_us();
	log("MAVLinkMKApp got mk_message", static_cast<int>(msg.type), Logger::LOGLEVEL_DEBUG);

	//send heartbeat approx. after 1s
	delta_time += message_time - tmp64;
	if( delta_time > 1000000 ) {
		send_heartbeat();
		delta_time = 0;
	}

	//TODO: check coded
	//TODO: check size of msg

	switch(msg.type) {
		case MK_MSG_TYPE_DEBUGOUT:
			break;
		case MK_MSG_TYPE_EXT_CTRL: {
			mavlink_manual_control_t manual_control;
			copy(&manual_control, (mk_extern_control_t*)msg.data);
			Lock tx_mav_lock(tx_mav_mutex);
			mavlink_msg_manual_control_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&manual_control);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			break;
		}
		case MK_MSG_TYPE_COMPASS:
			break;
		case MK_MSG_TYPE_POLL_DEBUG:
			break;
		case MK_MSG_TYPE_ANGLE_OUT: {
			mavlink_debug_t debug;
			debug.ind = 0;
			mk_angles_t *mk_angles = (mk_angles_t*)msg.data;
			debug.value = ((float)mk_angles->pitch) / 10;
			Lock tx_mav_lock(tx_mav_mutex);
			mavlink_msg_debug_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&debug);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			break;
		}
		case MK_MSG_TYPE_MOTORTEST:
			break;
		case MK_MSG_TYPE_SETTING:
			break;
		default:
			break;
	}

}
Exemplo n.º 3
0
  void FC_Mpkg::run() {
		int buf[1];
		mavlink_message_t msg_i;

		Logger::log("FC_Mpkg starting", name(), Logger::LOGLEVEL_INFO);

		buf[0] = 10;
		// MKPackage msg_debug_on(1, 'd', 1, buf);
		// call constructor with: numdata (1), <buf(buf), buflen(1)> pairs
		mk_message_t msg_debug_on;
		mklink_msg_pack(&msg_debug_on, MK_FC_ADDRESS, MK_MSG_TYPE_POLL_DEBUG, buf, 1);
		sleep(3);
		AppLayer<mk_message_t>::send(msg_debug_on);
		Logger::log("FC_Mpkg debug request sent to FC", Logger::LOGLEVEL_INFO);
		// MKPackage msg_setneutral(1, 'c');
		while(true) {
			// send(msg_setneutral);
			// Logger::log("FC_Mpkg running", Logger::LOGLEVEL_INFO);
			// XXX: pass on data
			// 1. put into datacenter?
			// 2. retransmit onto protocolstack?

			mavlink_msg_huch_attitude_encode(system_id(), static_cast<uint8_t>(component_id), &msg_i, &huch_attitude);
			AppLayer<mavlink_message_t>::send(msg_i);
			mavlink_msg_huch_fc_altitude_encode(system_id(), static_cast<uint8_t>(component_id), &msg_i, &huch_altitude);
			AppLayer<mavlink_message_t>::send(msg_i);
			// mavlink_msg_huch_ranger_encode(system_id(), static_cast<uint8_t>(component_id), &msg_i, &huch_ranger);
			// send(msg_i);
			mavlink_msg_mk_fc_status_encode(system_id(), static_cast<uint8_t>(component_id), &msg_i, &mk_fc_status);
			AppLayer<mavlink_message_t>::send(msg_i);
			// send pixhawk std struct
			// mavlink_msg_raw_imu_encode(owner->system_id(), static_cast<uint8_t>(component_id), &msg_i, &raw_imu);
			// send(msg_i);
			// mavlink_msg_attitude_encode(owner->system_id(), static_cast<uint8_t>(component_id), &msg_i, &attitude);
			// owner->send(msg_i);
			mavlink_msg_manual_control_encode(system_id(), static_cast<uint8_t>(component_id), &msg_i, &manual_control);
			AppLayer<mavlink_message_t>::send(msg_i);

			// Logger::log("msg len", msg_i.len, Logger::LOGLEVEL_INFO);
			usleep(10000);
		}
		return;
  }
Exemplo n.º 4
0
void MAVLinkMKApp::send_heartbeat() {
	Lock tx_lock(tx_mav_mutex);
	mavlink_msg_heartbeat_pack(system_id(),
		component_id,
		&tx_mav_msg,
		MAV_TYPE_QUADROTOR,
		MAV_AUTOPILOT_GENERIC,
		MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,	//base mode
		0,	//custom mode
		MAV_STATE_ACTIVE);	//system status
	AppLayer<mavlink_message_t>::send(tx_mav_msg);
}
Exemplo n.º 5
0
void MavlinkAppLayer::request_data_stream(const unsigned int target_system,
	const unsigned int target_component,
	const unsigned int stream_id,
	const unsigned int rate) const {

	Lock tx_lock(tx_mav_mutex);
	mavlink_msg_request_data_stream_pack(system_id(),
		component_id,
		&tx_mav_msg,
		target_system,
		target_component,
		stream_id,
		rate,
		rate > 0 ? 1 : 0);
	send(tx_mav_msg);
}
Exemplo n.º 6
0
void FPGACamToolApp::send_heartbeat() {
	Lock tx_lock(tx_mav_mutex);
	mavlink_msg_heartbeat_pack(system_id(), component_id, &tx_mav_msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_INVALID, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 0, MAV_STATE_ACTIVE);
	AppLayer<mavlink_message_t>::send(tx_mav_msg);
}
Exemplo n.º 7
0
	// copy huch data into std pixhawk attitude
	void FC_Mpkg::set_pxh_manual_control() {
		manual_control.target = system_id();
		manual_control.r = (float)debugout_getval_u(&mk_debugout, CTL_stickgas);
	}
Exemplo n.º 8
0
Arquivo: atlas.c Projeto: cpence/arcus
arcus_atlas *atlas_from_xml(scew_element *element)
{
    arcus_atlas *atlas;
    scew_list *list, *i;
    int expected_transforms, ii, jj, kk;

    if (!element)
    {
        set_error_string("atlas_from_xml: NULL element");
        return NULL;
    }
    if (strcmp(scew_element_name(element), "atlas") != 0)
    {
        set_error_string("atlas_from_xml: element name != 'atlas'");
        return NULL;
    }
    
    atlas = atlas_create();
    if (!atlas)
        return NULL;
    
    list = scew_element_list_by_name(element, "system");
    if (!list)
    {
        set_error_string("atlas_from_xml: no systems in atlas");
        
        atlas_destroy(atlas);
        return NULL;
    }
    
    for (i = list ; i ; i = scew_list_next(i))
    {
        scew_element *e;
        arcus_system *s;
        
        e = (scew_element *)scew_list_data(i);
        s = system_from_xml(e);
        
        if (!s)
        {
            atlas_destroy(atlas);
            return NULL;
        }
        
        atlas->systems = (arcus_system **)realloc(atlas->systems, (atlas->num_systems + 1) * sizeof(arcus_system *));
        atlas->systems[atlas->num_systems] = s;
        atlas->num_systems++;
    }
    
    // If there's only one system, ignore the transforms
    if (atlas->num_systems == 1)
        return atlas;
    
    // Figure out how many transforms we should have (n choose k, both directions)
    expected_transforms = binomial(atlas->num_systems, 2) * 2;

    // Load transforms
    list = scew_element_list_by_name(element, "transform");
    if (!list)
    {
        set_error_string("atlas_from_xml: no transforms in atlas when expected");
        
        atlas_destroy(atlas);
        return NULL;
    }
    
    if (scew_list_size(list) != expected_transforms)
    {
        set_error_string("atlas_from_xml: wrong number of transforms in atlas");
        
        atlas_destroy(atlas);
        return NULL;
    }
    
    for (i = list ; i ; i = scew_list_next(i))
    {
        scew_element *e;
        arcus_transform *t;
        
        e = (scew_element *)scew_list_data(i);
        t = transform_from_xml(e);
        
        if (!t)
        {
            atlas_destroy(atlas);
            return NULL;
        }
        
        atlas->transforms = (arcus_transform **)realloc(atlas->transforms, (atlas->num_transforms + 1) * sizeof(arcus_transform *));
        atlas->transforms[atlas->num_transforms] = t;
        atlas->num_transforms++;
    }
    
    // Load differential transforms
    list = scew_element_list_by_name(element, "transform_diff");
    if (!list)
    {
        set_error_string("atlas_from_xml: no differential transforms in atlas when expected");
        
        atlas_destroy(atlas);
        return NULL;
    }
    
    if (scew_list_size(list) != expected_transforms)
    {
        set_error_string("atlas_from_xml: wrong number of differential transforms in atlas");
        
        atlas_destroy(atlas);
        return NULL;
    }
    
    for (i = list ; i ; i = scew_list_next(i))
    {
        scew_element *e;
        arcus_transform *t;
        
        e = (scew_element *)scew_list_data(i);
        t = transform_from_xml(e);
        
        if (!t)
        {
            atlas_destroy(atlas);
            return NULL;
        }
        
        atlas->diff_transforms = (arcus_transform **)realloc(atlas->diff_transforms, (atlas->num_diff_transforms + 1) * sizeof(arcus_transform *));
        atlas->diff_transforms[atlas->num_diff_transforms] = t;
        atlas->num_diff_transforms++;
    }
    
    // Check the accuracy and completeness of the transform lists
    for (ii = 0 ; ii < expected_transforms ; ii++)
    {
        arcus_transform *t, *dt;
        int found_t_from = 0, found_t_to = 0;
        int found_dt_from = 0, found_dt_to = 0;
        
        t = atlas->transforms[ii];
        dt = atlas->diff_transforms[ii];
        
        for (jj = 0 ; jj < atlas->num_systems ; jj++)
        {
            if (strcmp(system_id(atlas->systems[jj]), transform_from(t)) == 0)
                found_t_from = 1;
            if (strcmp(system_id(atlas->systems[jj]), transform_from(dt)) == 0)
                found_dt_from = 1;
            if (strcmp(system_id(atlas->systems[jj]), transform_to(t)) == 0)
                found_t_to = 1;
            if (strcmp(system_id(atlas->systems[jj]), transform_to(dt)) == 0)
                found_dt_to = 1;
        }
        
        if (!found_t_from || !found_t_to)
        {
            set_error_string("atlas_from_xml: transform has invalid from/to");
            
            atlas_destroy(atlas);
            return NULL;
        }
        
        if (!found_dt_from || !found_dt_to)
        {
            set_error_string("atlas_from_xml: transform_diff has invalid from/to");
            
            atlas_destroy(atlas);
            return NULL;
        }
    }
    
    for (ii = 0 ; ii < atlas->num_systems ; ii++)
    {
        for (jj = 0 ; jj < atlas->num_systems ; jj++)
        {
            int found_ij = 0, found_ji = 0, found_ij_d = 0, found_ji_d = 0;
            
            if (ii == jj)
                continue;
            
            arcus_system *si, *sj;
            si = atlas->systems[ii];
            sj = atlas->systems[jj];
            
            for (kk = 0 ; kk < expected_transforms ; kk++)
            {
                arcus_transform *t = atlas->transforms[kk];
                
                if (strcmp(system_id(si), transform_from(t)) == 0 &&
                    strcmp(system_id(sj), transform_to(t)) == 0)
                    found_ij = 1;
                else if (strcmp(system_id(sj), transform_from(t)) == 0 &&
                         strcmp(system_id(si), transform_to(t)) == 0)
                    found_ji = 1;
                
                t = atlas->diff_transforms[kk];
                
                if (strcmp(system_id(si), transform_from(t)) == 0 &&
                    strcmp(system_id(sj), transform_to(t)) == 0)
                    found_ij_d = 1;
                else if (strcmp(system_id(sj), transform_from(t)) == 0 &&
                         strcmp(system_id(si), transform_to(t)) == 0)
                    found_ji_d = 1;
            }
            
            if (!found_ij || !found_ji)
            {
                set_error_string("atlas_from_xml: no transforms found for one pair of systems");
                
                atlas_destroy(atlas);
                return NULL;
            }
            
            if (!found_ij_d || !found_ji_d)
            {
                set_error_string("atlas_from_xml: no differential transforms found for one pair of systems");
                
                atlas_destroy(atlas);
                return NULL;
            }
        }
    }
    
    return atlas;
}
Exemplo n.º 9
0
void MAVLinkMKHUCHApp::handle_input(const mkhuch_message_t& msg) {
	uint64_t last_mkhuch_msg_time = mkhuch_msg_time;
	mkhuch_msg_time = get_time_us();

	log("MAVLinkMKHUCHApp got mkhuch_message", static_cast<int>(msg.type), Logger::LOGLEVEL_DEBUG);

	//send heartbeat approx. after 1s
	heartbeat_time += mkhuch_msg_time - last_mkhuch_msg_time;
	if( heartbeat_time > 1000000 ) {
		send_heartbeat();
		heartbeat_time = 0;
	}

	switch(msg.type) {
		case MKHUCH_MSG_TYPE_MK_IMU: {
			const mkhuch_mk_imu_t *mk_imu = reinterpret_cast<const mkhuch_mk_imu_t*>(msg.data);
			mavlink_huch_imu_raw_adc_t imu_raw_adc;
			imu_raw_adc.xacc = mk_imu->x_adc_acc;
			imu_raw_adc.yacc = mk_imu->y_adc_acc;
			imu_raw_adc.zacc = mk_imu->z_adc_acc;
			imu_raw_adc.xgyro = mk_imu->x_adc_gyro;
			imu_raw_adc.ygyro = mk_imu->y_adc_gyro;
			imu_raw_adc.zgyro = mk_imu->z_adc_gyro;
			DataCenter::set_huch_imu_raw_adc(imu_raw_adc);
			Lock tx_lock(tx_mav_mutex);
			//forward raw ADC
			mavlink_msg_huch_imu_raw_adc_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&imu_raw_adc
				);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			//forward MK IMU
			//TODO: add compass value and baro
			mavlink_huch_mk_imu_t huch_mk_imu;
			huch_mk_imu.usec = mkhuch_msg_time;
			huch_mk_imu.xacc = (2500*mk_imu->x_acc)/1024; //convert normalized analog to mg
			huch_mk_imu.yacc = (2500*mk_imu->y_acc)/1024;
			huch_mk_imu.zacc = (2500*mk_imu->z_acc)/1024;
			huch_mk_imu.xgyro = (6700*mk_imu->x_adc_gyro)/(3*1024); //convert analog to 0.1 deg./sec.
			huch_mk_imu.ygyro = (6700*mk_imu->y_adc_gyro)/(3*1024);
			huch_mk_imu.zgyro = (6700*mk_imu->z_adc_gyro)/(3*1024);
			DataCenter::set_huch_mk_imu(huch_mk_imu);
			mavlink_msg_huch_mk_imu_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&huch_mk_imu
				);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			//forward pressure
			mavlink_raw_pressure_t raw_pressure;
			raw_pressure.time_usec = mkhuch_msg_time;
			raw_pressure.press_abs = mk_imu->press_abs;
			raw_pressure.press_diff1 = 0;	//press_diff1
			raw_pressure.press_diff2 = 0;	//press_diff2
			raw_pressure.temperature = 0;	//temperature
			DataCenter::set_raw_pressure(raw_pressure);
			mavlink_msg_raw_pressure_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&raw_pressure
				);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			//TODO: forward magneto
			break;
		}
/*		case MKHUCH_MSG_TYPE_PARAM_VALUE: {
			const mkhuch_param_t *param = reinterpret_cast<const mkhuch_param_t*>(msg.data);
			//set parameter
			uint8_t index;
			if(param->index >= parameter_count)
				index = parameter_count-1;
			else
				index = param->index;
			parameters[index] = param->value;
			//ask for next parameter
			if(index < parameter_count - 1) {
				parameter_request = index + 1;
				mk_param_type_t param_type= static_cast<mk_param_type_t>(parameter_request);
				send(MKHUCH_MSG_TYPE_PARAM_REQUEST, &param_type, sizeof(mk_param_type_t));
				parameter_time = message_time;
			} else { //got all parameters
				parameter_request = 255;
			}
			//inform others
			send_mavlink_param_value( static_cast<mk_param_type_t>(index) );
			break;
		}*/
/*		case MKHUCH_MSG_TYPE_ACTION_ACK: {

			Lock tx_lock(tx_mav_mutex);
			mavlink_msg_action_ack_pack(owner()->system_id(), component_id, &tx_mav_msg, msg.data[0], msg.data[1]);
			send(tx_mav_msg);
			break;
		}*/
	case MKHUCH_MSG_TYPE_STICKS: {
		const mkhuch_sticks_t *sticks = reinterpret_cast<const mkhuch_sticks_t*>(msg.data);
		mavlink_huch_attitude_control_t attitude_control;
		attitude_control.roll = (float)sticks->roll;
		attitude_control.pitch = (float)sticks->pitch;
		attitude_control.yaw = (float)sticks->yaw;
		attitude_control.thrust = (float)sticks->thrust;
		attitude_control.target = system_id();
		attitude_control.mask = 0;
		// log("MAVLinkMKHUCHApp got mkhuch_sticks msg", static_cast<int16_t>(sticks->roll), static_cast<int16_t>(sticks->pitch), Logger::LOGLEVEL_DEBUG);
		// log("MAVLinkMKHUCHApp got mkhuch_sticks msg", static_cast<int16_t>(sticks->yaw), static_cast<int16_t>(sticks->thrust), Logger::LOGLEVEL_DEBUG);
		// ALERT uint64_t -> uint32_t cast
		uint32_t time_ms = get_time_ms();
		Lock tx_lock(tx_mav_mutex);
		mavlink_msg_huch_attitude_control_encode(
																					 system_id(),
																					 component_id,
																					 &tx_mav_msg,
																					 &attitude_control
																					 );
		AppLayer<mavlink_message_t>::send(tx_mav_msg);

		// mavlink_msg_named_value_int_pack(system_id(),
		// 	component_id,
		// 	&tx_mav_msg,
		// 	time_ms,
		// 	"stk_roll",
		// 	sticks->roll);
		// AppLayer<mavlink_message_t>::send(tx_mav_msg);
		// mavlink_msg_named_value_int_pack(system_id(),
		// 	component_id,
		// 	&tx_mav_msg,
		// 	time_ms,
		// 	"stk_pitch",
		// 	sticks->pitch);
		// AppLayer<mavlink_message_t>::send(tx_mav_msg);
		// mavlink_msg_named_value_int_pack(system_id(),
		// 	component_id,
		// 	&tx_mav_msg,
		// 	time_ms,
		// 	"stk_yaw",
		// 	sticks->yaw);
		// AppLayer<mavlink_message_t>::send(tx_mav_msg);
		// mavlink_msg_named_value_int_pack(system_id(),
		// 	component_id,
		// 	&tx_mav_msg,
		// 	time_ms,
		// 	"stk_thrust",
		// 	sticks->thrust);
		// AppLayer<mavlink_message_t>::send(tx_mav_msg);
		break;
	}

	case MKHUCH_MSG_TYPE_RC_CHANNELS_RAW: {
		const mkhuch_rc_channels_raw_t *rc_channels_raw = reinterpret_cast<const mkhuch_rc_channels_raw_t*>(msg.data);
		//log("MAVLinkMKHUCHApp got mkhuch_rc_channels_raw msg", static_cast<int16_t>(rc_channels_raw->channel_2), Logger::LOGLEVEL_DEBUG);
		Lock tx_lock(tx_mav_mutex);
		mavlink_msg_rc_channels_raw_pack(system_id(),
			component_id,
			&tx_mav_msg,
			get_time_ms(),
			0,	//FIXME
			rc_channels_raw->channel_1,
			rc_channels_raw->channel_2,
			rc_channels_raw->channel_3,
			rc_channels_raw->channel_4,
			rc_channels_raw->channel_5,
			rc_channels_raw->channel_6,
			rc_channels_raw->channel_7,
			rc_channels_raw->channel_8,
			rc_channels_raw->rssi);
		AppLayer<mavlink_message_t>::send(tx_mav_msg);
		break;
	}

	case MKHUCH_MSG_TYPE_SYSTEM_STATUS: {
/* FIXME
			const mkhuch_system_status_t *sys_status = reinterpret_cast<const mkhuch_system_status_t*>(msg.data);
			Lock tx_lock(tx_mav_mutex);
			mavlink_msg_sys_status_pack(system_id(),
				component_id,
				&tx_mav_msg,
				sys_status->mode,
				sys_status->nav_mode,
				sys_status->state,
				1000, //FIXME: use glibtop to get load of linux system
				sys_status->vbat*100, //convert dV to mV
				0,//motor block (unsupported)
				sys_status->packet_drop);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
*/
			break;
		}
/*		case MKHUCH_MSG_TYPE_BOOT:
			//TODO
			break;*/
		case MKHUCH_MSG_TYPE_ATTITUDE: {
			const mkhuch_attitude_t *mkhuch_attitude = reinterpret_cast<const mkhuch_attitude_t*>(msg.data);
			mavlink_attitude_t mavlink_attitude;
			mavlink_attitude.time_boot_ms = mkhuch_msg_time / 1000;
			mavlink_attitude.roll = 0.001745329251994329577*(mkhuch_attitude->roll_angle);		//convert cdeg to rad with (pi/180)/10 
			mavlink_attitude.pitch = 0.001745329251994329577*(mkhuch_attitude->pitch_angle);	//convert cdeg to rad with (pi/180)/10
			mavlink_attitude.yaw = 0.001745329251994329577*(mkhuch_attitude->yaw_angle);		//convert cdeg to rad with (pi/180)/10
			//FIXME
			mavlink_attitude.rollspeed = 0;
			mavlink_attitude.pitchspeed = 0;
			mavlink_attitude.yawspeed = 0;
			Lock tx_lock(tx_mav_mutex);
			mavlink_msg_attitude_encode(system_id(),
				component_id,
				&tx_mav_msg,
				&mavlink_attitude);
			AppLayer<mavlink_message_t>::send(tx_mav_msg);
			break;
		}
		default:
			break;
	}
}
Exemplo n.º 10
0
void MAVLinkMKHUCHApp::handle_input(const mavlink_message_t &msg) {
	log("MAVLinkMKHUCHApp got mavlink_message", static_cast<int>(msg.msgid), Logger::LOGLEVEL_DEBUG);
	
	switch(msg.msgid) {
		// case MAVLINK_MSG_ID_PING: {
		// 	mavlink_ping_t ping;
		// 	mavlink_msg_ping_decode(&msg, &ping);
		// 	if(ping.target_system == 0) { //ping request
		// 		if(ping.target_component != 0
		// 		&& ping.target_component != component_id) break;

		// 		//FIXME: put sending in run-loop
		// 		Lock tx_lock(tx_mav_mutex);
		// 		mavlink_msg_ping_pack(owner()->system_id(),
		// 			component_id,
		// 			&tx_mav_msg,
		// 			mavlink_msg_ping_get_seq(&msg),
		// 			msg.sysid,
		// 			msg.compid,
		// 			ping.time);
		// 		send(tx_mav_msg);
		// 	} else if(ping.target_system == owner()->system_id()) { //ping answer
		// 		if(ping.target_component != component_id) break;
		// 		//TODO: react on answer
		// 	}
		// 	break;
		// }

		// case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
		// 	if( (mavlink_msg_param_request_list_get_target_system(&msg) == owner()->system_id())
		// 	&& (mavlink_msg_param_request_list_get_target_component(&msg) == component_id) ) {
		// 		//ask for first parameter value
		// 		mk_param_type_t param_type= REVISION;
		// 		send(MKHUCH_MSG_TYPE_PARAM_REQUEST, &param_type, sizeof(mk_param_type_t));
		// 		parameter_request = REVISION;
		// 		parameter_time = get_time_us();
		// 	}
		// 	break;

		// case MAVLINK_MSG_ID_PARAM_SET:
		// 	if( (mavlink_msg_param_set_get_target_system(&msg) == owner()->system_id())
		// 	&& (mavlink_msg_param_set_get_target_component(&msg) == component_id) ) {
		// 		int8_t parameter_id[15];
		// 		mavlink_msg_param_set_get_param_id(&msg, parameter_id);
		// 		uint16_t index = parameter_id_to_index(parameter_id);

		// 		if(index >= 0) {
		// 			//send parameter to MK
		// 			mkhuch_param_t parameter;
		// 			parameter.index = index;
		// 			float value = mavlink_msg_param_set_get_param_value(&msg);
		// 			parameter.value = static_cast<uint8_t>(value);
		// 			send(MKHUCH_MSG_TYPE_PARAM_VALUE, &parameter, sizeof(mkhuch_param_t));
		// 		}
		// 	}
		// 	break;

		// case MAVLINK_MSG_ID_MANUAL_CONTROL:
		// 	if( (mavlink_msg_param_request_read_get_target_system(&msg) == owner()->system_id()) ) {
		// 		mkhuch_extern_control_t extern_control;
		// 		//set values
		// 		extern_control.roll = static_cast<int16_t>( mavlink_msg_manual_control_get_roll(&msg) );
		// 		extern_control.pitch = static_cast<int16_t>( mavlink_msg_manual_control_get_pitch(&msg) );
		// 		extern_control.yaw = static_cast<int16_t>( mavlink_msg_manual_control_get_yaw(&msg) );
		// 		extern_control.thrust = static_cast<uint16_t>( mavlink_msg_manual_control_get_thrust(&msg) );
		// 		//set mask
    //                             extern_control.mask = 0;
		// 		if( mavlink_msg_manual_control_get_roll_manual(&msg) ) {
		// 			extern_control.mask |= (1 << ROLL_MANUAL_MASK);
		// 		}
		// 		if( mavlink_msg_manual_control_get_pitch_manual(&msg) ) {
		// 			extern_control.mask |= (1 << PITCH_MANUAL_MASK);
		// 		}
		// 		if( mavlink_msg_manual_control_get_yaw_manual(&msg) ) {
		// 			extern_control.mask |= (1 << YAW_MANUAL_MASK);
		// 		}
		// 		if( mavlink_msg_manual_control_get_thrust_manual(&msg) ) {
		// 			extern_control.mask |= (1 << THRUST_MANUAL_MASK);
		// 		}
		// 		send(MKHUCH_MSG_TYPE_EXT_CTRL, &extern_control, sizeof(mkhuch_extern_control_t));
		// 	}
		// 	break;

		case MAVLINK_MSG_ID_HUCH_ACTION:
			//log("MAVLinkMKHUCHApp got mkhuch_message: MAVLINK_MSG_ID_ACTION", Logger::LOGLEVEL_DEBUG);
			// if( (mavlink_msg_action_get_target(&msg) == owner()->system_id())
			// && (mavlink_msg_action_get_target_component(&msg) == component_id) ) {
			// 	mkhuch_action_t action;
			// 	action.id = mavlink_msg_action_get_action(&msg);
			// 	send(MKHUCH_MSG_TYPE_ACTION, &action, sizeof(mkhuch_action_t));
			// }
			break;

		case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
			if(mavlink_msg_request_data_stream_get_target_system(&msg) == system_id()) {
				log("MAVLinkMKHUCHApp got mkhuch_message: MAVLINK_MSG_ID_REQUEST_DATA_STREAM for sys:comp", system_id(), Logger::LOGLEVEL_DEBUG);
				log("MAVLinkMKHUCHApp got mkhuch_message: MAVLINK_MSG_ID_REQUEST_DATA_STREAM stream_id",
						(int)mavlink_msg_request_data_stream_get_req_stream_id (&msg),
						Logger::LOGLEVEL_DEBUG);
				log("MAVLinkMKHUCHApp got mkhuch_message: MAVLINK_MSG_ID_REQUEST_DATA_STREAM stream_rate",
						(int)mavlink_msg_request_data_stream_get_req_message_rate (&msg),
						Logger::LOGLEVEL_DEBUG);
				log("MAVLinkMKHUCHApp got mkhuch_message: MAVLINK_MSG_ID_REQUEST_DATA_STREAM stream_start_stop",
						(int)mavlink_msg_request_data_stream_get_start_stop (&msg),
						Logger::LOGLEVEL_DEBUG);

				mkhuch_request_stream_t request_stream;
				request_stream.id = mavlink_msg_request_data_stream_get_req_stream_id (&msg);
				request_stream.rate = mavlink_msg_request_data_stream_get_req_message_rate (&msg);
				switch(request_stream.id) {
				case MAV_DATA_STREAM_RAW_SENSORS:
					request_stream.id = DATA_STREAM_RAW;
					break;
				case MAV_DATA_STREAM_POSITION:
					request_stream.id = DATA_STREAM_ATTITUDE;
					break;
				case MAV_DATA_STREAM_EXTENDED_STATUS:
					request_stream.id = DATA_STREAM_SYS_STATUS;
					break;
				case MAV_DATA_STREAM_RC_CHANNELS: // map 3 to 4
					request_stream.id = DATA_STREAM_RC_CHANNELS;
					break;
				case MAV_DATA_STREAM_EXTRA1: // map 10 to 5
					request_stream.id = 5; // stick values
					break;
				}
				Lock tx_lock(tx_mkhuch_mutex); // lock msg buf
				mkhuchlink_msg_encode(&tx_mkhuch_msg, MKHUCH_MSG_TYPE_REQUEST_STREAM, &request_stream, sizeof(mkhuch_request_stream_t));
				AppLayer<mkhuch_message_t>::send(tx_mkhuch_msg);
			}
			break;

		default:
			break;
	}
}