Script started on 2019-09-05 07:50:28+02:00 [TERM="xterm-256color" TTY="/dev/pts/0" COLUMNS="158" LINES="83"]
_ZN10MavlinkFTP14handle_messageEPK17__mavlink_message:
  127|      0|{
  128|      0|	//warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2);
  129|      0|
  130|      0|	if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
  131|      0|		mavlink_file_transfer_protocol_t ftp_request;
  132|      0|		mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request);
  133|      0|
  134|       |#ifdef MAVLINK_FTP_DEBUG
  135|       |		PX4_INFO("FTP: received ftp protocol message target_system: %d target_component: %d",
  136|       |			 ftp_request.target_system, ftp_request.target_component);
  137|       |#endif
  138|       |
  139|      0|		if ((ftp_request.target_system == _getServerSystemId() || ftp_request.target_system == 0) &&
  140|      0|		    (ftp_request.target_component == _getServerComponentId() || ftp_request.target_component == 0)) {
  141|      0|			_process_request(&ftp_request, msg->sysid, msg->compid);
  142|      0|		}
  143|      0|	}
  144|      0|}
_ZN14MavlinkFTPTest14handle_messageEPK17__mavlink_message:
  127|      0|{
  128|      0|	//warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2);
  129|      0|
  130|      0|	if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
  131|      0|		mavlink_file_transfer_protocol_t ftp_request;
  132|      0|		mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request);
  133|      0|
  134|       |#ifdef MAVLINK_FTP_DEBUG
  135|       |		PX4_INFO("FTP: received ftp protocol message target_system: %d target_component: %d",
  136|       |			 ftp_request.target_system, ftp_request.target_component);
  137|       |#endif
  138|       |
  139|      0|		if ((ftp_request.target_system == _getServerSystemId() || ftp_request.target_system == 0) &&
  140|      0|		    (ftp_request.target_component == _getServerComponentId() || ftp_request.target_component == 0)) {
  141|      0|			_process_request(&ftp_request, msg->sysid, msg->compid);
  142|      0|		}
  143|      0|	}
  144|      0|}

_ZN17MavlinkLogHandler14handle_messageEPK17__mavlink_message:
   92|      0|{
   93|      0|	switch (msg->msgid) {
   94|      0|	case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
   95|      0|		_log_request_list(msg);
   96|      0|		break;
   97|      0|
   98|      0|	case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
   99|      0|		_log_request_data(msg);
  100|      0|		break;
  101|      0|
  102|      0|	case MAVLINK_MSG_ID_LOG_ERASE:
  103|      0|		_log_request_erase(msg);
  104|      0|		break;
  105|      0|
  106|      0|	case MAVLINK_MSG_ID_LOG_REQUEST_END:
  107|      0|		_log_request_end(msg);
  108|      0|		break;
  109|      0|	}
  110|      0|}

_ZN7Mavlink14handle_messageEPK17__mavlink_message:
 1140|      0|{
 1141|      0|	/*
 1142|      0|	 *  NOTE: this is called from the receiver thread
 1143|      0|	 */
 1144|      0|
 1145|      0|	if (get_forwarding_on()) {
 1146|      0|		/* forward any messages to other mavlink instances */
 1147|      0|		Mavlink::forward_message(msg, this);
 1148|      0|	}
 1149|      0|}

_ZN21MavlinkMissionManager14handle_messageEPK17__mavlink_message:
  557|      0|{
  558|      0|	switch (msg->msgid) {
  559|      0|	case MAVLINK_MSG_ID_MISSION_ACK:
  560|      0|		handle_mission_ack(msg);
  561|      0|		break;
  562|      0|
  563|      0|	case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
  564|      0|		handle_mission_set_current(msg);
  565|      0|		break;
  566|      0|
  567|      0|	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
  568|      0|		handle_mission_request_list(msg);
  569|      0|		break;
  570|      0|
  571|      0|	case MAVLINK_MSG_ID_MISSION_REQUEST:
  572|      0|		handle_mission_request(msg);
  573|      0|		break;
  574|      0|
  575|      0|	case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
  576|      0|		handle_mission_request_int(msg);
  577|      0|		break;
  578|      0|
  579|      0|	case MAVLINK_MSG_ID_MISSION_COUNT:
  580|      0|		handle_mission_count(msg);
  581|      0|		break;
  582|      0|
  583|      0|	case MAVLINK_MSG_ID_MISSION_ITEM:
  584|      0|		handle_mission_item(msg);
  585|      0|		break;
  586|      0|
  587|      0|	case MAVLINK_MSG_ID_MISSION_ITEM_INT:
  588|      0|		handle_mission_item_int(msg);
  589|      0|		break;
  590|      0|
  591|      0|	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
  592|      0|		handle_mission_clear_all(msg);
  593|      0|		break;
  594|      0|
  595|      0|	default:
  596|      0|		break;
  597|      0|	}
  598|      0|}

_ZN24MavlinkParametersManager14handle_messageEPK17__mavlink_message:
   61|      0|{
   62|      0|	switch (msg->msgid) {
   63|      0|	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
   64|      0|			/* request all parameters */
   65|      0|			mavlink_param_request_list_t req_list;
   66|      0|			mavlink_msg_param_request_list_decode(msg, &req_list);
   67|      0|
   68|      0|			if (req_list.target_system == mavlink_system.sysid &&
   69|      0|			    (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
   70|      0|				if (_send_all_index < 0) {
   71|      0|					_send_all_index = PARAM_HASH;
   72|      0|
   73|      0|				} else {
   74|      0|					/* a restart should skip the hash check on the ground */
   75|      0|					_send_all_index = 0;
   76|      0|				}
   77|      0|			}
   78|      0|
   79|      0|			if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
   80|      0|			    (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
   81|      0|				// publish list request to UAVCAN driver via uORB.
   82|      0|				uavcan_parameter_request_s req;
   83|      0|				req.message_type = msg->msgid;
   84|      0|				req.node_id = req_list.target_component;
   85|      0|				req.param_index = 0;
   86|      0|
   87|      0|				_uavcan_parameter_request_pub.publish(req);
   88|      0|			}
   89|      0|
   90|      0|			break;
   91|      0|		}
   92|      0|
   93|      0|	case MAVLINK_MSG_ID_PARAM_SET: {
   94|      0|			/* set parameter */
   95|      0|			mavlink_param_set_t set;
   96|      0|			mavlink_msg_param_set_decode(msg, &set);
   97|      0|
   98|      0|			if (set.target_system == mavlink_system.sysid &&
   99|      0|			    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
  100|      0|
  101|      0|				/* local name buffer to enforce null-terminated string */
  102|      0|				char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
  103|      0|				strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
  104|      0|				/* enforce null termination */
  105|      0|				name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
  106|      0|
  107|      0|				/* Whatever the value is, we're being told to stop sending */
  108|      0|				if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) {
  109|      0|
  110|      0|					if (_mavlink->hash_check_enabled()) {
  111|      0|						_send_all_index = -1;
  112|      0|					}
  113|      0|
  114|      0|					/* No other action taken, return */
  115|      0|					return;
  116|      0|				}
  117|      0|
  118|      0|				/* attempt to find parameter, set and send it */
  119|      0|				param_t param = param_find_no_notification(name);
  120|      0|
  121|      0|				if (param == PARAM_INVALID) {
  122|      0|					char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
  123|      0|					sprintf(buf, "[pm] unknown param: %s", name);
  124|      0|					_mavlink->send_statustext_info(buf);
  125|      0|
  126|      0|				} else {
  127|      0|					// According to the mavlink spec we should always acknowledge a write operation.
  128|      0|					param_set(param, &(set.param_value));
  129|      0|					send_param(param);
  130|      0|				}
  131|      0|			}
  132|      0|
  133|      0|			if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
  134|      0|			    (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
  135|      0|				// publish set request to UAVCAN driver via uORB.
  136|      0|				uavcan_parameter_request_s req;
  137|      0|				req.message_type = msg->msgid;
  138|      0|				req.node_id = set.target_component;
  139|      0|				req.param_index = -1;
  140|      0|				strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
  141|      0|				req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
  142|      0|
  143|      0|				if (set.param_type == MAV_PARAM_TYPE_REAL32) {
  144|      0|					req.param_type = MAV_PARAM_TYPE_REAL32;
  145|      0|					req.real_value = set.param_value;
  146|      0|
  147|      0|				} else {
  148|      0|					int32_t val;
  149|      0|					memcpy(&val, &set.param_value, sizeof(int32_t));
  150|      0|					req.param_type = MAV_PARAM_TYPE_INT64;
  151|      0|					req.int_value = val;
  152|      0|				}
  153|      0|
  154|      0|				_uavcan_parameter_request_pub.publish(req);
  155|      0|			}
  156|      0|
  157|      0|			break;
  158|      0|		}
  159|      0|
  160|      0|	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
  161|      0|			/* request one parameter */
  162|      0|			mavlink_param_request_read_t req_read;
  163|      0|			mavlink_msg_param_request_read_decode(msg, &req_read);
  164|      0|
  165|      0|			if (req_read.target_system == mavlink_system.sysid &&
  166|      0|			    (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
  167|      0|
  168|      0|				/* when no index is given, loop through string ids and compare them */
  169|      0|				if (req_read.param_index < 0) {
  170|      0|					/* XXX: I left this in so older versions of QGC wouldn't break */
  171|      0|					if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) {
  172|      0|						/* return hash check for cached params */
  173|      0|						uint32_t hash = param_hash_check();
  174|      0|
  175|      0|						/* build the one-off response message */
  176|      0|						mavlink_param_value_t param_value;
  177|      0|						param_value.param_count = param_count_used();
  178|      0|						param_value.param_index = -1;
  179|      0|						strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
  180|      0|						param_value.param_type = MAV_PARAM_TYPE_UINT32;
  181|      0|						memcpy(&param_value.param_value, &hash, sizeof(hash));
  182|      0|						mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
  183|      0|
  184|      0|					} else {
  185|      0|						/* local name buffer to enforce null-terminated string */
  186|      0|						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
  187|      0|						strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
  188|      0|						/* enforce null termination */
  189|      0|						name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
  190|      0|						/* attempt to find parameter and send it */
  191|      0|						send_param(param_find_no_notification(name));
  192|      0|					}
  193|      0|
  194|      0|				} else {
  195|      0|					/* when index is >= 0, send this parameter again */
  196|      0|					int ret = send_param(param_for_used_index(req_read.param_index));
  197|      0|
  198|      0|					if (ret == 1) {
  199|      0|						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
  200|      0|						sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index);
  201|      0|						_mavlink->send_statustext_info(buf);
  202|      0|
  203|      0|					} else if (ret == 2) {
  204|      0|						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
  205|      0|						sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index);
  206|      0|						_mavlink->send_statustext_info(buf);
  207|      0|					}
  208|      0|				}
  209|      0|			}
  210|      0|
  211|      0|			if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
  212|      0|			    (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
  213|      0|				// publish set request to UAVCAN driver via uORB.
  214|      0|				uavcan_parameter_request_s req = {};
  215|      0|				req.timestamp = hrt_absolute_time();
  216|      0|				req.message_type = msg->msgid;
  217|      0|				req.node_id = req_read.target_component;
  218|      0|				req.param_index = req_read.param_index;
  219|      0|				strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
  220|      0|				req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
  221|      0|
  222|      0|				// Enque the request and forward the first to the uavcan node
  223|      0|				enque_uavcan_request(&req);
  224|      0|				request_next_uavcan_parameter();
  225|      0|			}
  226|      0|
  227|      0|			break;
  228|      0|		}
  229|      0|
  230|      0|	case MAVLINK_MSG_ID_PARAM_MAP_RC: {
  231|      0|			/* map a rc channel to a parameter */
  232|      0|			mavlink_param_map_rc_t map_rc;
  233|      0|			mavlink_msg_param_map_rc_decode(msg, &map_rc);
  234|      0|
  235|      0|			if (map_rc.target_system == mavlink_system.sysid &&
  236|      0|			    (map_rc.target_component == mavlink_system.compid ||
  237|      0|			     map_rc.target_component == MAV_COMP_ID_ALL)) {
  238|      0|
  239|      0|				/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
  240|      0|				size_t i = map_rc.parameter_rc_channel_index;
  241|      0|				_rc_param_map.param_index[i] = map_rc.param_index;
  242|      0|				strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id,
  243|      0|					MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
  244|      0|				/* enforce null termination */
  245|      0|				_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
  246|      0|				_rc_param_map.scale[i] = map_rc.scale;
  247|      0|				_rc_param_map.value0[i] = map_rc.param_value0;
  248|      0|				_rc_param_map.value_min[i] = map_rc.param_value_min;
  249|      0|				_rc_param_map.value_max[i] = map_rc.param_value_max;
  250|      0|
  251|      0|				if (map_rc.param_index == -2) { // -2 means unset map
  252|      0|					_rc_param_map.valid[i] = false;
  253|      0|
  254|      0|				} else {
  255|      0|					_rc_param_map.valid[i] = true;
  256|      0|				}
  257|      0|
  258|      0|				_rc_param_map.timestamp = hrt_absolute_time();
  259|      0|
  260|      0|				if (_rc_param_map_pub == nullptr) {
  261|      0|					_rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map);
  262|      0|
  263|      0|				} else {
  264|      0|					orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map);
  265|      0|				}
  266|      0|
  267|      0|			}
  268|      0|
  269|      0|			break;
  270|      0|		}
  271|      0|
  272|      0|	default:
  273|      0|		break;
  274|      0|	}
  275|      0|}

