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::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);
}
static int eth_txts(uint8 *pkt, int len)
{
int res;
for( ; ; ) {
if (!irq_inside_int())
tx_lock();
res = net_tx(pkt, len, irq_inside_int());
if (!irq_inside_int())
tx_unlock();
if (!res || irq_inside_int())
break;
thd_pass();
}
return res;
}
static int eth_txts(uint8 *pkt, int len)
{
int res;
int oldirq;
for( ; ; ) {
if (!irq_inside_int())
tx_lock();
#if 0
STOPIRQ;
while (bba_dma_busy) {
STARTIRQ;
thd_pass();
STOPIRQ;
}
#endif
#ifdef NICE
res = net_tx(pkt, len, irq_inside_int());
#else
res = net_tx(pkt, len, 1);
#endif
#if 0
STARTIRQ;
#endif
if (!irq_inside_int())
tx_unlock();
if (!res || irq_inside_int())
break;
thd_pass();
}
return res;
}
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);
}
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;
}
}
