void mavlink_connector::handle_message(mavlink_message_t *msg)
{
switch (msg->msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
// printf("I can hear you heart\n");
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
handle_command_long(msg);
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
case MAVLINK_MSG_ID_MISSION_REQUEST:
case MAVLINK_MSG_ID_MISSION_COUNT:
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
case MAVLINK_MSG_ID_MISSION_ITEM:
handle_missions(msg);
//break;
default:
printf("msg !!!!!!%d\n",msg->msgid);
}
}
void GCS_Mavlink<T>::mavlink_receive()
{
mavlink_message_t msg;
mavlink_status_t status;
if (_serial.available())
{
char c = (char) _serial.read();
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
_GCS_connection = true;
_lastReceivedHearthbeat = millis();
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
handle_request_param_list();
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
handle_param_set(&msg);
break;
}
case MAVLINK_MSG_ID_SET_MODE: {
handle_set_mode(&msg);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
handle_param_request_read(&msg);
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL: {
handle_manual_control(&msg);
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
Serial.println("MAVLINK_MSG_ID_MISSION_REQUEST_LIST");
handle_mission_request_list(&msg);
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
Serial.println("MAVLINK_MSG_ID_MISSION_COUNT");
handle_mission_count(&msg);
break;
}
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
Serial.println("MAVLINK_MSG_ID_REQUEST_DATA_STREAM");
handle_request_data_stream(&msg);
break;
}
case MAVLINK_MSG_ID_COMMAND_LONG: {
Serial.println("MAVLINK_MSG_ID_COMMAND_LONG");
handle_command_long(&msg);
break;
}
default:
// _serial.print("msgid:");
// _serial.println((uint8_t) msg.msgid);
Serial.print("msgid:");
Serial.println(msg.msgid);
break;
}
}
}
if (_GCS_connection)
send_param_list_next();
}