_ZN15MavlinkReceiver14handle_messageEP17__mavlink_message:
  106|      0|{
  107|      0|	switch (msg->msgid) {
  108|      0|	case MAVLINK_MSG_ID_COMMAND_LONG:
  109|      0|		handle_message_command_long(msg);
  110|      0|		break;
  111|      0|
  112|      0|	case MAVLINK_MSG_ID_COMMAND_INT:
  113|      0|		handle_message_command_int(msg);
  114|      0|		break;
  115|      0|
  116|      0|	case MAVLINK_MSG_ID_COMMAND_ACK:
  117|      0|		handle_message_command_ack(msg);
  118|      0|		break;
  119|      0|
  120|      0|	case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
  121|      0|		handle_message_optical_flow_rad(msg);
  122|      0|		break;
  123|      0|
  124|      0|	case MAVLINK_MSG_ID_PING:
  125|      0|		handle_message_ping(msg);
  126|      0|		break;
  127|      0|
  128|      0|	case MAVLINK_MSG_ID_SET_MODE:
  129|      0|		handle_message_set_mode(msg);
  130|      0|		break;
  131|      0|
  132|      0|	case MAVLINK_MSG_ID_ATT_POS_MOCAP:
  133|      0|		handle_message_att_pos_mocap(msg);
  134|      0|		break;
  135|      0|
  136|      0|	case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
  137|      0|		handle_message_set_position_target_local_ned(msg);
  138|      0|		break;
  139|      0|
  140|      0|	case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
  141|      0|		handle_message_set_attitude_target(msg);
  142|      0|		break;
  143|      0|
  144|      0|	case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET:
  145|      0|		handle_message_set_actuator_control_target(msg);
  146|      0|		break;
  147|      0|
  148|      0|	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
  149|      0|		handle_message_vision_position_estimate(msg);
  150|      0|		break;
  151|      0|
  152|      0|	case MAVLINK_MSG_ID_ODOMETRY:
  153|      0|		handle_message_odometry(msg);
  154|      0|		break;
  155|      0|
  156|      0|	case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN:
  157|      0|		handle_message_gps_global_origin(msg);
  158|      0|		break;
  159|      0|
  160|      0|	case MAVLINK_MSG_ID_RADIO_STATUS:
  161|      0|		handle_message_radio_status(msg);
  162|      0|		break;
  163|      0|
  164|      0|	case MAVLINK_MSG_ID_MANUAL_CONTROL:
  165|      0|		handle_message_manual_control(msg);
  166|      0|		break;
  167|      0|
  168|      0|	case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
  169|      0|		handle_message_rc_channels_override(msg);
  170|      0|		break;
  171|      0|
  172|      0|	case MAVLINK_MSG_ID_HEARTBEAT:
  173|      0|		handle_message_heartbeat(msg);
  174|      0|		break;
  175|      0|
  176|      0|	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
  177|      0|		handle_message_distance_sensor(msg);
  178|      0|		break;
  179|      0|
  180|      0|	case MAVLINK_MSG_ID_FOLLOW_TARGET:
  181|      0|		handle_message_follow_target(msg);
  182|      0|		break;
  183|      0|
  184|      0|	case MAVLINK_MSG_ID_LANDING_TARGET:
  185|      0|		handle_message_landing_target(msg);
  186|      0|		break;
  187|      0|
  188|      0|	case MAVLINK_MSG_ID_ADSB_VEHICLE:
  189|      0|		handle_message_adsb_vehicle(msg);
  190|      0|		break;
  191|      0|
  192|      0|	case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION:
  193|      0|		handle_message_utm_global_position(msg);
  194|      0|		break;
  195|      0|
  196|      0|	case MAVLINK_MSG_ID_COLLISION:
  197|      0|		handle_message_collision(msg);
  198|      0|		break;
  199|      0|
  200|      0|	case MAVLINK_MSG_ID_GPS_RTCM_DATA:
  201|      0|		handle_message_gps_rtcm_data(msg);
  202|      0|		break;
  203|      0|
  204|      0|	case MAVLINK_MSG_ID_BATTERY_STATUS:
  205|      0|		handle_message_battery_status(msg);
  206|      0|		break;
  207|      0|
  208|      0|	case MAVLINK_MSG_ID_SERIAL_CONTROL:
  209|      0|		handle_message_serial_control(msg);
  210|      0|		break;
  211|      0|
  212|      0|	case MAVLINK_MSG_ID_LOGGING_ACK:
  213|      0|		handle_message_logging_ack(msg);
  214|      0|		break;
  215|      0|
  216|      0|	case MAVLINK_MSG_ID_PLAY_TUNE:
  217|      0|		handle_message_play_tune(msg);
  218|      0|		break;
  219|      0|
  220|      0|	case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
  221|      0|		handle_message_obstacle_distance(msg);
  222|      0|		break;
  223|      0|
  224|      0|	case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS:
  225|      0|		handle_message_trajectory_representation_waypoints(msg);
  226|      0|		break;
  227|      0|
  228|      0|	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
  229|      0|		handle_message_named_value_float(msg);
  230|      0|		break;
  231|      0|
  232|      0|	case MAVLINK_MSG_ID_DEBUG:
  233|      0|		handle_message_debug(msg);
  234|      0|		break;
  235|      0|
  236|      0|	case MAVLINK_MSG_ID_DEBUG_VECT:
  237|      0|		handle_message_debug_vect(msg);
  238|      0|		break;
  239|      0|
  240|      0|	case MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY:
  241|      0|		handle_message_debug_float_array(msg);
  242|      0|		break;
  243|      0|
  244|      0|	default:
  245|      0|		break;
  246|      0|	}
  247|      0|
  248|      0|	/*
  249|      0|	 * Only decode hil messages in HIL mode.
  250|      0|	 *
  251|      0|	 * The HIL mode is enabled by the HIL bit flag
  252|      0|	 * in the system mode. Either send a set mode
  253|      0|	 * COMMAND_LONG message or a SET_MODE message
  254|      0|	 *
  255|      0|	 * Accept HIL GPS messages if use_hil_gps flag is true.
  256|      0|	 * This allows to provide fake gps measurements to the system.
  257|      0|	 */
  258|      0|	if (_mavlink->get_hil_enabled()) {
  259|      0|		switch (msg->msgid) {
  260|      0|		case MAVLINK_MSG_ID_HIL_SENSOR:
  261|      0|			handle_message_hil_sensor(msg);
  262|      0|			break;
  263|      0|
  264|      0|		case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
  265|      0|			handle_message_hil_state_quaternion(msg);
  266|      0|			break;
  267|      0|
  268|      0|		case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
  269|      0|			handle_message_hil_optical_flow(msg);
  270|      0|			break;
  271|      0|
  272|      0|		default:
  273|      0|			break;
  274|      0|		}
  275|      0|	}
  276|      0|
  277|      0|
  278|      0|	if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
  279|      0|		switch (msg->msgid) {
  280|      0|		case MAVLINK_MSG_ID_HIL_GPS:
  281|      0|			handle_message_hil_gps(msg);
  282|      0|			break;
  283|      0|
  284|      0|		default:
  285|      0|			break;
  286|      0|		}
  287|      0|
  288|      0|	}
  289|      0|
  290|      0|	/* If we've received a valid message, mark the flag indicating so.
  291|      0|	   This is used in the '-w' command-line flag. */
  292|      0|	_mavlink->set_has_received_messages(true);
  293|      0|}
_ZN15MavlinkReceiver27handle_message_command_longEP17__mavlink_message:
  377|      0|{
  378|      0|	/* command */
  379|      0|	mavlink_command_long_t cmd_mavlink;
  380|      0|	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
  381|      0|
  382|      0|	vehicle_command_s vcmd{};
  383|      0|
  384|      0|	vcmd.timestamp = hrt_absolute_time();
  385|      0|
  386|      0|	/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
  387|      0|	vcmd.param1 = cmd_mavlink.param1;
  388|      0|	vcmd.param2 = cmd_mavlink.param2;
  389|      0|	vcmd.param3 = cmd_mavlink.param3;
  390|      0|	vcmd.param4 = cmd_mavlink.param4;
  391|      0|	vcmd.param5 = (double)cmd_mavlink.param5;
  392|      0|	vcmd.param6 = (double)cmd_mavlink.param6;
  393|      0|	vcmd.param7 = cmd_mavlink.param7;
  394|      0|	vcmd.command = cmd_mavlink.command;
  395|      0|	vcmd.target_system = cmd_mavlink.target_system;
  396|      0|	vcmd.target_component = cmd_mavlink.target_component;
  397|      0|	vcmd.source_system = msg->sysid;
  398|      0|	vcmd.source_component = msg->compid;
  399|      0|	vcmd.confirmation = cmd_mavlink.confirmation;
  400|      0|	vcmd.from_external = true;
  401|      0|
  402|      0|	handle_message_command_both(msg, cmd_mavlink, vcmd);
  403|      0|}
_ZN15MavlinkReceiver26handle_message_command_intEP17__mavlink_message:
  407|      0|{
  408|      0|	/* command */
  409|      0|	mavlink_command_int_t cmd_mavlink;
  410|      0|	mavlink_msg_command_int_decode(msg, &cmd_mavlink);
  411|      0|
  412|      0|	vehicle_command_s vcmd{};
  413|      0|	vcmd.timestamp = hrt_absolute_time();
  414|      0|
  415|      0|	/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
  416|      0|	vcmd.param1 = cmd_mavlink.param1;
  417|      0|	vcmd.param2 = cmd_mavlink.param2;
  418|      0|	vcmd.param3 = cmd_mavlink.param3;
  419|      0|	vcmd.param4 = cmd_mavlink.param4;
  420|      0|	vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
  421|      0|	vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
  422|      0|	vcmd.param7 = cmd_mavlink.z;
  423|      0|	vcmd.command = cmd_mavlink.command;
  424|      0|	vcmd.target_system = cmd_mavlink.target_system;
  425|      0|	vcmd.target_component = cmd_mavlink.target_component;
  426|      0|	vcmd.source_system = msg->sysid;
  427|      0|	vcmd.source_component = msg->compid;
  428|      0|	vcmd.confirmation = false;
  429|      0|	vcmd.from_external = true;
  430|      0|
  431|      0|	handle_message_command_both(msg, cmd_mavlink, vcmd);
  432|      0|}
_ZN15MavlinkReceiver26handle_message_command_ackEP17__mavlink_message:
  517|      0|{
  518|      0|	mavlink_command_ack_t ack;
  519|      0|	mavlink_msg_command_ack_decode(msg, &ack);
  520|      0|
  521|      0|	MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink->get_channel());
  522|      0|
  523|      0|	vehicle_command_ack_s command_ack{};
  524|      0|
  525|      0|	command_ack.timestamp = hrt_absolute_time();
  526|      0|	command_ack.result_param2 = ack.result_param2;
  527|      0|	command_ack.command = ack.command;
  528|      0|	command_ack.result = ack.result;
  529|      0|	command_ack.from_external = true;
  530|      0|	command_ack.result_param1 = ack.progress;
  531|      0|	command_ack.target_system = ack.target_system;
  532|      0|	command_ack.target_component = ack.target_component;
  533|      0|
  534|      0|	_cmd_ack_pub.publish(command_ack);
  535|      0|
  536|      0|	// TODO: move it to the same place that sent the command
  537|      0|	if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) {
  538|      0|		if (msg->compid == MAV_COMP_ID_CAMERA) {
  539|      0|			PX4_WARN("Got unsuccessful result %d from camera", ack.result);
  540|      0|		}
  541|      0|	}
  542|      0|}
_ZN15MavlinkReceiver31handle_message_optical_flow_radEP17__mavlink_message:
  546|      0|{
  547|      0|	/* optical flow */
  548|      0|	mavlink_optical_flow_rad_t flow;
  549|      0|	mavlink_msg_optical_flow_rad_decode(msg, &flow);
  550|      0|
  551|      0|	optical_flow_s f{};
  552|      0|
  553|      0|	f.timestamp = hrt_absolute_time();
  554|      0|	f.time_since_last_sonar_update = flow.time_delta_distance_us;
  555|      0|	f.integration_timespan  = flow.integration_time_us;
  556|      0|	f.pixel_flow_x_integral = flow.integrated_x;
  557|      0|	f.pixel_flow_y_integral = flow.integrated_y;
  558|      0|	f.gyro_x_rate_integral  = flow.integrated_xgyro;
  559|      0|	f.gyro_y_rate_integral  = flow.integrated_ygyro;
  560|      0|	f.gyro_z_rate_integral  = flow.integrated_zgyro;
  561|      0|	f.gyro_temperature      = flow.temperature;
  562|      0|	f.ground_distance_m     = flow.distance;
  563|      0|	f.quality               = flow.quality;
  564|      0|	f.sensor_id             = flow.sensor_id;
  565|      0|	f.max_flow_rate         = _param_sens_flow_maxr.get();
  566|      0|	f.min_ground_distance   = _param_sens_flow_minhgt.get();
  567|      0|	f.max_ground_distance   = _param_sens_flow_maxhgt.get();
  568|      0|
  569|      0|	/* read flow sensor parameters */
  570|      0|	const Rotation flow_rot = (Rotation)_param_sens_flow_rot.get();
  571|      0|
  572|      0|	/* rotate measurements according to parameter */
  573|      0|	float zero_val = 0.0f;
  574|      0|	rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val);
  575|      0|	rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);
  576|      0|
  577|      0|	_flow_pub.publish(f);
  578|      0|
  579|      0|	/* Use distance value for distance sensor topic */
  580|      0|	if (flow.distance > 0.0f) { // negative values signal invalid data
  581|      0|
  582|      0|		distance_sensor_s d{};
  583|      0|
  584|      0|		d.timestamp = f.timestamp;
  585|      0|		d.min_distance = 0.3f;
  586|      0|		d.max_distance = 5.0f;
  587|      0|		d.current_distance = flow.distance; /* both are in m */
  588|      0|		d.type = 1;
  589|      0|		d.id = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
  590|      0|		d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
  591|      0|		d.variance = 0.0;
  592|      0|
  593|      0|		_flow_distance_sensor_pub.publish(d);
  594|      0|	}
  595|      0|}
_ZN15MavlinkReceiver31handle_message_hil_optical_flowEP17__mavlink_message:
  599|      0|{
  600|      0|	/* optical flow */
  601|      0|	mavlink_hil_optical_flow_t flow;
  602|      0|	mavlink_msg_hil_optical_flow_decode(msg, &flow);
  603|      0|
  604|      0|	optical_flow_s f{};
  605|      0|
  606|      0|	f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
  607|      0|	f.integration_timespan = flow.integration_time_us;
  608|      0|	f.pixel_flow_x_integral = flow.integrated_x;
  609|      0|	f.pixel_flow_y_integral = flow.integrated_y;
  610|      0|	f.gyro_x_rate_integral = flow.integrated_xgyro;
  611|      0|	f.gyro_y_rate_integral = flow.integrated_ygyro;
  612|      0|	f.gyro_z_rate_integral = flow.integrated_zgyro;
  613|      0|	f.time_since_last_sonar_update = flow.time_delta_distance_us;
  614|      0|	f.ground_distance_m = flow.distance;
  615|      0|	f.quality = flow.quality;
  616|      0|	f.sensor_id = flow.sensor_id;
  617|      0|	f.gyro_temperature = flow.temperature;
  618|      0|
  619|      0|	_flow_pub.publish(f);
  620|      0|
  621|      0|	/* Use distance value for distance sensor topic */
  622|      0|	distance_sensor_s d{};
  623|      0|
  624|      0|	d.timestamp = hrt_absolute_time();
  625|      0|	d.min_distance = 0.3f;
  626|      0|	d.max_distance = 5.0f;
  627|      0|	d.current_distance = flow.distance; /* both are in m */
  628|      0|	d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
  629|      0|	d.id = 0;
  630|      0|	d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
  631|      0|	d.variance = 0.0;
  632|      0|
  633|      0|	_flow_distance_sensor_pub.publish(d);
  634|      0|}
_ZN15MavlinkReceiver23handle_message_set_modeEP17__mavlink_message:
  638|      0|{
  639|      0|	mavlink_set_mode_t new_mode;
  640|      0|	mavlink_msg_set_mode_decode(msg, &new_mode);
  641|      0|
  642|      0|	union px4_custom_mode custom_mode;
  643|      0|	custom_mode.data = new_mode.custom_mode;
  644|      0|
  645|      0|	vehicle_command_s vcmd{};
  646|      0|
  647|      0|	vcmd.timestamp = hrt_absolute_time();
  648|      0|
  649|      0|	/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
  650|      0|	vcmd.param1 = (float)new_mode.base_mode;
  651|      0|	vcmd.param2 = (float)custom_mode.main_mode;
  652|      0|	vcmd.param3 = (float)custom_mode.sub_mode;
  653|      0|
  654|      0|	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
  655|      0|	vcmd.target_system = new_mode.target_system;
  656|      0|	vcmd.target_component = MAV_COMP_ID_ALL;
  657|      0|	vcmd.source_system = msg->sysid;
  658|      0|	vcmd.source_component = msg->compid;
  659|      0|	vcmd.confirmation = true;
  660|      0|	vcmd.from_external = true;
  661|      0|
  662|      0|	_cmd_pub.publish(vcmd);
  663|      0|}
_ZN15MavlinkReceiver30handle_message_distance_sensorEP17__mavlink_message:
  667|      0|{
  668|      0|	/* distance sensor */
  669|      0|	mavlink_distance_sensor_t dist_sensor;
  670|      0|	mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
  671|      0|
  672|      0|	distance_sensor_s d{};
  673|      0|
  674|      0|	d.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
  675|      0|	d.min_distance = float(dist_sensor.min_distance) * 1e-2f; /* cm to m */
  676|      0|	d.max_distance = float(dist_sensor.max_distance) * 1e-2f; /* cm to m */
  677|      0|	d.current_distance = float(dist_sensor.current_distance) * 1e-2f; /* cm to m */
  678|      0|	d.type = dist_sensor.type;
  679|      0|	d.id = 	MAV_DISTANCE_SENSOR_LASER;
  680|      0|	d.orientation = dist_sensor.orientation;
  681|      0|	d.variance = dist_sensor.covariance * 1e-4f; // cm^2 to m^2
  682|      0|
  683|      0|	/// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum
  684|      0|	_distance_sensor_pub.publish(d);
  685|      0|}
