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);
}
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;
}
}
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;
}
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);
}
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);
}
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);
}
// 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);
}
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;
}
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, ¶m_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;
}
}
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, ¶m_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, ¶meter, 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;
}
}