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 GCS_Mavlink<T>::debug_parameter(float val, uint8_t id)
{
mavlink_debug_t debug;
debug.time_boot_ms = millis();
debug.value = (float) val;
debug.ind = id;
id++;
mavlink_message_t msg;
mavlink_msg_debug_encode(_systemId, MAV_COMP_ID_ALL, &msg, &debug);
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
void GCS_Mavlink<T>::debug_parameter(const char* str)
{
mavlink_debug_t debug;
debug.time_boot_ms = millis();
var_type type;
debug.value = Mav_Param::get_value_id_by_name(debug.ind, type, str);
_serial.print(debug.value);
_serial.print(",");
_serial.println(debug.ind);
mavlink_message_t msg;
mavlink_msg_debug_encode(_systemId, MAV_COMP_ID_ALL, &msg, &debug);
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