_ZN15MavlinkReceiver28handle_message_att_pos_mocapEP17__mavlink_message:
  689|      0|{
  690|      0|	mavlink_att_pos_mocap_t mocap;
  691|      0|	mavlink_msg_att_pos_mocap_decode(msg, &mocap);
  692|      0|
  693|      0|	vehicle_odometry_s mocap_odom{};
  694|      0|
  695|      0|	mocap_odom.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec);
  696|      0|	mocap_odom.x = mocap.x;
  697|      0|	mocap_odom.y = mocap.y;
  698|      0|	mocap_odom.z = mocap.z;
  699|      0|	mocap_odom.q[0] = mocap.q[0];
  700|      0|	mocap_odom.q[1] = mocap.q[1];
  701|      0|	mocap_odom.q[2] = mocap.q[2];
  702|      0|	mocap_odom.q[3] = mocap.q[3];
  703|      0|
  704|      0|	const size_t URT_SIZE = sizeof(mocap_odom.pose_covariance) / sizeof(mocap_odom.pose_covariance[0]);
  705|      0|	static_assert(URT_SIZE == (sizeof(mocap.covariance) / sizeof(mocap.covariance[0])),
  706|      0|		      "Odometry Pose Covariance matrix URT array size mismatch");
  707|      0|
  708|      0|	for (size_t i = 0; i < URT_SIZE; i++) {
  709|      0|		mocap_odom.pose_covariance[i] = mocap.covariance[i];
  710|      0|	}
  711|      0|
  712|      0|	mocap_odom.vx = NAN;
  713|      0|	mocap_odom.vy = NAN;
  714|      0|	mocap_odom.vz = NAN;
  715|      0|	mocap_odom.rollspeed = NAN;
  716|      0|	mocap_odom.pitchspeed = NAN;
  717|      0|	mocap_odom.yawspeed = NAN;
  718|      0|	mocap_odom.velocity_covariance[0] = NAN;
  719|      0|
  720|      0|	_mocap_odometry_pub.publish(mocap_odom);
  721|      0|}
_ZN15MavlinkReceiver44handle_message_set_position_target_local_nedEP17__mavlink_message:
  725|      0|{
  726|      0|	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
  727|      0|	mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
  728|      0|
  729|      0|	const bool values_finite =
  730|      0|		PX4_ISFINITE(set_position_target_local_ned.x) &&
  731|      0|		PX4_ISFINITE(set_position_target_local_ned.y) &&
  732|      0|		PX4_ISFINITE(set_position_target_local_ned.z) &&
  733|      0|		PX4_ISFINITE(set_position_target_local_ned.vx) &&
  734|      0|		PX4_ISFINITE(set_position_target_local_ned.vy) &&
  735|      0|		PX4_ISFINITE(set_position_target_local_ned.vz) &&
  736|      0|		PX4_ISFINITE(set_position_target_local_ned.afx) &&
  737|      0|		PX4_ISFINITE(set_position_target_local_ned.afy) &&
  738|      0|		PX4_ISFINITE(set_position_target_local_ned.afz) &&
  739|      0|		PX4_ISFINITE(set_position_target_local_ned.yaw);
  740|      0|
  741|      0|	/* Only accept messages which are intended for this system */
  742|      0|	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
  743|      0|	     set_position_target_local_ned.target_system == 0) &&
  744|      0|	    (mavlink_system.compid == set_position_target_local_ned.target_component ||
  745|      0|	     set_position_target_local_ned.target_component == 0) &&
  746|      0|	    values_finite) {
  747|      0|
  748|      0|		offboard_control_mode_s offboard_control_mode{};
  749|      0|
  750|      0|		/* convert mavlink type (local, NED) to uORB offboard control struct */
  751|      0|		offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
  752|      0|		offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4);
  753|      0|		offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
  754|      0|		offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
  755|      0|		offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
  756|      0|		offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask & 0x800);
  757|      0|		offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask & 0x800);
  758|      0|		offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask & 0x800);
  759|      0|
  760|      0|		/* yaw ignore flag mapps to ignore_attitude */
  761|      0|		bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
  762|      0|
  763|      0|		bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000);
  764|      0|		bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000);
  765|      0|		bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000);
  766|      0|		bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);
  767|      0|
  768|      0|		offboard_control_mode.timestamp = hrt_absolute_time();
  769|      0|		_offboard_control_mode_pub.publish(offboard_control_mode);
  770|      0|
  771|      0|		/* If we are in offboard control mode and offboard control loop through is enabled
  772|      0|		 * also publish the setpoint topic which is read by the controller */
  773|      0|		if (_mavlink->get_forward_externalsp()) {
  774|      0|
  775|      0|			vehicle_control_mode_s control_mode{};
  776|      0|			_control_mode_sub.copy(&control_mode);
  777|      0|
  778|      0|			if (control_mode.flag_control_offboard_enabled) {
  779|      0|				if (is_force_sp && offboard_control_mode.ignore_position &&
  780|      0|				    offboard_control_mode.ignore_velocity) {
  781|      0|
  782|      0|					PX4_WARN("force setpoint not supported");
  783|      0|
  784|      0|				} else {
  785|      0|					/* It's not a pure force setpoint: publish to setpoint triplet  topic */
  786|      0|					position_setpoint_triplet_s pos_sp_triplet{};
  787|      0|
  788|      0|					pos_sp_triplet.timestamp = hrt_absolute_time();
  789|      0|					pos_sp_triplet.previous.valid = false;
  790|      0|					pos_sp_triplet.next.valid = false;
  791|      0|					pos_sp_triplet.current.valid = true;
  792|      0|
  793|      0|					/* Order of statements matters. Takeoff can override loiter.
  794|      0|					 * See https://github.com/mavlink/mavlink/pull/670 for a broader conversation. */
  795|      0|					if (is_loiter_sp) {
  796|      0|						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
  797|      0|
  798|      0|					} else if (is_takeoff_sp) {
  799|      0|						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
  800|      0|
  801|      0|					} else if (is_land_sp) {
  802|      0|						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
  803|      0|
  804|      0|					} else if (is_idle_sp) {
  805|      0|						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
  806|      0|
  807|      0|					} else {
  808|      0|						pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
  809|      0|					}
  810|      0|
  811|      0|					/* set the local pos values */
  812|      0|					if (!offboard_control_mode.ignore_position) {
  813|      0|
  814|      0|						pos_sp_triplet.current.position_valid = true;
  815|      0|						pos_sp_triplet.current.x = set_position_target_local_ned.x;
  816|      0|						pos_sp_triplet.current.y = set_position_target_local_ned.y;
  817|      0|						pos_sp_triplet.current.z = set_position_target_local_ned.z;
  818|      0|
  819|      0|					} else {
  820|      0|						pos_sp_triplet.current.position_valid = false;
  821|      0|					}
  822|      0|
  823|      0|					/* set the local vel values */
  824|      0|					if (!offboard_control_mode.ignore_velocity) {
  825|      0|
  826|      0|						pos_sp_triplet.current.velocity_valid = true;
  827|      0|						pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
  828|      0|						pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
  829|      0|						pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
  830|      0|
  831|      0|						pos_sp_triplet.current.velocity_frame = set_position_target_local_ned.coordinate_frame;
  832|      0|
  833|      0|					} else {
  834|      0|						pos_sp_triplet.current.velocity_valid = false;
  835|      0|					}
  836|      0|
  837|      0|					if (!offboard_control_mode.ignore_alt_hold) {
  838|      0|						pos_sp_triplet.current.alt_valid = true;
  839|      0|						pos_sp_triplet.current.z = set_position_target_local_ned.z;
  840|      0|
  841|      0|					} else {
  842|      0|						pos_sp_triplet.current.alt_valid = false;
  843|      0|					}
  844|      0|
  845|      0|					/* set the local acceleration values if the setpoint type is 'local pos' and none
  846|      0|					 * of the accelerations fields is set to 'ignore' */
  847|      0|					if (!offboard_control_mode.ignore_acceleration_force) {
  848|      0|
  849|      0|						pos_sp_triplet.current.acceleration_valid = true;
  850|      0|						pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
  851|      0|						pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
  852|      0|						pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
  853|      0|						pos_sp_triplet.current.acceleration_is_force = is_force_sp;
  854|      0|
  855|      0|					} else {
  856|      0|						pos_sp_triplet.current.acceleration_valid = false;
  857|      0|					}
  858|      0|
  859|      0|					/* set the yaw sp value */
  860|      0|					if (!offboard_control_mode.ignore_attitude) {
  861|      0|						pos_sp_triplet.current.yaw_valid = true;
  862|      0|						pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
  863|      0|
  864|      0|					} else {
  865|      0|						pos_sp_triplet.current.yaw_valid = false;
  866|      0|					}
  867|      0|
  868|      0|					/* set the yawrate sp value */
  869|      0|					if (!(offboard_control_mode.ignore_bodyrate_x ||
  870|      0|					      offboard_control_mode.ignore_bodyrate_y ||
  871|      0|					      offboard_control_mode.ignore_bodyrate_z)) {
  872|      0|
  873|      0|						pos_sp_triplet.current.yawspeed_valid = true;
  874|      0|						pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
  875|      0|
  876|      0|					} else {
  877|      0|						pos_sp_triplet.current.yawspeed_valid = false;
  878|      0|					}
  879|      0|
  880|      0|					//XXX handle global pos setpoints (different MAV frames)
  881|      0|					_pos_sp_triplet_pub.publish(pos_sp_triplet);
  882|      0|				}
  883|      0|			}
  884|      0|		}
  885|      0|	}
  886|      0|}
_ZN15MavlinkReceiver42handle_message_set_actuator_control_targetEP17__mavlink_message:
  890|      0|{
  891|      0|	mavlink_set_actuator_control_target_t set_actuator_control_target;
  892|      0|	mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target);
  893|      0|
  894|      0|	bool values_finite =
  895|      0|		PX4_ISFINITE(set_actuator_control_target.controls[0]) &&
  896|      0|		PX4_ISFINITE(set_actuator_control_target.controls[1]) &&
  897|      0|		PX4_ISFINITE(set_actuator_control_target.controls[2]) &&
  898|      0|		PX4_ISFINITE(set_actuator_control_target.controls[3]) &&
  899|      0|		PX4_ISFINITE(set_actuator_control_target.controls[4]) &&
  900|      0|		PX4_ISFINITE(set_actuator_control_target.controls[5]) &&
  901|      0|		PX4_ISFINITE(set_actuator_control_target.controls[6]) &&
  902|      0|		PX4_ISFINITE(set_actuator_control_target.controls[7]);
  903|      0|
  904|      0|	if ((mavlink_system.sysid == set_actuator_control_target.target_system ||
  905|      0|	     set_actuator_control_target.target_system == 0) &&
  906|      0|	    (mavlink_system.compid == set_actuator_control_target.target_component ||
  907|      0|	     set_actuator_control_target.target_component == 0) &&
  908|      0|	    values_finite) {
  909|      0|
  910|      0|		/* Ignore all setpoints except when controlling the gimbal(group_mlx==2) as we are setting raw actuators here */
  911|      0|		bool ignore_setpoints = bool(set_actuator_control_target.group_mlx != 2);
  912|      0|
  913|      0|		offboard_control_mode_s offboard_control_mode{};
  914|      0|
  915|      0|		offboard_control_mode.ignore_thrust             = ignore_setpoints;
  916|      0|		offboard_control_mode.ignore_attitude           = ignore_setpoints;
  917|      0|		offboard_control_mode.ignore_bodyrate_x         = ignore_setpoints;
  918|      0|		offboard_control_mode.ignore_bodyrate_y         = ignore_setpoints;
  919|      0|		offboard_control_mode.ignore_bodyrate_z         = ignore_setpoints;
  920|      0|		offboard_control_mode.ignore_position           = ignore_setpoints;
  921|      0|		offboard_control_mode.ignore_velocity           = ignore_setpoints;
  922|      0|		offboard_control_mode.ignore_acceleration_force = ignore_setpoints;
  923|      0|
  924|      0|		offboard_control_mode.timestamp = hrt_absolute_time();
  925|      0|
  926|      0|		_offboard_control_mode_pub.publish(offboard_control_mode);
  927|      0|
  928|      0|		/* If we are in offboard control mode, publish the actuator controls */
  929|      0|		vehicle_control_mode_s control_mode{};
  930|      0|		_control_mode_sub.copy(&control_mode);
  931|      0|
  932|      0|		if (control_mode.flag_control_offboard_enabled) {
  933|      0|
  934|      0|			actuator_controls_s actuator_controls{};
  935|      0|			actuator_controls.timestamp = hrt_absolute_time();
  936|      0|
  937|      0|			/* Set duty cycles for the servos in the actuator_controls message */
  938|      0|			for (size_t i = 0; i < 8; i++) {
  939|      0|				actuator_controls.control[i] = set_actuator_control_target.controls[i];
  940|      0|			}
  941|      0|
  942|      0|			switch (set_actuator_control_target.group_mlx) {
  943|      0|			case 0:
  944|      0|				_actuator_controls_pubs[0].publish(actuator_controls);
  945|      0|				break;
  946|      0|
  947|      0|			case 1:
  948|      0|				_actuator_controls_pubs[1].publish(actuator_controls);
  949|      0|				break;
  950|      0|
  951|      0|			case 2:
  952|      0|				_actuator_controls_pubs[2].publish(actuator_controls);
  953|      0|				break;
  954|      0|
  955|      0|			case 3:
  956|      0|				_actuator_controls_pubs[3].publish(actuator_controls);
  957|      0|				break;
  958|      0|
  959|      0|			default:
  960|      0|				break;
  961|      0|			}
  962|      0|		}
  963|      0|	}
  964|      0|
  965|      0|}
_ZN15MavlinkReceiver32handle_message_gps_global_originEP17__mavlink_message:
  969|      0|{
  970|      0|	mavlink_gps_global_origin_t origin;
  971|      0|	mavlink_msg_gps_global_origin_decode(msg, &origin);
  972|      0|
  973|      0|	if (!globallocalconverter_initialized()) {
  974|      0|		/* Set reference point conversion of local coordiantes <--> global coordinates */
  975|      0|		globallocalconverter_init((double)origin.latitude * 1.0e-7, (double)origin.longitude * 1.0e-7,
  976|      0|					  (float)origin.altitude * 1.0e-3f, hrt_absolute_time());
  977|      0|		_global_ref_timestamp = hrt_absolute_time();
  978|      0|
  979|      0|	}
  980|      0|}
_ZN15MavlinkReceiver39handle_message_vision_position_estimateEP17__mavlink_message:
  984|      0|{
  985|      0|	mavlink_vision_position_estimate_t ev;
  986|      0|	mavlink_msg_vision_position_estimate_decode(msg, &ev);
  987|      0|
  988|      0|	vehicle_odometry_s visual_odom{};
  989|      0|
  990|      0|	visual_odom.timestamp = _mavlink_timesync.sync_stamp(ev.usec);
  991|      0|	visual_odom.x = ev.x;
  992|      0|	visual_odom.y = ev.y;
  993|      0|	visual_odom.z = ev.z;
  994|      0|	matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
  995|      0|	q.copyTo(visual_odom.q);
  996|      0|
  997|      0|	// TODO:
  998|      0|	// - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define
  999|      0|	// a frame of reference which is not aligned with NED or ENU
 1000|      0|	// - add usage on the estimator side
 1001|      0|	visual_odom.local_frame = visual_odom.LOCAL_FRAME_NED;
 1002|      0|
 1003|      0|	const size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_covariance[0]);
 1004|      0|	static_assert(URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
 1005|      0|		      "Odometry Pose Covariance matrix URT array size mismatch");
 1006|      0|
 1007|      0|	for (size_t i = 0; i < URT_SIZE; i++) {
 1008|      0|		visual_odom.pose_covariance[i] = ev.covariance[i];
 1009|      0|	}
 1010|      0|
 1011|      0|	visual_odom.vx = NAN;
 1012|      0|	visual_odom.vy = NAN;
 1013|      0|	visual_odom.vz = NAN;
 1014|      0|	visual_odom.rollspeed = NAN;
 1015|      0|	visual_odom.pitchspeed = NAN;
 1016|      0|	visual_odom.yawspeed = NAN;
 1017|      0|	visual_odom.velocity_covariance[0] = NAN;
 1018|      0|
 1019|      0|	_visual_odometry_pub.publish(visual_odom);
 1020|      0|}
