void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg) { /* all messages from usart2 are directly forwarded */ if(chan == MAVLINK_COMM_1) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; // Copy to USART 3 len = mavlink_msg_to_send_buffer(buf, msg); mavlink_send_uart_bytes(MAVLINK_COMM_0, buf, len); if(global_data.param[PARAM_USB_SEND_FORWARD]) mavlink_send_uart_bytes(MAVLINK_COMM_2, buf, len); return; } /* forwarded received messages from usb and usart3 to usart2 */ uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; /* copy to usart2 */ len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { usart2_tx_ringbuffer_push(buf, len); } /* handling messages */ switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t set; mavlink_msg_param_request_read_decode(msg, &set); /* Check if this message is for this system */ if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; if (set.param_id[0] == '\0') { /* Choose parameter based on index */ if (set.param_index < ONBOARD_PARAM_COUNT) { /* Report back value */ mavlink_msg_param_value_send(chan, global_data.param_name[set.param_index], global_data.param[set.param_index], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, set.param_index); } } else { for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { /* Compare */ if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } /* End matching if null termination is reached */ if (((char) global_data.param_name[i][j]) == '\0') { break; } } /* Check if matched */ if (match) { /* Report back value */ mavlink_msg_param_value_send(chan, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); } } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ m_parameter_i = 0; } break; case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); /* Check if this message is for this system */ if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { /* Compare */ if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } /* End matching if null termination is reached */ if (((char) global_data.param_name[i][j]) == '\0') { break; } } /* Check if matched */ if (match) { /* Only write and emit changes if there is actually a difference * AND only write if new value is NOT "not-a-number" * AND is NOT infinity */ if (global_data.param[i] != set.param_value && !isnan(set.param_value) && !isinf(set.param_value) && global_data.param_access[i]) { global_data.param[i] = set.param_value; /* handle sensor position */ if(i == PARAM_SENSOR_POSITION) { set_sensor_position_settings((uint8_t) set.param_value); mt9v034_context_configuration(); dma_reconfigure(); buffer_reset(); } /* handle low light mode and noise correction */ else if(i == PARAM_IMAGE_LOW_LIGHT || i == PARAM_IMAGE_ROW_NOISE_CORR) { mt9v034_context_configuration(); dma_reconfigure(); buffer_reset(); } /* handle calibration on/off */ else if(i == PARAM_CALIBRATION_ON) { mt9v034_set_context(); dma_reconfigure(); buffer_reset(); if(global_data.param[PARAM_CALIBRATION_ON]) debug_string_message_buffer("Calibration Mode On"); else debug_string_message_buffer("Calibration Mode Off"); } /* handle sensor position */ else if(i == PARAM_GYRO_SENSITIVITY_DPS) { l3gd20_config(); } else { debug_int_message_buffer("Parameter received, param id =", i); } /* report back new value */ mavlink_msg_param_value_send(MAVLINK_COMM_0, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); mavlink_msg_param_value_send(MAVLINK_COMM_2, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); } else { /* send back current value because it is not accepted or not write access*/ mavlink_msg_param_value_send(MAVLINK_COMM_0, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); mavlink_msg_param_value_send(MAVLINK_COMM_2, global_data.param_name[i], global_data.param[i], MAVLINK_TYPE_FLOAT, ONBOARD_PARAM_COUNT, m_parameter_i); } } } } } break; case MAVLINK_MSG_ID_PING: { mavlink_ping_t ping; mavlink_msg_ping_decode(msg, &ping); if (ping.target_system == 0 && ping.target_component == 0) { /* Respond to ping */ uint64_t r_timestamp = get_boot_time_ms() * 1000; mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp); } } break; default: break; } }
//@{ void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; switch (chan) { case MAVLINK_COMM_0: { if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) { // Copy to COMM 1 len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { uart1_transmit(buf[i]); } } } break; case MAVLINK_COMM_1: { if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE && msg->msgid != MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { // Copy to COMM 0 len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { uart0_transmit(buf[i]); } break; } } default: break; } switch (msg->msgid) { case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(msg, &mode); // Check if this system should change the mode if (mode.target == (uint8_t)global_data.param[PARAM_SYSTEM_ID]) { sys_set_mode(mode.mode); // Emit current mode mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); } } break; case MAVLINK_MSG_ID_ACTION: { execute_action(mavlink_msg_action_get_action(msg)); //Forwart actions from Xbee to Onboard Computer and vice versa if (chan == MAVLINK_COMM_1) { mavlink_send_uart(MAVLINK_COMM_0, msg); } else if (chan == MAVLINK_COMM_0) { mavlink_send_uart(MAVLINK_COMM_1, msg); } } break; case MAVLINK_MSG_ID_SYSTEM_TIME: { if (!sys_time_clock_get_unix_offset()) { int64_t offset = ((int64_t) mavlink_msg_system_time_get_time_usec( msg)) - (int64_t) sys_time_clock_get_time_usec(); sys_time_clock_set_unix_offset(offset); debug_message_buffer("UNIX offset updated"); } else { // debug_message_buffer("UNIX offset REFUSED"); } } break; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t stream; mavlink_msg_request_data_stream_decode(msg, &stream); switch (stream.req_stream_id) { case 0: // UNIMPLEMENTED break; case 1: // RAW SENSOR DATA global_data.param[PARAM_SEND_SLOT_RAW_IMU] = stream.start_stop; break; case 2: // EXTENDED SYSTEM STATUS global_data.param[PARAM_SEND_SLOT_ATTITUDE] = stream.start_stop; break; case 3: // REMOTE CONTROL CHANNELS global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] = stream.start_stop; break; case 4: // RAW CONTROLLER //global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; //global_data.param[PARAM_SEND_SLOT_DEBUG_3] = stream.start_stop; global_data.param[PARAM_SEND_SLOT_CONTROLLER_OUTPUT] = stream.start_stop; break; case 5: // SENSOR FUSION //LOST IN GROUDNCONTROL // global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; break; case 6: // POSITION global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; break; case 7: // EXTRA1 global_data.param[PARAM_SEND_SLOT_DEBUG_2] = stream.start_stop; break; case 8: // EXTRA2 global_data.param[PARAM_SEND_SLOT_DEBUG_4] = stream.start_stop; break; case 9: // EXTRA3 global_data.param[PARAM_SEND_SLOT_DEBUG_6] = stream.start_stop; break; default: // Do nothing break; } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t set; mavlink_msg_param_request_read_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; if (set.param_id[0] == '\0') { // Choose parameter based on index if (set.param_index < ONBOARD_PARAM_COUNT) { // Report back value mavlink_msg_param_value_send(chan, (int8_t*) global_data.param_name[set.param_index], global_data.param[set.param_index], ONBOARD_PARAM_COUNT, set.param_index); } } else { for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { // Compare if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } // End matching if null termination is reached if (((char) global_data.param_name[i][j]) == '\0') { break; } } // Check if matched if (match) { // Report back value mavlink_msg_param_value_send(chan, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); } } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // Start sending parameters m_parameter_i = 0; } break; case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { // Compare if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } // End matching if null termination is reached if (((char) global_data.param_name[i][j]) == '\0') { break; } } // Check if matched if (match) { // Only write and emit changes if there is actually a difference // AND only write if new value is NOT "not-a-number" // AND is NOT infy if (global_data.param[i] != set.param_value && !isnan(set.param_value) && !isinf(set.param_value)) { global_data.param[i] = set.param_value; // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); mavlink_msg_param_value_send(MAVLINK_COMM_1, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); debug_message_buffer_sprintf("Parameter received param id=%i",i); } } } } } break; case MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET: { mavlink_position_control_setpoint_set_t pos; mavlink_msg_position_control_setpoint_set_decode(msg, &pos); if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1) { // global_data.position_setpoint.x = pos.x; // global_data.position_setpoint.y = pos.y; // global_data.position_setpoint.z = pos.z; debug_message_buffer("Position setpoint updated. OLD?\n"); } else { debug_message_buffer( "Position setpoint recieved but not updated. OLD?\n"); } // Send back a message confirming the new position mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_0, pos.id, pos.x, pos.y, pos.z, pos.yaw); mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_1, pos.id, pos.x, pos.y, pos.z, pos.yaw); } break; case MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET: { mavlink_position_control_offset_set_t set; mavlink_msg_position_control_offset_set_decode(msg, &set); //global_data.attitude_setpoint_pos_body_offset.z = set.yaw; //Ball Tracking if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1 && global_data.param[PARAM_POSITION_YAW_TRACKING]==1) { global_data.param[PARAM_POSITION_SETPOINT_YAW] = global_data.attitude.z + set.yaw; mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 92, set.yaw); } } break; case MAVLINK_MSG_ID_SET_CAM_SHUTTER: { // Decode the desired shutter mavlink_set_cam_shutter_t cam; mavlink_msg_set_cam_shutter_decode(msg, &cam); shutter_set(cam.interval, cam.exposure); debug_message_buffer_sprintf("set_cam_shutter. interval %i", cam.interval); } break; case MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL: { uint8_t enable = mavlink_msg_image_trigger_control_get_enable(msg); shutter_control(enable); if (enable) { debug_message_buffer("CAM: Enabling hardware trigger"); } else { debug_message_buffer("CAM: Disabling hardware trigger"); } } break; case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: { mavlink_vision_position_estimate_t pos; mavlink_msg_vision_position_estimate_decode(msg, &pos); vision_buffer_handle_data(&pos); // Update validity time is done in vision buffer } break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: { mavlink_vicon_position_estimate_t pos; mavlink_msg_vicon_position_estimate_decode(msg, &pos); global_data.vicon_data.x = pos.x; global_data.vicon_data.y = pos.y; global_data.vicon_data.z = pos.z; global_data.state.vicon_new_data=1; // Update validity time global_data.vicon_last_valid = sys_time_clock_get_time_usec(); global_data.state.vicon_ok=1; // //Set data from Vicon into vision filter // global_data.vision_data.ang.x = pos.roll; // global_data.vision_data.ang.y = pos.pitch; // global_data.vision_data.ang.z = pos.yaw; // // global_data.vision_data.pos.x = pos.x; // global_data.vision_data.pos.y = pos.y; // global_data.vision_data.pos.z = pos.z; // // global_data.vision_data.new_data = 1; if (!global_data.state.vision_ok) { //Correct YAW global_data.attitude.z = pos.yaw; //If yaw goes to infy (no idea why) set it to setpoint, next time will be better if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559) { global_data.attitude.z = global_data.yaw_pos_setpoint; debug_message_buffer( "vicon CRITICAL FAULT yaw was bigger than 6 PI! prevented crash"); } } //send the vicon message to UART0 with new timestamp mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_0, global_data.vicon_last_valid, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw); } break; case MAVLINK_MSG_ID_PING: { mavlink_ping_t ping; mavlink_msg_ping_decode(msg, &ping); if (ping.target_system == 0 && ping.target_component == 0) { // Respond to ping uint64_t r_timestamp = sys_time_clock_get_unix_time(); mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp); } } break; case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: { mavlink_local_position_setpoint_set_t sp; mavlink_msg_local_position_setpoint_set_decode(msg, &sp); if (sp.target_system == global_data.param[PARAM_SYSTEM_ID]) { if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1) { if (sp.x >= global_data.position_setpoint_min.x && sp.y >= global_data.position_setpoint_min.y && sp.z >= global_data.position_setpoint_min.z && sp.x <= global_data.position_setpoint_max.x && sp.y <= global_data.position_setpoint_max.y && sp.z <= global_data.position_setpoint_max.z) { debug_message_buffer("Setpoint accepted and set."); global_data.param[PARAM_POSITION_SETPOINT_X] = sp.x; global_data.param[PARAM_POSITION_SETPOINT_Y] = sp.y; global_data.param[PARAM_POSITION_SETPOINT_Z] = sp.z; if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 0) { // Only update yaw if we are not tracking ball. global_data.param[PARAM_POSITION_SETPOINT_YAW] = sp.yaw; } //check if we want to start or land if (global_data.state.status == MAV_STATE_ACTIVE || global_data.state.status == MAV_STATE_CRITICAL) { if (sp.z > -0.1) { if (!(global_data.state.fly == FLY_GROUNDED || global_data.state.fly == FLY_SINKING || global_data.state.fly == FLY_WAIT_LANDING || global_data.state.fly == FLY_LANDING || global_data.state.fly == FLY_RAMP_DOWN)) { //if setpoint is lower that ground iate landing global_data.state.fly = FLY_SINKING; global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.2;//with lowpass debug_message_buffer( "Sinking for LANDING. (z-sp lower than 10cm)"); } else if (!(global_data.state.fly == FLY_GROUNDED)) { global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.2;//with lowpass } } else if (global_data.state.fly == FLY_GROUNDED && sp.z < -0.50) { //start if we were grounded and get a sp over 0.5m global_data.state.fly = FLY_WAIT_MOTORS; debug_message_buffer( "STARTING wait motors. (z-sp higher than 50cm)"); //set point changed with lowpass, after 5s it will be ok. } } //SINK TO 0.7m if we are critical or emergency if (global_data.state.status == MAV_STATE_EMERGENCY || global_data.state.status == MAV_STATE_CRITICAL) { global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.7;//with lowpass } } else { debug_message_buffer("Setpoint refused. Out of range."); } } else { debug_message_buffer("Setpoint refused. Param setpoint accept=0."); } } } break; default: break; } }
/** * This method parses all incoming bytes and constructs a MAVLink packet. * It can handle multiple links in parallel, as each link has it's own buffer/ * parsing state machine. * @param link The interface to read from * @see LinkInterface **/ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { // receiveMutex.lock(); mavlink_message_t message; mavlink_status_t status; // Cache the link ID for common use. int linkId = link->getId(); static int mavlink09Count = 0; static int nonmavlinkCount = 0; static bool decodedFirstPacket = false; static bool warnedUser = false; static bool checkedUserNonMavlink = false; static bool warnedUserNonMavlink = false; // FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS for (int position = 0; position < b.size(); position++) { unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status); if ((uint8_t)b[position] == 0x55) mavlink09Count++; if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser) { warnedUser = true; // Obviously the user tries to use a 0.9 autopilot // with QGroundControl built for version 1.0 emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. " "Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. " "Please upgrade the MAVLink version of your autopilot. " "If your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same.")); } if (decodeState == 0 && !decodedFirstPacket) { nonmavlinkCount++; if (nonmavlinkCount > 2000 && !warnedUserNonMavlink) { //2000 bytes with no mavlink message. Are we connected to a mavlink capable device? if (!checkedUserNonMavlink) { link->requestReset(); checkedUserNonMavlink = true; } else { warnedUserNonMavlink = true; emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. " "Please check if the baud rates of QGroundControl and your autopilot are the same.")); } } } if (decodeState == 1) { decodedFirstPacket = true; if (message.msgid == MAVLINK_MSG_ID_PING) { // process ping requests (tgt_system and tgt_comp must be zero) mavlink_ping_t ping; mavlink_msg_ping_decode(&message, &ping); if (!ping.target_system && !ping.target_component) { mavlink_message_t msg; mavlink_msg_ping_pack(getSystemId(), getComponentId(), &msg, ping.time_usec, ping.seq, message.sysid, message.compid); sendMessage(msg); } } if (message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { // process telemetry status message mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(&message, &rstatus); emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rstatus.rssi, rstatus.remrssi, rstatus.txbuf, rstatus.noise, rstatus.remnoise); } // Log data if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) { uint8_t buf[MAVLINK_MAX_PACKET_LEN + sizeof(quint64)]; // Write the uint64 time in microseconds in big endian format before the message. // This timestamp is saved in UTC time. We are only saving in ms precision because // getting more than this isn't possible with Qt without a ton of extra code. quint64 time = (quint64)QDateTime::currentMSecsSinceEpoch() * 1000; qToBigEndian(time, buf); // Then write the message to the buffer int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &message); // Determine how many bytes were written by adding the timestamp size to the message size len += sizeof(quint64); // Now write this timestamp/message pair to the log. QByteArray b((const char*)buf, len); if (_tempLogFile.write(b) != len) { // If there's an error logging data, raise an alert and stop logging. emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName())); _stopLogging(); _logSuspendError = true; } } // ORDER MATTERS HERE! // If the matching UAS object does not yet exist, it has to be created // before emitting the packetReceived signal UASInterface* uas = UASManager::instance()->getUASForId(message.sysid); // Check and (if necessary) create UAS object if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { // ORDER MATTERS HERE! // The UAS object has first to be created and connected, // only then the rest of the application can be made aware // of its existence, as it only then can send and receive // it's first messages. // Check if the UAS has the same id like this system if (message.sysid == getSystemId()) { emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId())); } // Create a new UAS based on the heartbeat received // Todo dynamically load plugin at run-time for MAV // WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION // First create new UAS object // Decode heartbeat message mavlink_heartbeat_t heartbeat; // Reset version field to 0 heartbeat.mavlink_version = 0; mavlink_msg_heartbeat_decode(&message, &heartbeat); // Check if the UAS has a different protocol version if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION)) { // Bring up dialog to inform user if (!versionMismatchIgnore) { emit protocolStatusMessage(tr("MAVLink Protocol"), tr("The MAVLink protocol version on the MAV and QGroundControl mismatch! " "It is unsafe to use different MAVLink versions. " "QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION)); versionMismatchIgnore = true; } // Ignore this message and continue gracefully continue; } // Create a new UAS object uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat); } // Increase receive counter totalReceiveCounter[linkId]++; currReceiveCounter[linkId]++; // Determine what the next expected sequence number is, accounting for // never having seen a message for this system/component pair. int lastSeq = lastIndex[message.sysid][message.compid]; int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1); // And if we didn't encounter that sequence number, record the error if (message.seq != expectedSeq) { // Determine how many messages were skipped int lostMessages = message.seq - expectedSeq; // Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity if (lostMessages < 0) { lostMessages = 0; } // And log how many were lost for all time and just this timestep totalLossCounter[linkId] += lostMessages; currLossCounter[linkId] += lostMessages; } // And update the last sequence number for this system/component pair lastIndex[message.sysid][message.compid] = expectedSeq; // Update on every 32th packet if ((totalReceiveCounter[linkId] & 0x1F) == 0) { // Calculate new loss ratio // Receive loss float receiveLoss = (double)currLossCounter[linkId] / (double)(currReceiveCounter[linkId] + currLossCounter[linkId]); receiveLoss *= 100.0f; currLossCounter[linkId] = 0; currReceiveCounter[linkId] = 0; emit receiveLossChanged(message.sysid, receiveLoss); } // The packet is emitted as a whole, as it is only 255 - 261 bytes short // kind of inefficient, but no issue for a groundstation pc. // It buys as reentrancy for the whole code over all threads emit messageReceived(link, message); // Multiplex message if enabled if (m_multiplexingEnabled) { // Get all links connected to this unit QList<LinkInterface*> links = _linkMgr->getLinks(); // Emit message on all links that are currently connected foreach(LinkInterface* currLink, links) { // Only forward this message to the other links, // not the link the message was received on if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid); } } } }