_ZN15MavlinkReceiver23handle_message_odometryEP17__mavlink_message:
 1024|      0|{
 1025|      0|	mavlink_odometry_t odom;
 1026|      0|	mavlink_msg_odometry_decode(msg, &odom);
 1027|      0|
 1028|      0|	vehicle_odometry_s odometry{};
 1029|      0|
 1030|      0|	odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec);
 1031|      0|
 1032|      0|	/* The position is in a local FRD frame */
 1033|      0|	odometry.x = odom.x;
 1034|      0|	odometry.y = odom.y;
 1035|      0|	odometry.z = odom.z;
 1036|      0|
 1037|      0|	/* The quaternion of the ODOMETRY msg represents a rotation from body frame to
 1038|      0|	 * a local frame*/
 1039|      0|	matrix::Quatf q_body_to_local(odom.q);
 1040|      0|	q_body_to_local.normalize();
 1041|      0|	q_body_to_local.copyTo(odometry.q);
 1042|      0|
 1043|      0|	// TODO:
 1044|      0|	// - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define
 1045|      0|	// a frame of reference which is not aligned with NED or ENU
 1046|      0|	// - add usage on the estimator side
 1047|      0|	odometry.local_frame = odometry.LOCAL_FRAME_NED;
 1048|      0|
 1049|      0|	// pose_covariance
 1050|      0|	static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
 1051|      0|	static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])),
 1052|      0|		      "Odometry Pose Covariance matrix URT array size mismatch");
 1053|      0|
 1054|      0|	// velocity_covariance
 1055|      0|	static constexpr size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]);
 1056|      0|	static_assert(VEL_URT_SIZE == (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])),
 1057|      0|		      "Odometry Velocity Covariance matrix URT array size mismatch");
 1058|      0|
 1059|      0|	// create a method to simplify covariance copy
 1060|      0|	for (size_t i = 0; i < POS_URT_SIZE; i++) {
 1061|      0|		odometry.pose_covariance[i] = odom.pose_covariance[i];
 1062|      0|	}
 1063|      0|
 1064|      0|
 1065|      0|	/*
 1066|      0|	 * PX4 expects the body's linear velocity in the local frame,
 1067|      0|	 * the linear velocity is rotated from the odom child_frame to the
 1068|      0|	 * local NED frame. The angular velocity needs to be expressed in the
 1069|      0|	 * body (fcu_frd) frame.
 1070|      0|	 */
 1071|      0|	if (odom.child_frame_id == MAV_FRAME_BODY_FRD) {
 1072|      0|		/* Linear velocity has to be rotated to the local NED frame
 1073|      0|		 * Angular velocities are already in the right body frame  */
 1074|      0|		const matrix::Dcmf R_body_to_local = matrix::Dcmf(q_body_to_local);
 1075|      0|
 1076|      0|		/* the linear velocities needs to be transformed to the local NED frame */
 1077|      0|		matrix::Vector3<float> linvel_local(R_body_to_local * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));
 1078|      0|		odometry.vx = linvel_local(0);
 1079|      0|		odometry.vy = linvel_local(1);
 1080|      0|		odometry.vz = linvel_local(2);
 1081|      0|
 1082|      0|		odometry.rollspeed = odom.rollspeed;
 1083|      0|		odometry.pitchspeed = odom.pitchspeed;
 1084|      0|		odometry.yawspeed = odom.yawspeed;
 1085|      0|
 1086|      0|		//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
 1087|      0|		for (size_t i = 0; i < VEL_URT_SIZE; i++) {
 1088|      0|			odometry.velocity_covariance[i] = odom.velocity_covariance[i];
 1089|      0|		}
 1090|      0|
 1091|      0|	} else {
 1092|      0|		PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id);
 1093|      0|	}
 1094|      0|
 1095|      0|	if (odom.frame_id == MAV_FRAME_VISION_NED) {
 1096|      0|		_visual_odometry_pub.publish(odometry);
 1097|      0|
 1098|      0|	} else if (odom.frame_id == MAV_FRAME_MOCAP_NED) {
 1099|      0|		_mocap_odometry_pub.publish(odometry);
 1100|      0|
 1101|      0|	} else {
 1102|      0|		PX4_ERR("Local frame %u not supported. Unable to publish pose and velocity", odom.frame_id);
 1103|      0|	}
 1104|      0|}
_ZN15MavlinkReceiver34handle_message_set_attitude_targetEP17__mavlink_message:
 1108|      0|{
 1109|      0|	mavlink_set_attitude_target_t set_attitude_target;
 1110|      0|	mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
 1111|      0|
 1112|      0|	bool values_finite =
 1113|      0|		PX4_ISFINITE(set_attitude_target.q[0]) &&
 1114|      0|		PX4_ISFINITE(set_attitude_target.q[1]) &&
 1115|      0|		PX4_ISFINITE(set_attitude_target.q[2]) &&
 1116|      0|		PX4_ISFINITE(set_attitude_target.q[3]) &&
 1117|      0|		PX4_ISFINITE(set_attitude_target.thrust) &&
 1118|      0|		PX4_ISFINITE(set_attitude_target.body_roll_rate) &&
 1119|      0|		PX4_ISFINITE(set_attitude_target.body_pitch_rate) &&
 1120|      0|		PX4_ISFINITE(set_attitude_target.body_yaw_rate);
 1121|      0|
 1122|      0|	/* Only accept messages which are intended for this system */
 1123|      0|	if ((mavlink_system.sysid == set_attitude_target.target_system ||
 1124|      0|	     set_attitude_target.target_system == 0) &&
 1125|      0|	    (mavlink_system.compid == set_attitude_target.target_component ||
 1126|      0|	     set_attitude_target.target_component == 0) &&
 1127|      0|	    values_finite) {
 1128|      0|
 1129|      0|		offboard_control_mode_s offboard_control_mode{};
 1130|      0|
 1131|      0|		/* set correct ignore flags for thrust field: copy from mavlink message */
 1132|      0|		offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
 1133|      0|
 1134|      0|		/*
 1135|      0|		 * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust
 1136|      0|		 * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore
 1137|      0|		 * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits
 1138|      0|		 * set for everything else.
 1139|      0|		 */
 1140|      0|
 1141|      0|		/*
 1142|      0|		 * if attitude or body rate have been used (not ignored) previously and this message only sends
 1143|      0|		 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
 1144|      0|		 * body rates to keep the controllers running
 1145|      0|		 */
 1146|      0|		bool ignore_bodyrate_msg_x = (bool)(set_attitude_target.type_mask & 0x1);
 1147|      0|		bool ignore_bodyrate_msg_y = (bool)(set_attitude_target.type_mask & 0x2);
 1148|      0|		bool ignore_bodyrate_msg_z = (bool)(set_attitude_target.type_mask & 0x4);
 1149|      0|		bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7));
 1150|      0|
 1151|      0|
 1152|      0|		if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y ||
 1153|      0|		     ignore_bodyrate_msg_z) &&
 1154|      0|		    ignore_attitude_msg && !offboard_control_mode.ignore_thrust) {
 1155|      0|
 1156|      0|			/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
 1157|      0|			offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x && offboard_control_mode.ignore_bodyrate_x;
 1158|      0|			offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y && offboard_control_mode.ignore_bodyrate_y;
 1159|      0|			offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z && offboard_control_mode.ignore_bodyrate_z;
 1160|      0|			offboard_control_mode.ignore_attitude = ignore_attitude_msg && offboard_control_mode.ignore_attitude;
 1161|      0|
 1162|      0|		} else {
 1163|      0|			offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x;
 1164|      0|			offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y;
 1165|      0|			offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z;
 1166|      0|			offboard_control_mode.ignore_attitude = ignore_attitude_msg;
 1167|      0|		}
 1168|      0|
 1169|      0|		offboard_control_mode.ignore_position = true;
 1170|      0|		offboard_control_mode.ignore_velocity = true;
 1171|      0|		offboard_control_mode.ignore_acceleration_force = true;
 1172|      0|
 1173|      0|		offboard_control_mode.timestamp = hrt_absolute_time();
 1174|      0|
 1175|      0|		_offboard_control_mode_pub.publish(offboard_control_mode);
 1176|      0|
 1177|      0|		/* If we are in offboard control mode and offboard control loop through is enabled
 1178|      0|		 * also publish the setpoint topic which is read by the controller */
 1179|      0|		if (_mavlink->get_forward_externalsp()) {
 1180|      0|
 1181|      0|			vehicle_control_mode_s control_mode{};
 1182|      0|			_control_mode_sub.copy(&control_mode);
 1183|      0|
 1184|      0|			if (control_mode.flag_control_offboard_enabled) {
 1185|      0|
 1186|      0|				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
 1187|      0|				if (!(offboard_control_mode.ignore_attitude)) {
 1188|      0|					vehicle_attitude_setpoint_s att_sp = {};
 1189|      0|					att_sp.timestamp = hrt_absolute_time();
 1190|      0|
 1191|      0|					if (!ignore_attitude_msg) { // only copy att sp if message contained new data
 1192|      0|						matrix::Quatf q(set_attitude_target.q);
 1193|      0|						q.copyTo(att_sp.q_d);
 1194|      0|						att_sp.q_d_valid = true;
 1195|      0|
 1196|      0|						matrix::Eulerf euler{q};
 1197|      0|						att_sp.roll_body = euler.phi();
 1198|      0|						att_sp.pitch_body = euler.theta();
 1199|      0|						att_sp.yaw_body = euler.psi();
 1200|      0|						att_sp.yaw_sp_move_rate = 0.0f;
 1201|      0|					}
 1202|      0|
 1203|      0|					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
 1204|      0|						// Fill correct field by checking frametype
 1205|      0|						// TODO: add as needed
 1206|      0|						switch (_mavlink->get_system_type()) {
 1207|      0|						case MAV_TYPE_GENERIC:
 1208|      0|							break;
 1209|      0|
 1210|      0|						case MAV_TYPE_FIXED_WING:
 1211|      0|							att_sp.thrust_body[0] = set_attitude_target.thrust;
 1212|      0|							break;
 1213|      0|
 1214|      0|						case MAV_TYPE_QUADROTOR:
 1215|      0|						case MAV_TYPE_HEXAROTOR:
 1216|      0|						case MAV_TYPE_OCTOROTOR:
 1217|      0|						case MAV_TYPE_TRICOPTER:
 1218|      0|						case MAV_TYPE_HELICOPTER:
 1219|      0|							att_sp.thrust_body[2] = -set_attitude_target.thrust;
 1220|      0|							break;
 1221|      0|
 1222|      0|						case MAV_TYPE_GROUND_ROVER:
 1223|      0|							att_sp.thrust_body[0] = set_attitude_target.thrust;
 1224|      0|							break;
 1225|      0|						}
 1226|      0|					}
 1227|      0|
 1228|      0|					_att_sp_pub.publish(att_sp);
 1229|      0|				}
 1230|      0|
 1231|      0|				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
 1232|      0|				if (!offboard_control_mode.ignore_bodyrate_x ||
 1233|      0|				    !offboard_control_mode.ignore_bodyrate_y ||
 1234|      0|				    !offboard_control_mode.ignore_bodyrate_z) {
 1235|      0|
 1236|      0|					vehicle_rates_setpoint_s rates_sp{};
 1237|      0|
 1238|      0|					rates_sp.timestamp = hrt_absolute_time();
 1239|      0|
 1240|      0|					// only copy att rates sp if message contained new data
 1241|      0|					if (!ignore_bodyrate_msg_x) {
 1242|      0|						rates_sp.roll = set_attitude_target.body_roll_rate;
 1243|      0|					}
 1244|      0|
 1245|      0|					if (!ignore_bodyrate_msg_y) {
 1246|      0|						rates_sp.pitch = set_attitude_target.body_pitch_rate;
 1247|      0|					}
 1248|      0|
 1249|      0|					if (!ignore_bodyrate_msg_z) {
 1250|      0|						rates_sp.yaw = set_attitude_target.body_yaw_rate;
 1251|      0|					}
 1252|      0|
 1253|      0|					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
 1254|      0|						switch (_mavlink->get_system_type()) {
 1255|      0|						case MAV_TYPE_GENERIC:
 1256|      0|							break;
 1257|      0|
 1258|      0|						case MAV_TYPE_FIXED_WING:
 1259|      0|							rates_sp.thrust_body[0] = set_attitude_target.thrust;
 1260|      0|							break;
 1261|      0|
 1262|      0|						case MAV_TYPE_QUADROTOR:
 1263|      0|						case MAV_TYPE_HEXAROTOR:
 1264|      0|						case MAV_TYPE_OCTOROTOR:
 1265|      0|						case MAV_TYPE_TRICOPTER:
 1266|      0|						case MAV_TYPE_HELICOPTER:
 1267|      0|							rates_sp.thrust_body[2] = -set_attitude_target.thrust;
 1268|      0|							break;
 1269|      0|
 1270|      0|						case MAV_TYPE_GROUND_ROVER:
 1271|      0|							rates_sp.thrust_body[0] = set_attitude_target.thrust;
 1272|      0|							break;
 1273|      0|						}
 1274|      0|
 1275|      0|					}
 1276|      0|
 1277|      0|					_rates_sp_pub.publish(rates_sp);
 1278|      0|				}
 1279|      0|			}
 1280|      0|		}
 1281|      0|	}
 1282|      0|}
_ZN15MavlinkReceiver27handle_message_radio_statusEP17__mavlink_message:
 1286|      0|{
 1287|      0|	/* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */
 1288|      0|	if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
 1289|      0|		mavlink_radio_status_t rstatus;
 1290|      0|		mavlink_msg_radio_status_decode(msg, &rstatus);
 1291|      0|
 1292|      0|		radio_status_s status{};
 1293|      0|
 1294|      0|		status.timestamp = hrt_absolute_time();
 1295|      0|		status.rssi = rstatus.rssi;
 1296|      0|		status.remote_rssi = rstatus.remrssi;
 1297|      0|		status.txbuf = rstatus.txbuf;
 1298|      0|		status.noise = rstatus.noise;
 1299|      0|		status.remote_noise = rstatus.remnoise;
 1300|      0|		status.rxerrors = rstatus.rxerrors;
 1301|      0|		status.fix = rstatus.fixed;
 1302|      0|
 1303|      0|		_mavlink->update_radio_status(status);
 1304|      0|
 1305|      0|		_radio_status_pub.publish(status);
 1306|      0|	}
 1307|      0|}
_ZN15MavlinkReceiver19handle_message_pingEP17__mavlink_message:
 1311|      0|{
 1312|      0|	mavlink_ping_t ping;
 1313|      0|	mavlink_msg_ping_decode(msg, &ping);
 1314|      0|
 1315|      0|	if ((ping.target_system == 0) &&
 1316|      0|	    (ping.target_component == 0)) {	   // This is a ping request. Return it to the system which requested the ping.
 1317|      0|
 1318|      0|		ping.target_system = msg->sysid;
 1319|      0|		ping.target_component = msg->compid;
 1320|      0|		mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping);
 1321|      0|
 1322|      0|	} else if ((ping.target_system == mavlink_system.sysid) &&
 1323|      0|		   (ping.target_component ==
 1324|      0|		    mavlink_system.compid)) { // This is a returned ping message from this system. Calculate latency from it.
 1325|      0|
 1326|      0|		const hrt_abstime now = hrt_absolute_time();
 1327|      0|
 1328|      0|		// Calculate round trip time
 1329|      0|		float rtt_ms = (now - ping.time_usec) / 1000.0f;
 1330|      0|
 1331|      0|		// Update ping statistics
 1332|      0|		struct Mavlink::ping_statistics_s &pstats = _mavlink->get_ping_statistics();
 1333|      0|
 1334|      0|		pstats.last_ping_time = now;
 1335|      0|
 1336|      0|		if (pstats.last_ping_seq == 0 && ping.seq > 0) {
 1337|      0|			// This is the first reply we are receiving from an offboard system.
 1338|      0|			// We may have been broadcasting pings for some time before it came online,
 1339|      0|			// and these do not count as dropped packets.
 1340|      0|
 1341|      0|			// Reset last_ping_seq counter for correct packet drop detection
 1342|      0|			pstats.last_ping_seq = ping.seq - 1;
 1343|      0|		}
 1344|      0|
 1345|      0|		// We can only count dropped packets after the first message
 1346|      0|		if (ping.seq > pstats.last_ping_seq) {
 1347|      0|			pstats.dropped_packets += ping.seq - pstats.last_ping_seq - 1;
 1348|      0|		}
 1349|      0|
 1350|      0|		pstats.last_ping_seq = ping.seq;
 1351|      0|		pstats.last_rtt = rtt_ms;
 1352|      0|		pstats.mean_rtt = (rtt_ms + pstats.mean_rtt) / 2.0f;
 1353|      0|		pstats.max_rtt = fmaxf(rtt_ms, pstats.max_rtt);
 1354|      0|		pstats.min_rtt = pstats.min_rtt > 0.0f ? fminf(rtt_ms, pstats.min_rtt) : rtt_ms;
 1355|      0|
 1356|      0|		/* Ping status is supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */
 1357|      0|		if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
 1358|      0|
 1359|      0|			ping_s uorb_ping_msg{};
 1360|      0|
 1361|      0|			uorb_ping_msg.timestamp = now;
 1362|      0|			uorb_ping_msg.ping_time = ping.time_usec;
 1363|      0|			uorb_ping_msg.ping_sequence = ping.seq;
 1364|      0|			uorb_ping_msg.dropped_packets = pstats.dropped_packets;
 1365|      0|			uorb_ping_msg.rtt_ms = rtt_ms;
 1366|      0|			uorb_ping_msg.system_id = msg->sysid;
 1367|      0|			uorb_ping_msg.component_id = msg->compid;
 1368|      0|
 1369|      0|			_ping_pub.publish(uorb_ping_msg);
 1370|      0|		}
 1371|      0|	}
 1372|      0|}
_ZN15MavlinkReceiver29handle_message_battery_statusEP17__mavlink_message:
 1376|      0|{
 1377|      0|	if (msg->sysid != mavlink_system.sysid) {
 1378|      0|		// ignore battery status of other system
 1379|      0|		return;
 1380|      0|	}
 1381|      0|
 1382|      0|	// external battery measurements
 1383|      0|	mavlink_battery_status_t battery_mavlink;
 1384|      0|	mavlink_msg_battery_status_decode(msg, &battery_mavlink);
 1385|      0|
 1386|      0|	battery_status_s battery_status{};
 1387|      0|	battery_status.timestamp = hrt_absolute_time();
 1388|      0|
 1389|      0|	float voltage_sum = 0.0f;
 1390|      0|	uint8_t cell_count = 0;
 1391|      0|
 1392|      0|	while (battery_mavlink.voltages[cell_count] < UINT16_MAX && cell_count < 10) {
 1393|      0|		voltage_sum += (float)(battery_mavlink.voltages[cell_count]) / 1000.0f;
 1394|      0|		cell_count++;
 1395|      0|	}
 1396|      0|
 1397|      0|	battery_status.voltage_v = voltage_sum;
 1398|      0|	battery_status.voltage_filtered_v  = voltage_sum;
 1399|      0|	battery_status.current_a = battery_status.current_filtered_a = (float)(battery_mavlink.current_battery) / 100.0f;
 1400|      0|	battery_status.current_filtered_a = battery_status.current_a;
 1401|      0|	battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f;
 1402|      0|	battery_status.discharged_mah = (float)battery_mavlink.current_consumed;
 1403|      0|	battery_status.cell_count = cell_count;
 1404|      0|	battery_status.connected = true;
 1405|      0|
 1406|      0|	// Set the battery warning based on remaining charge.
 1407|      0|	//  Note: Smallest values must come first in evaluation.
 1408|      0|	if (battery_status.remaining < _param_bat_emergen_thr.get()) {
 1409|      0|		battery_status.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
 1410|      0|
 1411|      0|	} else if (battery_status.remaining < _param_bat_crit_thr.get()) {
 1412|      0|		battery_status.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
 1413|      0|
 1414|      0|	} else if (battery_status.remaining < _param_bat_low_thr.get()) {
 1415|      0|		battery_status.warning = battery_status_s::BATTERY_WARNING_LOW;
 1416|      0|	}
 1417|      0|
 1418|      0|	_battery_pub.publish(battery_status);
 1419|      0|}
_ZN15MavlinkReceiver29handle_message_serial_controlEP17__mavlink_message:
 1423|      0|{
 1424|      0|	mavlink_serial_control_t serial_control_mavlink;
 1425|      0|	mavlink_msg_serial_control_decode(msg, &serial_control_mavlink);
 1426|      0|
 1427|      0|	// we only support shell commands
 1428|      0|	if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL
 1429|      0|	    || (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) {
 1430|      0|		return;
 1431|      0|	}
 1432|      0|
 1433|      0|	MavlinkShell *shell = _mavlink->get_shell();
 1434|      0|
 1435|      0|	if (shell) {
 1436|      0|		// we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message
 1437|      0|		if (serial_control_mavlink.count > 0) {
 1438|      0|			shell->write(serial_control_mavlink.data, serial_control_mavlink.count);
 1439|      0|		}
 1440|      0|
 1441|      0|		// if no response requested, assume the shell is no longer used
 1442|      0|		if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
 1443|      0|			_mavlink->close_shell();
 1444|      0|		}
 1445|      0|	}
 1446|      0|}
_ZN15MavlinkReceiver26handle_message_logging_ackEP17__mavlink_message:
 1450|      0|{
 1451|      0|	mavlink_logging_ack_t logging_ack;
 1452|      0|	mavlink_msg_logging_ack_decode(msg, &logging_ack);
 1453|      0|
 1454|      0|	MavlinkULog *ulog_streaming = _mavlink->get_ulog_streaming();
 1455|      0|
 1456|      0|	if (ulog_streaming) {
 1457|      0|		ulog_streaming->handle_ack(logging_ack);
 1458|      0|	}
 1459|      0|}
_ZN15MavlinkReceiver24handle_message_play_tuneEP17__mavlink_message:
 1463|      0|{
 1464|      0|	mavlink_play_tune_t play_tune;
 1465|      0|	mavlink_msg_play_tune_decode(msg, &play_tune);
 1466|      0|
 1467|      0|	char *tune = play_tune.tune;
 1468|      0|
 1469|      0|	if ((mavlink_system.sysid == play_tune.target_system ||
 1470|      0|	     play_tune.target_system == 0) &&
 1471|      0|	    (mavlink_system.compid == play_tune.target_component ||
 1472|      0|	     play_tune.target_component == 0)) {
 1473|      0|
 1474|      0|		if (*tune == 'M') {
 1475|      0|			int fd = px4_open(TONE_ALARM0_DEVICE_PATH, PX4_F_WRONLY);
 1476|      0|
 1477|      0|			if (fd >= 0) {
 1478|      0|				px4_write(fd, tune, strlen(tune) + 1);
 1479|      0|				px4_close(fd);
 1480|      0|			}
 1481|      0|		}
 1482|      0|	}
 1483|      0|}
_ZN15MavlinkReceiver32handle_message_obstacle_distanceEP17__mavlink_message:
 1487|      0|{
 1488|      0|	mavlink_obstacle_distance_t mavlink_obstacle_distance;
 1489|      0|	mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance);
 1490|      0|
 1491|      0|	obstacle_distance_s obstacle_distance{};
 1492|      0|
 1493|      0|	obstacle_distance.timestamp = hrt_absolute_time();
 1494|      0|	obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type;
 1495|      0|	memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances));
 1496|      0|
 1497|      0|	if (mavlink_obstacle_distance.increment_f > 0.f) {
 1498|      0|		obstacle_distance.increment = mavlink_obstacle_distance.increment_f;
 1499|      0|
 1500|      0|	} else {
 1501|      0|		obstacle_distance.increment = (float)mavlink_obstacle_distance.increment;
 1502|      0|	}
 1503|      0|
 1504|      0|	obstacle_distance.min_distance = mavlink_obstacle_distance.min_distance;
 1505|      0|	obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance;
 1506|      0|	obstacle_distance.angle_offset = mavlink_obstacle_distance.angle_offset;
 1507|      0|
 1508|      0|	_obstacle_distance_pub.publish(obstacle_distance);
 1509|      0|}
_ZN15MavlinkReceiver50handle_message_trajectory_representation_waypointsEP17__mavlink_message:
 1513|      0|{
 1514|      0|	mavlink_trajectory_representation_waypoints_t trajectory;
 1515|      0|	mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory);
 1516|      0|
 1517|      0|	vehicle_trajectory_waypoint_s trajectory_waypoint{};
 1518|      0|
 1519|      0|	trajectory_waypoint.timestamp = hrt_absolute_time();
 1520|      0|	const int number_valid_points = trajectory.valid_points;
 1521|      0|
 1522|      0|	for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
 1523|      0|		trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i];
 1524|      0|		trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i];
 1525|      0|		trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i];
 1526|      0|
 1527|      0|		trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i];
 1528|      0|		trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i];
 1529|      0|		trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i];
 1530|      0|
 1531|      0|		trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i];
 1532|      0|		trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i];
 1533|      0|		trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i];
 1534|      0|
 1535|      0|		trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i];
 1536|      0|		trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i];
 1537|      0|
 1538|      0|		trajectory_waypoint.waypoints[i].type = UINT8_MAX;
 1539|      0|	}
 1540|      0|
 1541|      0|	for (int i = 0; i < number_valid_points; ++i) {
 1542|      0|		trajectory_waypoint.waypoints[i].point_valid = true;
 1543|      0|	}
 1544|      0|
 1545|      0|	for (int i = number_valid_points; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
 1546|      0|		trajectory_waypoint.waypoints[i].point_valid = false;
 1547|      0|	}
 1548|      0|
 1549|      0|	_trajectory_waypoint_pub.publish(trajectory_waypoint);
 1550|      0|}
_ZN15MavlinkReceiver35handle_message_rc_channels_overrideEP17__mavlink_message:
 1596|      0|{
 1597|      0|	mavlink_rc_channels_override_t man;
 1598|      0|	mavlink_msg_rc_channels_override_decode(msg, &man);
 1599|      0|
 1600|      0|	// Check target
 1601|      0|	if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) {
 1602|      0|		return;
 1603|      0|	}
 1604|      0|
 1605|      0|	// fill uORB message
 1606|      0|	input_rc_s rc{};
 1607|      0|
 1608|      0|	// metadata
 1609|      0|	rc.timestamp = hrt_absolute_time();
 1610|      0|	rc.timestamp_last_signal = rc.timestamp;
 1611|      0|	rc.rssi = RC_INPUT_RSSI_MAX;
 1612|      0|	rc.rc_failsafe = false;
 1613|      0|	rc.rc_lost = false;
 1614|      0|	rc.rc_lost_frame_count = 0;
 1615|      0|	rc.rc_total_frame_count = 1;
 1616|      0|	rc.rc_ppm_frame_length = 0;
 1617|      0|	rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
 1618|      0|
 1619|      0|	// channels
 1620|      0|	rc.values[0] = man.chan1_raw;
 1621|      0|	rc.values[1] = man.chan2_raw;
 1622|      0|	rc.values[2] = man.chan3_raw;
 1623|      0|	rc.values[3] = man.chan4_raw;
 1624|      0|	rc.values[4] = man.chan5_raw;
 1625|      0|	rc.values[5] = man.chan6_raw;
 1626|      0|	rc.values[6] = man.chan7_raw;
 1627|      0|	rc.values[7] = man.chan8_raw;
 1628|      0|	rc.values[8] = man.chan9_raw;
 1629|      0|	rc.values[9] = man.chan10_raw;
 1630|      0|	rc.values[10] = man.chan11_raw;
 1631|      0|	rc.values[11] = man.chan12_raw;
 1632|      0|	rc.values[12] = man.chan13_raw;
 1633|      0|	rc.values[13] = man.chan14_raw;
 1634|      0|	rc.values[14] = man.chan15_raw;
 1635|      0|	rc.values[15] = man.chan16_raw;
 1636|      0|	rc.values[16] = man.chan17_raw;
 1637|      0|	rc.values[17] = man.chan18_raw;
 1638|      0|
 1639|      0|	// check how many channels are valid
 1640|      0|	for (int i = 17; i >= 0; i--) {
 1641|      0|		const bool ignore_max = rc.values[i] == UINT16_MAX; // ignore any channel with value UINT16_MAX
 1642|      0|		const bool ignore_zero = (i > 7) && (rc.values[i] == 0); // ignore channel 8-18 if value is 0
 1643|      0|
 1644|      0|		if (ignore_max || ignore_zero) {
 1645|      0|			// set all ignored values to zero
 1646|      0|			rc.values[i] = 0;
 1647|      0|
 1648|      0|		} else {
 1649|      0|			// first channel to not ignore -> set count considering zero-based index
 1650|      0|			rc.channel_count = i + 1;
 1651|      0|			break;
 1652|      0|		}
 1653|      0|	}
 1654|      0|
 1655|      0|	// publish uORB message
 1656|      0|	_rc_pub.publish(rc);
 1657|      0|}
_ZN15MavlinkReceiver29handle_message_manual_controlEP17__mavlink_message:
 1661|      0|{
 1662|      0|	mavlink_manual_control_t man;
 1663|      0|	mavlink_msg_manual_control_decode(msg, &man);
 1664|      0|
 1665|      0|	// Check target
 1666|      0|	if (man.target != 0 && man.target != _mavlink->get_system_id()) {
 1667|      0|		return;
 1668|      0|	}
 1669|      0|
 1670|      0|	if (_mavlink->get_manual_input_mode_generation()) {
 1671|      0|
 1672|      0|		input_rc_s rc{};
 1673|      0|		rc.timestamp = hrt_absolute_time();
 1674|      0|		rc.timestamp_last_signal = rc.timestamp;
 1675|      0|
 1676|      0|		rc.channel_count = 8;
 1677|      0|		rc.rc_failsafe = false;
 1678|      0|		rc.rc_lost = false;
 1679|      0|		rc.rc_lost_frame_count = 0;
 1680|      0|		rc.rc_total_frame_count = 1;
 1681|      0|		rc.rc_ppm_frame_length = 0;
 1682|      0|		rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
 1683|      0|		rc.rssi = RC_INPUT_RSSI_MAX;
 1684|      0|
 1685|      0|		rc.values[0] = man.x / 2 + 1500;	// roll
 1686|      0|		rc.values[1] = man.y / 2 + 1500;	// pitch
 1687|      0|		rc.values[2] = man.r / 2 + 1500;	// yaw
 1688|      0|		rc.values[3] = math::constrain(man.z / 0.9f + 800.0f, 1000.0f, 2000.0f);	// throttle
 1689|      0|
 1690|      0|		/* decode all switches which fit into the channel mask */
 1691|      0|		unsigned max_switch = (sizeof(man.buttons) * 8);
 1692|      0|		unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0]));
 1693|      0|
 1694|      0|		if (max_switch > (max_channels - 4)) {
 1695|      0|			max_switch = (max_channels - 4);
 1696|      0|		}
 1697|      0|
 1698|      0|		/* fill all channels */
 1699|      0|		for (unsigned i = 0; i < max_switch; i++) {
 1700|      0|			rc.values[i + 4] = decode_switch_pos_n(man.buttons, i);
 1701|      0|		}
 1702|      0|
 1703|      0|		_mom_switch_state = man.buttons;
 1704|      0|
 1705|      0|		_rc_pub.publish(rc);
 1706|      0|
 1707|      0|	} else {
 1708|      0|		manual_control_setpoint_s manual{};
 1709|      0|
 1710|      0|		manual.timestamp = hrt_absolute_time();
 1711|      0|		manual.x = man.x / 1000.0f;
 1712|      0|		manual.y = man.y / 1000.0f;
 1713|      0|		manual.r = man.r / 1000.0f;
 1714|      0|		manual.z = man.z / 1000.0f;
 1715|      0|		manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
 1716|      0|
 1717|      0|		_manual_pub.publish(manual);
 1718|      0|	}
 1719|      0|}
_ZN15MavlinkReceiver24handle_message_heartbeatEP17__mavlink_message:
 1723|      0|{
 1724|      0|	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
 1725|      0|	if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
 1726|      0|		mavlink_heartbeat_t hb;
 1727|      0|		mavlink_msg_heartbeat_decode(msg, &hb);
 1728|      0|
 1729|      0|		/* Accept only heartbeats from GCS or ONBOARD Controller, skip heartbeats from other vehicles */
 1730|      0|		if ((msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) || (msg->sysid == mavlink_system.sysid
 1731|      0|				&& hb.type == MAV_TYPE_ONBOARD_CONTROLLER)) {
 1732|      0|
 1733|      0|			telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
 1734|      0|
 1735|      0|			/* set heartbeat time and topic time and publish -
 1736|      0|			 * the telem status also gets updated on telemetry events
 1737|      0|			 */
 1738|      0|			tstatus.heartbeat_time = hrt_absolute_time();
 1739|      0|			tstatus.remote_system_id = msg->sysid;
 1740|      0|			tstatus.remote_component_id = msg->compid;
 1741|      0|			tstatus.remote_type = hb.type;
 1742|      0|			tstatus.remote_system_status = hb.system_status;
 1743|      0|		}
 1744|      0|
 1745|      0|	}
 1746|      0|}
_ZN15MavlinkReceiver25handle_message_hil_sensorEP17__mavlink_message:
 1807|      0|{
 1808|      0|	mavlink_hil_sensor_t imu;
 1809|      0|	mavlink_msg_hil_sensor_decode(msg, &imu);
 1810|      0|
 1811|      0|	const uint64_t timestamp = hrt_absolute_time();
 1812|      0|
 1813|      0|	/* airspeed */
 1814|      0|	{
 1815|      0|		airspeed_s airspeed{};
 1816|      0|
 1817|      0|		float ias = calc_IAS(imu.diff_pressure * 1e2f);
 1818|      0|		float scale = 1.0f; //assume no instrument or pitot placement errors
 1819|      0|		float eas = calc_EAS_from_IAS(ias, scale);
 1820|      0|		float tas = calc_TAS_from_EAS(eas, imu.abs_pressure * 100, imu.temperature);
 1821|      0|
 1822|      0|		airspeed.timestamp = timestamp;
 1823|      0|		airspeed.indicated_airspeed_m_s = ias;
 1824|      0|		airspeed.true_airspeed_m_s = tas;
 1825|      0|
 1826|      0|		_airspeed_pub.publish(airspeed);
 1827|      0|	}
 1828|      0|
 1829|      0|	/* gyro */
 1830|      0|	{
 1831|      0|		sensor_gyro_s gyro{};
 1832|      0|
 1833|      0|		gyro.timestamp = timestamp;
 1834|      0|		gyro.x_raw = imu.xgyro * 1000.0f;
 1835|      0|		gyro.y_raw = imu.ygyro * 1000.0f;
 1836|      0|		gyro.z_raw = imu.zgyro * 1000.0f;
 1837|      0|		gyro.x = imu.xgyro;
 1838|      0|		gyro.y = imu.ygyro;
 1839|      0|		gyro.z = imu.zgyro;
 1840|      0|		gyro.temperature = imu.temperature;
 1841|      0|
 1842|      0|		_gyro_pub.publish(gyro);
 1843|      0|	}
 1844|      0|
 1845|      0|	/* accelerometer */
 1846|      0|	{
 1847|      0|		sensor_accel_s accel{};
 1848|      0|
 1849|      0|		accel.timestamp = timestamp;
 1850|      0|		accel.x_raw = imu.xacc / (CONSTANTS_ONE_G / 1000.0f);
 1851|      0|		accel.y_raw = imu.yacc / (CONSTANTS_ONE_G / 1000.0f);
 1852|      0|		accel.z_raw = imu.zacc / (CONSTANTS_ONE_G / 1000.0f);
 1853|      0|		accel.x = imu.xacc;
 1854|      0|		accel.y = imu.yacc;
 1855|      0|		accel.z = imu.zacc;
 1856|      0|		accel.temperature = imu.temperature;
 1857|      0|
 1858|      0|		_accel_pub.publish(accel);
 1859|      0|	}
 1860|      0|
 1861|      0|	/* magnetometer */
 1862|      0|	{
 1863|      0|		sensor_mag_s mag{};
 1864|      0|
 1865|      0|		mag.timestamp = timestamp;
 1866|      0|		mag.x_raw = imu.xmag * 1000.0f;
 1867|      0|		mag.y_raw = imu.ymag * 1000.0f;
 1868|      0|		mag.z_raw = imu.zmag * 1000.0f;
 1869|      0|		mag.x = imu.xmag;
 1870|      0|		mag.y = imu.ymag;
 1871|      0|		mag.z = imu.zmag;
 1872|      0|
 1873|      0|		_mag_pub.publish(mag);
 1874|      0|	}
 1875|      0|
 1876|      0|	/* baro */
 1877|      0|	{
 1878|      0|		sensor_baro_s baro{};
 1879|      0|
 1880|      0|		baro.timestamp = timestamp;
 1881|      0|		baro.pressure = imu.abs_pressure;
 1882|      0|		baro.temperature = imu.temperature;
 1883|      0|
 1884|      0|		/* fake device ID */
 1885|      0|		baro.device_id = 1234567;
 1886|      0|
 1887|      0|		_baro_pub.publish(baro);
 1888|      0|	}
 1889|      0|
 1890|      0|	/* battery status */
 1891|      0|	{
 1892|      0|		battery_status_s hil_battery_status{};
 1893|      0|
 1894|      0|		hil_battery_status.timestamp = timestamp;
 1895|      0|		hil_battery_status.voltage_v = 11.5f;
 1896|      0|		hil_battery_status.voltage_filtered_v = 11.5f;
 1897|      0|		hil_battery_status.current_a = 10.0f;
 1898|      0|		hil_battery_status.discharged_mah = -1.0f;
 1899|      0|
 1900|      0|		_battery_pub.publish(hil_battery_status);
 1901|      0|	}
 1902|      0|}
_ZN15MavlinkReceiver22handle_message_hil_gpsEP17__mavlink_message:
 1906|      0|{
 1907|      0|	mavlink_hil_gps_t gps;
 1908|      0|	mavlink_msg_hil_gps_decode(msg, &gps);
 1909|      0|
 1910|      0|	const uint64_t timestamp = hrt_absolute_time();
 1911|      0|
 1912|      0|	struct vehicle_gps_position_s hil_gps = {};
 1913|      0|
 1914|      0|	hil_gps.timestamp_time_relative = 0;
 1915|      0|	hil_gps.time_utc_usec = gps.time_usec;
 1916|      0|
 1917|      0|	hil_gps.timestamp = timestamp;
 1918|      0|	hil_gps.lat = gps.lat;
 1919|      0|	hil_gps.lon = gps.lon;
 1920|      0|	hil_gps.alt = gps.alt;
 1921|      0|	hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
 1922|      0|	hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
 1923|      0|
 1924|      0|	hil_gps.s_variance_m_s = 0.1f;
 1925|      0|
 1926|      0|	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
 1927|      0|	hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
 1928|      0|	hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
 1929|      0|	hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
 1930|      0|	hil_gps.vel_ned_valid = true;
 1931|      0|	hil_gps.cog_rad = ((gps.cog == 65535) ? NAN : wrap_2pi(math::radians(gps.cog * 1e-2f)));
 1932|      0|
 1933|      0|	hil_gps.fix_type = gps.fix_type;
 1934|      0|	hil_gps.satellites_used = gps.satellites_visible;  //TODO: rename mavlink_hil_gps_t sats visible to used?
 1935|      0|
 1936|      0|	hil_gps.heading = NAN;
 1937|      0|	hil_gps.heading_offset = NAN;
 1938|      0|
 1939|      0|	_gps_pub.publish(hil_gps);
 1940|      0|}
_ZN15MavlinkReceiver28handle_message_follow_targetEP17__mavlink_message:
 1944|      0|{
 1945|      0|	mavlink_follow_target_t follow_target_msg;
 1946|      0|	mavlink_msg_follow_target_decode(msg, &follow_target_msg);
 1947|      0|
 1948|      0|	follow_target_s follow_target_topic{};
 1949|      0|
 1950|      0|	follow_target_topic.timestamp = hrt_absolute_time();
 1951|      0|	follow_target_topic.lat = follow_target_msg.lat * 1e-7;
 1952|      0|	follow_target_topic.lon = follow_target_msg.lon * 1e-7;
 1953|      0|	follow_target_topic.alt = follow_target_msg.alt;
 1954|      0|
 1955|      0|	_follow_target_pub.publish(follow_target_topic);
 1956|      0|}
_ZN15MavlinkReceiver29handle_message_landing_targetEP17__mavlink_message:
 1960|      0|{
 1961|      0|	mavlink_landing_target_t landing_target;
 1962|      0|	mavlink_msg_landing_target_decode(msg, &landing_target);
 1963|      0|
 1964|      0|	if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) {
 1965|      0|		landing_target_pose_s landing_target_pose{};
 1966|      0|
 1967|      0|		landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec);
 1968|      0|		landing_target_pose.abs_pos_valid = true;
 1969|      0|		landing_target_pose.x_abs = landing_target.x;
 1970|      0|		landing_target_pose.y_abs = landing_target.y;
 1971|      0|		landing_target_pose.z_abs = landing_target.z;
 1972|      0|
 1973|      0|		_landing_target_pose_pub.publish(landing_target_pose);
 1974|      0|	}
 1975|      0|}
_ZN15MavlinkReceiver27handle_message_adsb_vehicleEP17__mavlink_message:
 1979|      0|{
 1980|      0|	mavlink_adsb_vehicle_t adsb;
 1981|      0|	mavlink_msg_adsb_vehicle_decode(msg, &adsb);
 1982|      0|
 1983|      0|	transponder_report_s t{};
 1984|      0|
 1985|      0|	t.timestamp = hrt_absolute_time();
 1986|      0|
 1987|      0|	t.icao_address = adsb.ICAO_address;
 1988|      0|	t.lat = adsb.lat * 1e-7;
 1989|      0|	t.lon = adsb.lon * 1e-7;
 1990|      0|	t.altitude_type = adsb.altitude_type;
 1991|      0|	t.altitude = adsb.altitude / 1000.0f;
 1992|      0|	t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F - M_PI_F;
 1993|      0|	t.hor_velocity = adsb.hor_velocity / 100.0f;
 1994|      0|	t.ver_velocity = adsb.ver_velocity / 100.0f;
 1995|      0|	memcpy(&t.callsign[0], &adsb.callsign[0], sizeof(t.callsign));
 1996|      0|	t.emitter_type = adsb.emitter_type;
 1997|      0|	t.tslc = adsb.tslc;
 1998|      0|	t.squawk = adsb.squawk;
 1999|      0|
 2000|      0|	t.flags = transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE;  //Unset in receiver already broadcast its messages
 2001|      0|
 2002|      0|	if (adsb.flags & ADSB_FLAGS_VALID_COORDS) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; }
 2003|      0|
 2004|      0|	if (adsb.flags & ADSB_FLAGS_VALID_ALTITUDE) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; }
 2005|      0|
 2006|      0|	if (adsb.flags & ADSB_FLAGS_VALID_HEADING) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; }
 2007|      0|
 2008|      0|	if (adsb.flags & ADSB_FLAGS_VALID_VELOCITY) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; }
 2009|      0|
 2010|      0|	if (adsb.flags & ADSB_FLAGS_VALID_CALLSIGN) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; }
 2011|      0|
 2012|      0|	if (adsb.flags & ADSB_FLAGS_VALID_SQUAWK) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK; }
 2013|      0|
 2014|      0|	//PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc);
 2015|      0|
 2016|      0|	_transponder_report_pub.publish(t);
 2017|      0|}
_ZN15MavlinkReceiver34handle_message_utm_global_positionEP17__mavlink_message:
 2021|      0|{
 2022|      0|	mavlink_utm_global_position_t utm_pos;
 2023|      0|	mavlink_msg_utm_global_position_decode(msg, &utm_pos);
 2024|      0|
 2025|      0|	// Convert cm/s to m/s
 2026|      0|	float vx = utm_pos.vx / 100.0f;
 2027|      0|	float vy = utm_pos.vy / 100.0f;
 2028|      0|	float vz = utm_pos.vz / 100.0f;
 2029|      0|
 2030|      0|	transponder_report_s t{};
 2031|      0|	t.timestamp = hrt_absolute_time();
 2032|      0|	// TODO: ID
 2033|      0|	t.lat = utm_pos.lat * 1e-7;
 2034|      0|	t.lon = utm_pos.lon * 1e-7;
 2035|      0|	t.altitude = utm_pos.alt / 1000.0f;
 2036|      0|	t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
 2037|      0|	// UTM_GLOBAL_POSIION uses NED (north, east, down) coordinates for velocity, in cm / s.
 2038|      0|	t.heading = atan2f(vy, vx);
 2039|      0|	t.hor_velocity = sqrtf(vy * vy + vx * vx);
 2040|      0|	t.ver_velocity = -vz;
 2041|      0|	// TODO: Callsign
 2042|      0|	// For now, set it to all 0s. This is a null-terminated string, so not explicitly giving it a null
 2043|      0|	// terminator could cause problems.
 2044|      0|	memset(&t.callsign[0], 0, sizeof(t.callsign));
 2045|      0|	t.emitter_type = ADSB_EMITTER_TYPE_NO_INFO;  // TODO: Is this correct?
 2046|      0|
 2047|      0|	// The Mavlink docs do not specify what to do if tslc (time since last communication) is out of range of
 2048|      0|	// an 8-bit int, or if this is the first communication.
 2049|      0|	// Here, I assume that if this is the first communication, tslc = 0.
 2050|      0|	// If tslc > 255, then tslc = 255.
 2051|      0|	unsigned long time_passed = (t.timestamp - _last_utm_global_pos_com) / 1000000;
 2052|      0|
 2053|      0|	if (_last_utm_global_pos_com == 0) {
 2054|      0|		time_passed = 0;
 2055|      0|
 2056|      0|	} else if (time_passed > UINT8_MAX) {
 2057|      0|		time_passed = UINT8_MAX;
 2058|      0|	}
 2059|      0|
 2060|      0|	t.tslc = (uint8_t) time_passed;
 2061|      0|
 2062|      0|	t.flags = 0;
 2063|      0|
 2064|      0|	if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) {
 2065|      0|		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS;
 2066|      0|	}
 2067|      0|
 2068|      0|	if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) {
 2069|      0|		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
 2070|      0|	}
 2071|      0|
 2072|      0|	if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) {
 2073|      0|		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING;
 2074|      0|		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
 2075|      0|	}
 2076|      0|
 2077|      0|	// Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not
 2078|      0|	// provide these.
 2079|      0|	_transponder_report_pub.publish(t);
 2080|      0|
 2081|      0|	_last_utm_global_pos_com = t.timestamp;
 2082|      0|}
_ZN15MavlinkReceiver24handle_message_collisionEP17__mavlink_message:
 2086|      0|{
 2087|      0|	mavlink_collision_t collision;
 2088|      0|	mavlink_msg_collision_decode(msg, &collision);
 2089|      0|
 2090|      0|	collision_report_s collision_report{};
 2091|      0|
 2092|      0|	collision_report.timestamp = hrt_absolute_time();
 2093|      0|	collision_report.src = collision.src;
 2094|      0|	collision_report.id = collision.id;
 2095|      0|	collision_report.action = collision.action;
 2096|      0|	collision_report.threat_level = collision.threat_level;
 2097|      0|	collision_report.time_to_minimum_delta = collision.time_to_minimum_delta;
 2098|      0|	collision_report.altitude_minimum_delta = collision.altitude_minimum_delta;
 2099|      0|	collision_report.horizontal_minimum_delta = collision.horizontal_minimum_delta;
 2100|      0|
 2101|      0|	_collision_report_pub.publish(collision_report);
 2102|      0|}
_ZN15MavlinkReceiver28handle_message_gps_rtcm_dataEP17__mavlink_message:
 2106|      0|{
 2107|      0|	mavlink_gps_rtcm_data_t gps_rtcm_data_msg;
 2108|      0|	mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg);
 2109|      0|
 2110|      0|	gps_inject_data_s gps_inject_data_topic{};
 2111|      0|
 2112|      0|	gps_inject_data_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data),
 2113|      0|					      (int)sizeof(uint8_t) * gps_rtcm_data_msg.len);
 2114|      0|	gps_inject_data_topic.flags = gps_rtcm_data_msg.flags;
 2115|      0|	memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
 2116|      0|	       math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len));
 2117|      0|
 2118|      0|	_gps_inject_data_pub.publish(gps_inject_data_topic);
 2119|      0|}
_ZN15MavlinkReceiver35handle_message_hil_state_quaternionEP17__mavlink_message:
 2123|      0|{
 2124|      0|	mavlink_hil_state_quaternion_t hil_state;
 2125|      0|	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 2126|      0|
 2127|      0|	const uint64_t timestamp = hrt_absolute_time();
 2128|      0|
 2129|      0|	/* airspeed */
 2130|      0|	{
 2131|      0|		airspeed_s airspeed{};
 2132|      0|
 2133|      0|		airspeed.timestamp = timestamp;
 2134|      0|		airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
 2135|      0|		airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
 2136|      0|
 2137|      0|		_airspeed_pub.publish(airspeed);
 2138|      0|	}
 2139|      0|
 2140|      0|	/* attitude */
 2141|      0|	{
 2142|      0|		vehicle_attitude_s hil_attitude{};
 2143|      0|
 2144|      0|		hil_attitude.timestamp = timestamp;
 2145|      0|
 2146|      0|		matrix::Quatf q(hil_state.attitude_quaternion);
 2147|      0|		q.copyTo(hil_attitude.q);
 2148|      0|
 2149|      0|		_attitude_pub.publish(hil_attitude);
 2150|      0|	}
 2151|      0|
 2152|      0|	/* global position */
 2153|      0|	{
 2154|      0|		vehicle_global_position_s hil_global_pos{};
 2155|      0|
 2156|      0|		hil_global_pos.timestamp = timestamp;
 2157|      0|		hil_global_pos.lat = hil_state.lat / ((double)1e7);
 2158|      0|		hil_global_pos.lon = hil_state.lon / ((double)1e7);
 2159|      0|		hil_global_pos.alt = hil_state.alt / 1000.0f;
 2160|      0|		hil_global_pos.vel_n = hil_state.vx / 100.0f;
 2161|      0|		hil_global_pos.vel_e = hil_state.vy / 100.0f;
 2162|      0|		hil_global_pos.vel_d = hil_state.vz / 100.0f;
 2163|      0|		hil_global_pos.eph = 2.0f;
 2164|      0|		hil_global_pos.epv = 4.0f;
 2165|      0|
 2166|      0|		_global_pos_pub.publish(hil_global_pos);
 2167|      0|	}
 2168|      0|
 2169|      0|	/* local position */
 2170|      0|	{
 2171|      0|		double lat = hil_state.lat * 1e-7;
 2172|      0|		double lon = hil_state.lon * 1e-7;
 2173|      0|
 2174|      0|		if (!_hil_local_proj_inited) {
 2175|      0|			_hil_local_proj_inited = true;
 2176|      0|			_hil_local_alt0 = hil_state.alt / 1000.0f;
 2177|      0|
 2178|      0|			map_projection_init(&_hil_local_proj_ref, lat, lon);
 2179|      0|		}
 2180|      0|
 2181|      0|		float x = 0.0f;
 2182|      0|		float y = 0.0f;
 2183|      0|		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
 2184|      0|
 2185|      0|		vehicle_local_position_s hil_local_pos{};
 2186|      0|		hil_local_pos.timestamp = timestamp;
 2187|      0|
 2188|      0|		hil_local_pos.ref_timestamp = _hil_local_proj_ref.timestamp;
 2189|      0|		hil_local_pos.ref_lat = math::radians(_hil_local_proj_ref.lat_rad);
 2190|      0|		hil_local_pos.ref_lon = math::radians(_hil_local_proj_ref.lon_rad);
 2191|      0|		hil_local_pos.ref_alt = _hil_local_alt0;
 2192|      0|		hil_local_pos.xy_valid = true;
 2193|      0|		hil_local_pos.z_valid = true;
 2194|      0|		hil_local_pos.v_xy_valid = true;
 2195|      0|		hil_local_pos.v_z_valid = true;
 2196|      0|		hil_local_pos.x = x;
 2197|      0|		hil_local_pos.y = y;
 2198|      0|		hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f;
 2199|      0|		hil_local_pos.vx = hil_state.vx / 100.0f;
 2200|      0|		hil_local_pos.vy = hil_state.vy / 100.0f;
 2201|      0|		hil_local_pos.vz = hil_state.vz / 100.0f;
 2202|      0|
 2203|      0|		matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)};
 2204|      0|		hil_local_pos.yaw = euler.psi();
 2205|      0|		hil_local_pos.xy_global = true;
 2206|      0|		hil_local_pos.z_global = true;
 2207|      0|		hil_local_pos.vxy_max = INFINITY;
 2208|      0|		hil_local_pos.vz_max = INFINITY;
 2209|      0|		hil_local_pos.hagl_min = INFINITY;
 2210|      0|		hil_local_pos.hagl_max = INFINITY;
 2211|      0|
 2212|      0|		_local_pos_pub.publish(hil_local_pos);
 2213|      0|	}
 2214|      0|
 2215|      0|	/* accelerometer */
 2216|      0|	{
 2217|      0|		sensor_accel_s accel{};
 2218|      0|
 2219|      0|		accel.timestamp = timestamp;
 2220|      0|		accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
 2221|      0|		accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f;
 2222|      0|		accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f;
 2223|      0|		accel.x = hil_state.xacc;
 2224|      0|		accel.y = hil_state.yacc;
 2225|      0|		accel.z = hil_state.zacc;
 2226|      0|		accel.temperature = 25.0f;
 2227|      0|
 2228|      0|		_accel_pub.publish(accel);
 2229|      0|	}
 2230|      0|
 2231|      0|	/* gyroscope */
 2232|      0|	{
 2233|      0|		sensor_gyro_s gyro{};
 2234|      0|
 2235|      0|		gyro.timestamp = timestamp;
 2236|      0|		gyro.x_raw = hil_state.rollspeed * 1e3f;
 2237|      0|		gyro.y_raw = hil_state.pitchspeed * 1e3f;
 2238|      0|		gyro.z_raw = hil_state.yawspeed * 1e3f;
 2239|      0|		gyro.x = hil_state.rollspeed;
 2240|      0|		gyro.y = hil_state.pitchspeed;
 2241|      0|		gyro.z = hil_state.yawspeed;
 2242|      0|		gyro.temperature = 25.0f;
 2243|      0|
 2244|      0|		_gyro_pub.publish(gyro);
 2245|      0|	}
 2246|      0|
 2247|      0|	/* battery status */
 2248|      0|	{
 2249|      0|		battery_status_s hil_battery_status{};
 2250|      0|
 2251|      0|		hil_battery_status.timestamp = timestamp;
 2252|      0|		hil_battery_status.voltage_v = 11.1f;
 2253|      0|		hil_battery_status.voltage_filtered_v = 11.1f;
 2254|      0|		hil_battery_status.current_a = 10.0f;
 2255|      0|		hil_battery_status.discharged_mah = -1.0f;
 2256|      0|
 2257|      0|		_battery_pub.publish(hil_battery_status);
 2258|      0|	}
 2259|      0|}
_ZN15MavlinkReceiver32handle_message_named_value_floatEP17__mavlink_message:
 2263|      0|{
 2264|      0|	mavlink_named_value_float_t debug_msg;
 2265|      0|	mavlink_msg_named_value_float_decode(msg, &debug_msg);
 2266|      0|
 2267|      0|	debug_key_value_s debug_topic{};
 2268|      0|
 2269|      0|	debug_topic.timestamp = hrt_absolute_time();
 2270|      0|	memcpy(debug_topic.key, debug_msg.name, sizeof(debug_topic.key));
 2271|      0|	debug_topic.key[sizeof(debug_topic.key) - 1] = '\0'; // enforce null termination
 2272|      0|	debug_topic.value = debug_msg.value;
 2273|      0|
 2274|      0|	_debug_key_value_pub.publish(debug_topic);
 2275|      0|}
_ZN15MavlinkReceiver20handle_message_debugEP17__mavlink_message:
 2279|      0|{
 2280|      0|	mavlink_debug_t debug_msg;
 2281|      0|	mavlink_msg_debug_decode(msg, &debug_msg);
 2282|      0|
 2283|      0|	debug_value_s debug_topic{};
 2284|      0|
 2285|      0|	debug_topic.timestamp = hrt_absolute_time();
 2286|      0|	debug_topic.ind = debug_msg.ind;
 2287|      0|	debug_topic.value = debug_msg.value;
 2288|      0|
 2289|      0|	_debug_value_pub.publish(debug_topic);
 2290|      0|}
_ZN15MavlinkReceiver25handle_message_debug_vectEP17__mavlink_message:
 2294|      0|{
 2295|      0|	mavlink_debug_vect_t debug_msg;
 2296|      0|	mavlink_msg_debug_vect_decode(msg, &debug_msg);
 2297|      0|
 2298|      0|	debug_vect_s debug_topic{};
 2299|      0|
 2300|      0|	debug_topic.timestamp = hrt_absolute_time();
 2301|      0|	memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name));
 2302|      0|	debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination
 2303|      0|	debug_topic.x = debug_msg.x;
 2304|      0|	debug_topic.y = debug_msg.y;
 2305|      0|	debug_topic.z = debug_msg.z;
 2306|      0|
 2307|      0|	_debug_vect_pub.publish(debug_topic);
 2308|      0|}
_ZN15MavlinkReceiver32handle_message_debug_float_arrayEP17__mavlink_message:
 2312|      0|{
 2313|      0|	mavlink_debug_float_array_t debug_msg;
 2314|      0|	mavlink_msg_debug_float_array_decode(msg, &debug_msg);
 2315|      0|
 2316|      0|	debug_array_s debug_topic{};
 2317|      0|
 2318|      0|	debug_topic.timestamp = hrt_absolute_time();
 2319|      0|	debug_topic.id = debug_msg.array_id;
 2320|      0|	memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name));
 2321|      0|	debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination
 2322|      0|
 2323|      0|	for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
 2324|      0|		debug_topic.data[i] = debug_msg.data[i];
 2325|      0|	}
 2326|      0|
 2327|      0|	_debug_array_pub.publish(debug_topic);
 2328|      0|}
_ZN15MavlinkReceiver27handle_message_command_bothI24__mavlink_command_long_tEEvP17__mavlink_messageRKT_RK17vehicle_command_s:
  437|      0|{
  438|      0|	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
  439|      0|
  440|      0|	bool send_ack = true;
  441|      0|	uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
  442|      0|
  443|      0|	if (!target_ok) {
  444|      0|		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
  445|      0|		return;
  446|      0|	}
  447|      0|
  448|      0|	if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
  449|      0|		/* send autopilot version message */
  450|      0|		_mavlink->send_autopilot_capabilites();
  451|      0|
  452|      0|	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
  453|      0|		/* send protocol version message */
  454|      0|		_mavlink->send_protocol_version();
  455|      0|
  456|      0|	} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
  457|      0|		_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
  458|      0|
  459|      0|	} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
  460|      0|		if (set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3)) {
  461|      0|			result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
  462|      0|		}
  463|      0|
  464|      0|	} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
  465|      0|		get_message_interval((int)cmd_mavlink.param1);
  466|      0|
  467|      0|	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) {
  468|      0|		send_flight_information();
  469|      0|
  470|      0|	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_STORAGE_INFORMATION) {
  471|      0|		if ((int)(cmd_mavlink.param2 + 0.5f) == 1) {
  472|      0|			send_storage_information(cmd_mavlink.param1 + 0.5f);
  473|      0|		}
  474|      0|
  475|      0|	} else {
  476|      0|
  477|      0|		send_ack = false;
  478|      0|
  479|      0|		if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
  480|      0|			PX4_WARN("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid);
  481|      0|			return;
  482|      0|		}
  483|      0|
  484|      0|		if (cmd_mavlink.command == MAV_CMD_LOGGING_START) {
  485|      0|			// check that we have enough bandwidth available: this is given by the configured logger topics
  486|      0|			// and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming
  487|      0|			// on a radio link
  488|      0|			if (_mavlink->get_data_rate() < 5000) {
  489|      0|				send_ack = true;
  490|      0|				result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
  491|      0|				_mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming");
  492|      0|
  493|      0|			} else {
  494|      0|				// we already instanciate the streaming object, because at this point we know on which
  495|      0|				// mavlink channel streaming was requested. But in fact it's possible that the logger is
  496|      0|				// not even running. The main mavlink thread takes care of this by waiting for an ack
  497|      0|				// from the logger.
  498|      0|				_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid);
  499|      0|			}
  500|      0|
  501|      0|		} else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
  502|      0|			_mavlink->request_stop_ulog_streaming();
  503|      0|		}
  504|      0|
  505|      0|		if (!send_ack) {
  506|      0|			_cmd_pub.publish(vehicle_command);
  507|      0|		}
  508|      0|	}
  509|      0|
  510|      0|	if (send_ack) {
  511|      0|		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result);
  512|      0|	}
  513|      0|}
_ZN15MavlinkReceiver27handle_message_command_bothI23__mavlink_command_int_tEEvP17__mavlink_messageRKT_RK17vehicle_command_s:
  437|      0|{
  438|      0|	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
  439|      0|
  440|      0|	bool send_ack = true;
  441|      0|	uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
  442|      0|
  443|      0|	if (!target_ok) {
  444|      0|		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
  445|      0|		return;
  446|      0|	}
  447|      0|
  448|      0|	if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
  449|      0|		/* send autopilot version message */
  450|      0|		_mavlink->send_autopilot_capabilites();
  451|      0|
  452|      0|	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
  453|      0|		/* send protocol version message */
  454|      0|		_mavlink->send_protocol_version();
  455|      0|
  456|      0|	} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
  457|      0|		_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
  458|      0|
  459|      0|	} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
  460|      0|		if (set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3)) {
  461|      0|			result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
  462|      0|		}
  463|      0|
  464|      0|	} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
  465|      0|		get_message_interval((int)cmd_mavlink.param1);
  466|      0|
  467|      0|	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) {
  468|      0|		send_flight_information();
  469|      0|
  470|      0|	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_STORAGE_INFORMATION) {
  471|      0|		if ((int)(cmd_mavlink.param2 + 0.5f) == 1) {
  472|      0|			send_storage_information(cmd_mavlink.param1 + 0.5f);
  473|      0|		}
  474|      0|
  475|      0|	} else {
  476|      0|
  477|      0|		send_ack = false;
  478|      0|
  479|      0|		if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
  480|      0|			PX4_WARN("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid);
  481|      0|			return;
  482|      0|		}
  483|      0|
  484|      0|		if (cmd_mavlink.command == MAV_CMD_LOGGING_START) {
  485|      0|			// check that we have enough bandwidth available: this is given by the configured logger topics
  486|      0|			// and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming
  487|      0|			// on a radio link
  488|      0|			if (_mavlink->get_data_rate() < 5000) {
  489|      0|				send_ack = true;
  490|      0|				result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
  491|      0|				_mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming");
  492|      0|
  493|      0|			} else {
  494|      0|				// we already instanciate the streaming object, because at this point we know on which
  495|      0|				// mavlink channel streaming was requested. But in fact it's possible that the logger is
  496|      0|				// not even running. The main mavlink thread takes care of this by waiting for an ack
  497|      0|				// from the logger.
  498|      0|				_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid);
  499|      0|			}
  500|      0|
  501|      0|		} else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
  502|      0|			_mavlink->request_stop_ulog_streaming();
  503|      0|		}
  504|      0|
  505|      0|		if (!send_ack) {
  506|      0|			_cmd_pub.publish(vehicle_command);
  507|      0|		}
  508|      0|	}
  509|      0|
  510|      0|	if (send_ack) {
  511|      0|		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result);
  512|      0|	}
  513|      0|}

_ZN15MavlinkTimesync14handle_messageEPK17__mavlink_message:
   60|      0|{
   61|      0|	switch (msg->msgid) {
   62|      0|	case MAVLINK_MSG_ID_TIMESYNC: {
   63|      0|
   64|      0|			mavlink_timesync_t tsync = {};
   65|      0|			mavlink_msg_timesync_decode(msg, &tsync);
   66|      0|
   67|      0|			const uint64_t now = hrt_absolute_time();
   68|      0|
   69|      0|			if (tsync.tc1 == 0) {			// Message originating from remote system, timestamp and return it
   70|      0|
   71|      0|				mavlink_timesync_t rsync;
   72|      0|
   73|      0|				rsync.tc1 = now * 1000ULL;
   74|      0|				rsync.ts1 = tsync.ts1;
   75|      0|
   76|      0|				mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
   77|      0|
   78|      0|				return;
   79|      0|
   80|      0|			} else if (tsync.tc1 > 0) {		// Message originating from this system, compute time offset from it
   81|      0|
   82|      0|				// Calculate time offset between this system and the remote system, assuming RTT for
   83|      0|				// the timesync packet is roughly equal both ways.
   84|      0|				int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ;
   85|      0|
   86|      0|				// Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system
   87|      0|				uint64_t rtt_us = now - (tsync.ts1 / 1000ULL);
   88|      0|
   89|      0|				// Calculate the difference of this sample from the current estimate
   90|      0|				uint64_t deviation = llabs((int64_t)_time_offset - offset_us);
   91|      0|
   92|      0|				if (rtt_us < MAX_RTT_SAMPLE) {	// Only use samples with low RTT
   93|      0|
   94|      0|					if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) {
   95|      0|
   96|      0|						// Increment the counter if we have a good estimate and are getting samples far from the estimate
   97|      0|						_high_deviation_count++;
   98|      0|
   99|      0|						// We reset the filter if we received 5 consecutive samples which violate our present estimate.
  100|      0|						// This is most likely due to a time jump on the offboard system.
  101|      0|						if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) {
  102|      0|							PX4_ERR("[timesync] Time jump detected. Resetting time synchroniser.");
  103|      0|							// Reset the filter
  104|      0|							reset_filter();
  105|      0|						}
  106|      0|
  107|      0|					} else {
  108|      0|
  109|      0|						// Filter gain scheduling
  110|      0|						if (!sync_converged()) {
  111|      0|							// Interpolate with a sigmoid function
  112|      0|							double progress = (double)_sequence / (double)CONVERGENCE_WINDOW;
  113|      0|							double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress)));
  114|      0|							_filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL;
  115|      0|							_filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL;
  116|      0|
  117|      0|						} else {
  118|      0|							_filter_alpha = ALPHA_GAIN_FINAL;
  119|      0|							_filter_beta = BETA_GAIN_FINAL;
  120|      0|						}
  121|      0|
  122|      0|						// Perform filter update
  123|      0|						add_sample(offset_us);
  124|      0|
  125|      0|						// Increment sequence counter after filter update
  126|      0|						_sequence++;
  127|      0|
  128|      0|						// Reset high deviation count after filter update
  129|      0|						_high_deviation_count = 0;
  130|      0|
  131|      0|						// Reset high RTT count after filter update
  132|      0|						_high_rtt_count = 0;
  133|      0|					}
  134|      0|
  135|      0|				} else {
  136|      0|					// Increment counter if round trip time is too high for accurate timesync
  137|      0|					_high_rtt_count++;
  138|      0|
  139|      0|					if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) {
  140|      0|						PX4_WARN("[timesync] RTT too high for timesync: %llu ms", rtt_us / 1000ULL);
  141|      0|						// Reset counter to rate-limit warnings
  142|      0|						_high_rtt_count = 0;
  143|      0|					}
  144|      0|
  145|      0|				}
  146|      0|
  147|      0|				// Publish status message
  148|      0|				struct timesync_status_s tsync_status = {};
  149|      0|
  150|      0|				tsync_status.timestamp = hrt_absolute_time();
  151|      0|				tsync_status.remote_timestamp = tsync.tc1 / 1000ULL;
  152|      0|				tsync_status.observed_offset = offset_us;
  153|      0|				tsync_status.estimated_offset = (int64_t)_time_offset;
  154|      0|				tsync_status.round_trip_time = rtt_us;
  155|      0|
  156|      0|				if (_timesync_status_pub == nullptr) {
  157|      0|					int instance;
  158|      0|					_timesync_status_pub = orb_advertise_multi(ORB_ID(timesync_status), &tsync_status, &instance, ORB_PRIO_DEFAULT);
  159|      0|
  160|      0|				} else {
  161|      0|					orb_publish(ORB_ID(timesync_status), _timesync_status_pub, &tsync_status);
  162|      0|				}
  163|      0|
  164|      0|			}
  165|      0|
  166|      0|			break;
  167|      0|		}
  168|      0|
  169|      0|	case MAVLINK_MSG_ID_SYSTEM_TIME: {
  170|      0|
  171|      0|			mavlink_system_time_t time;
  172|      0|			mavlink_msg_system_time_decode(msg, &time);
  173|      0|
  174|      0|			timespec tv = {};
  175|      0|			px4_clock_gettime(CLOCK_REALTIME, &tv);
  176|      0|
  177|      0|			// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
  178|      0|			bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS;
  179|      0|			bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
  180|      0|
  181|      0|			if (!onb_unix_valid && ofb_unix_valid) {
  182|      0|				tv.tv_sec = time.time_unix_usec / 1000000ULL;
  183|      0|				tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
  184|      0|
  185|      0|				if (px4_clock_settime(CLOCK_REALTIME, &tv)) {
  186|      0|					PX4_ERR("[timesync] Failed setting realtime clock");
  187|      0|				}
  188|      0|			}
  189|      0|
  190|      0|			break;
  191|      0|		}
  192|      0|
  193|      0|	default:
  194|      0|		break;
  195|      0|	}
  196|      0|}

_ZN9Simulator14handle_messageEPK17__mavlink_message:
  295|      0|{
  296|      0|	switch (msg->msgid) {
  297|      0|	case MAVLINK_MSG_ID_HIL_SENSOR:
  298|      0|		handle_message_hil_sensor(msg);
  299|      0|		break;
  300|      0|
  301|      0|	case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
  302|      0|		handle_message_optical_flow(msg);
  303|      0|		break;
  304|      0|
  305|      0|	case MAVLINK_MSG_ID_ODOMETRY:
  306|      0|		handle_message_odometry(msg);
  307|      0|		break;
  308|      0|
  309|      0|	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
  310|      0|		handle_message_vision_position_estimate(msg);
  311|      0|		break;
  312|      0|
  313|      0|	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
  314|      0|		handle_message_distance_sensor(msg);
  315|      0|		break;
  316|      0|
  317|      0|	case MAVLINK_MSG_ID_HIL_GPS:
  318|      0|		handle_message_hil_gps(msg);
  319|      0|		break;
  320|      0|
  321|      0|	case MAVLINK_MSG_ID_RC_CHANNELS:
  322|      0|		handle_message_rc_channels(msg);
  323|      0|		break;
  324|      0|
  325|      0|	case MAVLINK_MSG_ID_LANDING_TARGET:
  326|      0|		handle_message_landing_target(msg);
  327|      0|		break;
  328|      0|
  329|      0|	case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
  330|      0|		handle_message_hil_state_quaternion(msg);
  331|      0|		break;
  332|      0|	}
  333|      0|}
_ZN9Simulator30handle_message_distance_sensorEPK17__mavlink_message:
  336|      0|{
  337|      0|	mavlink_distance_sensor_t dist;
  338|      0|	mavlink_msg_distance_sensor_decode(msg, &dist);
  339|      0|	publish_distance_topic(&dist);
  340|      0|}
_ZN9Simulator22handle_message_hil_gpsEPK17__mavlink_message:
  343|      0|{
  344|      0|	mavlink_hil_gps_t gps_sim;
  345|      0|	mavlink_msg_hil_gps_decode(msg, &gps_sim);
  346|      0|
  347|      0|	update_gps(&gps_sim);
  348|      0|}
_ZN9Simulator25handle_message_hil_sensorEPK17__mavlink_message:
  351|      0|{
  352|      0|	mavlink_hil_sensor_t imu;
  353|      0|	mavlink_msg_hil_sensor_decode(msg, &imu);
  354|      0|
  355|      0|	struct timespec ts;
  356|      0|	abstime_to_ts(&ts, imu.time_usec);
  357|      0|	px4_clock_settime(CLOCK_MONOTONIC, &ts);
  358|      0|
  359|      0|	hrt_abstime now_us = hrt_absolute_time();
  360|      0|
  361|       |#if 0
  362|       |	// This is just for to debug missing HIL_SENSOR messages.
  363|       |	static hrt_abstime last_time = 0;
  364|       |	hrt_abstime diff = now_us - last_time;
  365|       |	float step = diff / 4000.0f;
  366|       |
  367|       |	if (step > 1.1f || step < 0.9f) {
  368|       |		PX4_INFO("HIL_SENSOR: imu time_usec: %lu, time_usec: %lu, diff: %lu, step: %.2f", imu.time_usec, now_us, diff, step);
  369|       |	}
  370|       |
  371|       |	last_time = now_us;
  372|       |#endif
  373|       |
  374|      0|	update_sensors(now_us, imu);
  375|      0|
  376|      0|	// battery simulation (limit update to 100Hz)
  377|      0|	if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) {
  378|      0|
  379|      0|		const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000;
  380|      0|
  381|      0|		bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
  382|      0|
  383|      0|		if (!armed || batt_sim_start == 0 || batt_sim_start > now_us) {
  384|      0|			batt_sim_start = now_us;
  385|      0|		}
  386|      0|
  387|      0|		float ibatt = -1.0f; // no current sensor in simulation
  388|      0|
  389|      0|		/* Simulate the voltage of a linearly draining battery but stop at the minimum percentage */
  390|      0|		float battery_percentage = 1.0f - (now_us - batt_sim_start) / discharge_interval_us;
  391|      0|
  392|      0|		battery_percentage = math::max(battery_percentage, _battery_min_percentage.get() / 100.f);
  393|      0|		float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage());
  394|      0|		vbatt *= _battery.cell_count();
  395|      0|
  396|      0|		const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
  397|      0|		_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status);
  398|      0|
  399|      0|
  400|      0|		// publish the battery voltage
  401|      0|		int batt_multi;
  402|      0|		orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &batt_multi, ORB_PRIO_HIGH);
  403|      0|	}
  404|      0|}
_ZN9Simulator35handle_message_hil_state_quaternionEPK17__mavlink_message:
  407|      0|{
  408|      0|	mavlink_hil_state_quaternion_t hil_state;
  409|      0|	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
  410|      0|
  411|      0|	uint64_t timestamp = hrt_absolute_time();
  412|      0|
  413|      0|	/* angular velocity */
  414|      0|	vehicle_angular_velocity_s hil_angular_velocity{};
  415|      0|	{
  416|      0|		hil_angular_velocity.timestamp = timestamp;
  417|      0|
  418|      0|		hil_angular_velocity.xyz[0] = hil_state.rollspeed;
  419|      0|		hil_angular_velocity.xyz[1] = hil_state.pitchspeed;
  420|      0|		hil_angular_velocity.xyz[2] = hil_state.yawspeed;
  421|      0|
  422|      0|		// always publish ground truth attitude message
  423|      0|		int hilstate_multi;
  424|      0|		orb_publish_auto(ORB_ID(vehicle_angular_velocity_groundtruth), &_vehicle_angular_velocity_pub, &hil_angular_velocity,
  425|      0|				 &hilstate_multi, ORB_PRIO_HIGH);
  426|      0|	}
  427|      0|
  428|      0|	/* attitude */
  429|      0|	vehicle_attitude_s hil_attitude{};
  430|      0|	{
  431|      0|		hil_attitude.timestamp = timestamp;
  432|      0|
  433|      0|		matrix::Quatf q(hil_state.attitude_quaternion);
  434|      0|		q.copyTo(hil_attitude.q);
  435|      0|
  436|      0|		// always publish ground truth attitude message
  437|      0|		int hilstate_multi;
  438|      0|		orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH);
  439|      0|	}
  440|      0|
  441|      0|	/* global position */
  442|      0|	vehicle_global_position_s hil_gpos{};
  443|      0|	{
  444|      0|		hil_gpos.timestamp = timestamp;
  445|      0|
  446|      0|		hil_gpos.lat = hil_state.lat / 1E7;//1E7
  447|      0|		hil_gpos.lon = hil_state.lon / 1E7;//1E7
  448|      0|		hil_gpos.alt = hil_state.alt / 1E3;//1E3
  449|      0|
  450|      0|		hil_gpos.vel_n = hil_state.vx / 100.0f;
  451|      0|		hil_gpos.vel_e = hil_state.vy / 100.0f;
  452|      0|		hil_gpos.vel_d = hil_state.vz / 100.0f;
  453|      0|
  454|      0|		// always publish ground truth attitude message
  455|      0|		int hil_gpos_multi;
  456|      0|		orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi, ORB_PRIO_HIGH);
  457|      0|	}
  458|      0|
  459|      0|	/* local position */
  460|      0|	struct vehicle_local_position_s hil_lpos = {};
  461|      0|	{
  462|      0|		hil_lpos.timestamp = timestamp;
  463|      0|
  464|      0|		double lat = hil_state.lat * 1e-7;
  465|      0|		double lon = hil_state.lon * 1e-7;
  466|      0|
  467|      0|		if (!_hil_local_proj_inited) {
  468|      0|			_hil_local_proj_inited = true;
  469|      0|			map_projection_init(&_hil_local_proj_ref, lat, lon);
  470|      0|			_hil_ref_timestamp = timestamp;
  471|      0|			_hil_ref_lat = lat;
  472|      0|			_hil_ref_lon = lon;
  473|      0|			_hil_ref_alt = hil_state.alt / 1000.0f;
  474|      0|		}
  475|      0|
  476|      0|		float x;
  477|      0|		float y;
  478|      0|		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
  479|      0|		hil_lpos.timestamp = timestamp;
  480|      0|		hil_lpos.xy_valid = true;
  481|      0|		hil_lpos.z_valid = true;
  482|      0|		hil_lpos.v_xy_valid = true;
  483|      0|		hil_lpos.v_z_valid = true;
  484|      0|		hil_lpos.x = x;
  485|      0|		hil_lpos.y = y;
  486|      0|		hil_lpos.z = _hil_ref_alt - hil_state.alt / 1000.0f;
  487|      0|		hil_lpos.vx = hil_state.vx / 100.0f;
  488|      0|		hil_lpos.vy = hil_state.vy / 100.0f;
  489|      0|		hil_lpos.vz = hil_state.vz / 100.0f;
  490|      0|		matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
  491|      0|		hil_lpos.yaw = euler.psi();
  492|      0|		hil_lpos.xy_global = true;
  493|      0|		hil_lpos.z_global = true;
  494|      0|		hil_lpos.ref_lat = _hil_ref_lat;
  495|      0|		hil_lpos.ref_lon = _hil_ref_lon;
  496|      0|		hil_lpos.ref_alt = _hil_ref_alt;
  497|      0|		hil_lpos.ref_timestamp = _hil_ref_timestamp;
  498|      0|		hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
  499|      0|		hil_lpos.vz_max = std::numeric_limits<float>::infinity();
  500|      0|		hil_lpos.hagl_min = std::numeric_limits<float>::infinity();
  501|      0|		hil_lpos.hagl_max = std::numeric_limits<float>::infinity();
  502|      0|
  503|      0|		// always publish ground truth attitude message
  504|      0|		int hil_lpos_multi;
  505|      0|		orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi, ORB_PRIO_HIGH);
  506|      0|	}
  507|      0|}
_ZN9Simulator29handle_message_landing_targetEPK17__mavlink_message:
  510|      0|{
  511|      0|	mavlink_landing_target_t landing_target_mavlink;
  512|      0|	mavlink_msg_landing_target_decode(msg, &landing_target_mavlink);
  513|      0|
  514|      0|	struct irlock_report_s report = {};
  515|      0|
  516|      0|	report.timestamp = hrt_absolute_time();
  517|      0|	report.signature = landing_target_mavlink.target_num;
  518|      0|	report.pos_x = landing_target_mavlink.angle_x;
  519|      0|	report.pos_y = landing_target_mavlink.angle_y;
  520|      0|	report.size_x = landing_target_mavlink.size_x;
  521|      0|	report.size_y = landing_target_mavlink.size_y;
  522|      0|
  523|      0|	int irlock_multi;
  524|      0|	orb_publish_auto(ORB_ID(irlock_report), &_irlock_report_pub, &report, &irlock_multi, ORB_PRIO_HIGH);
  525|      0|}
_ZN9Simulator23handle_message_odometryEPK17__mavlink_message:
  528|      0|{
  529|      0|	publish_odometry_topic(msg);
  530|      0|}
_ZN9Simulator27handle_message_optical_flowEPK17__mavlink_message:
  533|      0|{
  534|      0|	mavlink_hil_optical_flow_t flow;
  535|      0|	mavlink_msg_hil_optical_flow_decode(msg, &flow);
  536|      0|	publish_flow_topic(&flow);
  537|      0|}
_ZN9Simulator26handle_message_rc_channelsEPK17__mavlink_message:
  540|      0|{
  541|      0|	mavlink_rc_channels_t rc_channels;
  542|      0|	mavlink_msg_rc_channels_decode(msg, &rc_channels);
  543|      0|	fill_rc_input_msg(&_rc_input, &rc_channels);
  544|      0|
  545|      0|	// publish message
  546|      0|	int rc_multi;
  547|      0|	orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH);
  548|      0|}
_ZN9Simulator39handle_message_vision_position_estimateEPK17__mavlink_message:
  551|      0|{
  552|      0|	publish_odometry_topic(msg);
  553|      0|}


Script done on 2019-09-05 07:50:29+02:00 [COMMAND_EXIT_CODE="0"]