bizhang_-obav/src/modules/mavlink/mavlink_receiver.h

/****************************************************************************
 *
 *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file mavlink_receiver.h
 * MAVLink receiver thread
 *
 * @author Lorenz Meier <lorenz@px4.io>
 * @author Anton Babushkin <anton@px4.io>
 */

#pragma once

#include "mavlink_ftp.h"
#include "mavlink_log_handler.h"
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
#include "mavlink_timesync.h"
#include "tune_publisher.h"

#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cellular_status.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
#include <uORB/topics/generator_status.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_manager_set_manual_control.h>
#include <uORB/topics/gimbal_device_information.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/irlock_report.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/log_message.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/onboard_computer_status.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/ping.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/tune_control.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_bezier.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>

#if !defined(CONSTRAINED_FLASH)
# include <uORB/topics/debug_array.h>
# include <uORB/topics/debug_key_value.h>
# include <uORB/topics/debug_value.h>
# include <uORB/topics/debug_vect.h>
#endif // !CONSTRAINED_FLASH

using namespace time_literals;

class Mavlink;

class MavlinkReceiver : public ModuleParams
{
public:
	MavlinkReceiver(Mavlink *parent);
	~MavlinkReceiver() override;

	void start();
	void stop();

	void print_detailed_rx_stats() const;

private:
	static void *start_trampoline(void *context);
	void run();

	void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result);

	/**
	 * Common method to handle both mavlink command types. T is one of mavlink_command_int_t or mavlink_command_long_t.
	 */
	template<class T>
	void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
					 const vehicle_command_s &vehicle_command);

	uint8_t handle_request_message_command(uint16_t message_id, float param2 = 0.0f, float param3 = 0.0f,
					       float param4 = 0.0f,
					       float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);

	void handle_message(mavlink_message_t *msg);

	void handle_message_adsb_vehicle(mavlink_message_t *msg);
	void handle_message_att_pos_mocap(mavlink_message_t *msg);
	void handle_message_battery_status(mavlink_message_t *msg);
	void handle_message_cellular_status(mavlink_message_t *msg);
	void handle_message_collision(mavlink_message_t *msg);
	void handle_message_command_ack(mavlink_message_t *msg);
	void handle_message_command_int(mavlink_message_t *msg);
	void handle_message_command_long(mavlink_message_t *msg);
	void handle_message_distance_sensor(mavlink_message_t *msg);
	void handle_message_follow_target(mavlink_message_t *msg);
	void handle_message_generator_status(mavlink_message_t *msg);
	void handle_message_set_gps_global_origin(mavlink_message_t *msg);
	void handle_message_gps_rtcm_data(mavlink_message_t *msg);
	void handle_message_heartbeat(mavlink_message_t *msg);
	void handle_message_hil_gps(mavlink_message_t *msg);
	void handle_message_hil_optical_flow(mavlink_message_t *msg);
	void handle_message_hil_sensor(mavlink_message_t *msg);
	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
	void handle_message_landing_target(mavlink_message_t *msg);
	void handle_message_logging_ack(mavlink_message_t *msg);
	void handle_message_manual_control(mavlink_message_t *msg);
	void handle_message_obstacle_distance(mavlink_message_t *msg);
	void handle_message_odometry(mavlink_message_t *msg);
	void handle_message_onboard_computer_status(mavlink_message_t *msg);
	void handle_message_optical_flow_rad(mavlink_message_t *msg);
	void handle_message_ping(mavlink_message_t *msg);
	void handle_message_play_tune(mavlink_message_t *msg);
	void handle_message_play_tune_v2(mavlink_message_t *msg);
	void handle_message_radio_status(mavlink_message_t *msg);
	void handle_message_rc_channels_override(mavlink_message_t *msg);
	void handle_message_serial_control(mavlink_message_t *msg);
	void handle_message_set_actuator_control_target(mavlink_message_t *msg);
	void handle_message_set_attitude_target(mavlink_message_t *msg);
	void handle_message_set_mode(mavlink_message_t *msg);
	void handle_message_set_position_target_global_int(mavlink_message_t *msg);
	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
	void handle_message_statustext(mavlink_message_t *msg);
	void handle_message_trajectory_representation_bezier(mavlink_message_t *msg);
	void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
	void handle_message_utm_global_position(mavlink_message_t *msg);
	void handle_message_vision_position_estimate(mavlink_message_t *msg);
	void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
	void handle_message_gimbal_device_information(mavlink_message_t *msg);

#if !defined(CONSTRAINED_FLASH)
	void handle_message_debug(mavlink_message_t *msg);
	void handle_message_debug_float_array(mavlink_message_t *msg);
	void handle_message_debug_vect(mavlink_message_t *msg);
	void handle_message_named_value_float(mavlink_message_t *msg);
#endif // !CONSTRAINED_FLASH

	void CheckHeartbeats(const hrt_abstime &t, bool force = false);

	/**
	 * Set the interval at which the given message stream is published.
	 * The rate is the number of messages per second.
	 *
	 * @param msgId The ID of the message interval to be set.
	 * @param interval The interval in usec to send the message.
	 * @param data_rate The total link data rate in bytes per second.
	 *
	 * @return PX4_OK on success, PX4_ERROR on fail.
	 */
	int set_message_interval(int msgId, float interval, int data_rate = -1);
	void get_message_interval(int msgId);

	/**
	 * Decode a switch position from a bitfield and state.
	 */
	int decode_switch_pos_n(uint16_t buttons, unsigned sw);

	bool evaluate_target_ok(int command, int target_system, int target_component);

	void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);

	void schedule_tune(const char *tune);

	void update_rx_stats(const mavlink_message_t &message);

	px4::atomic_bool 	_should_exit{false};
	pthread_t		_thread {};

	Mavlink				*_mavlink;

	MavlinkFTP			_mavlink_ftp;
	MavlinkLogHandler		_mavlink_log_handler;
	MavlinkMissionManager		_mission_manager;
	MavlinkParametersManager	_parameters_manager;
	MavlinkTimesync			_mavlink_timesync;

	mavlink_status_t		_status{}; ///< receiver status, used for mavlink_parse_char()

	orb_advert_t _mavlink_log_pub{nullptr};

	static constexpr int MAX_REMOTE_COMPONENTS{8};
	struct ComponentState {
		uint32_t last_time_received_ms{0};
		uint32_t received_messages{0};
		uint32_t missed_messages{0};
		uint8_t system_id{0};
		uint8_t component_id{0};
		uint8_t last_sequence{0};
	};
	ComponentState _component_states[MAX_REMOTE_COMPONENTS] {};
	bool _warned_component_states_full_once{false};

	uint64_t _total_received_counter{0};                            ///< The total number of successfully received messages
	uint64_t _total_lost_counter{0};                                ///< Total messages lost during transmission.

	uint8_t _mavlink_status_last_buffer_overrun{0};
	uint8_t _mavlink_status_last_parse_error{0};
	uint16_t _mavlink_status_last_packet_rx_drop_count{0};

	// ORB publications
	uORB::Publication<actuator_controls_s>			_actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)};
	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};
	uORB::Publication<follow_target_s>			_follow_target_pub{ORB_ID(follow_target)};
	uORB::Publication<gimbal_manager_set_attitude_s>	_gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
	uORB::Publication<gimbal_manager_set_manual_control_s>	_gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)};
	uORB::Publication<gimbal_device_information_s>		_gimbal_device_information_pub{ORB_ID(gimbal_device_information)};
	uORB::Publication<irlock_report_s>			_irlock_report_pub{ORB_ID(irlock_report)};
	uORB::Publication<landing_target_pose_s>		_landing_target_pose_pub{ORB_ID(landing_target_pose)};
	uORB::Publication<log_message_s>			_log_message_pub{ORB_ID(log_message)};
	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};
	uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
	uORB::Publication<optical_flow_s>			_flow_pub{ORB_ID(optical_flow)};
	uORB::Publication<sensor_gps_s>				_sensor_gps_pub{ORB_ID(sensor_gps)};
	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
	uORB::Publication<vehicle_attitude_setpoint_s>		_mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
	uORB::Publication<vehicle_attitude_setpoint_s>		_fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
	uORB::Publication<vehicle_global_position_s>		_global_pos_pub{ORB_ID(vehicle_global_position)};
	uORB::Publication<vehicle_land_detected_s>		_land_detector_pub{ORB_ID(vehicle_land_detected)};
	uORB::Publication<vehicle_local_position_s>		_local_pos_pub{ORB_ID(vehicle_local_position)};
	uORB::Publication<vehicle_local_position_setpoint_s>	_trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
	uORB::Publication<vehicle_odometry_s>			_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
	uORB::Publication<vehicle_odometry_s>			_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
	uORB::Publication<vehicle_rates_setpoint_s>		_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
	uORB::Publication<vehicle_trajectory_bezier_s>		_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
	uORB::Publication<vehicle_trajectory_waypoint_s>	_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};

#if !defined(CONSTRAINED_FLASH)
	uORB::Publication<debug_array_s>			_debug_array_pub {ORB_ID(debug_array)};
	uORB::Publication<debug_key_value_s>			_debug_key_value_pub{ORB_ID(debug_key_value)};
	uORB::Publication<debug_value_s>			_debug_value_pub{ORB_ID(debug_value)};
	uORB::Publication<debug_vect_s>				_debug_vect_pub{ORB_ID(debug_vect)};
#endif // !CONSTRAINED_FLASH

	// ORB publications (multi)
	uORB::PublicationMulti<distance_sensor_s>		_distance_sensor_pub{ORB_ID(distance_sensor)};
	uORB::PublicationMulti<distance_sensor_s>		_flow_distance_sensor_pub{ORB_ID(distance_sensor)};
	uORB::PublicationMulti<input_rc_s>			_rc_pub{ORB_ID(input_rc)};
	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
	uORB::PublicationMulti<ping_s>				_ping_pub{ORB_ID(ping)};
	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status)};

	// ORB publications (queue length > 1)
	uORB::Publication<gps_inject_data_s>     _gps_inject_data_pub{ORB_ID(gps_inject_data)};
	uORB::Publication<transponder_report_s>  _transponder_report_pub{ORB_ID(transponder_report)};
	uORB::Publication<vehicle_command_s>     _cmd_pub{ORB_ID(vehicle_command)};
	uORB::Publication<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};

	// ORB subscriptions
	uORB::Subscription	_actuator_armed_sub{ORB_ID(actuator_armed)};
	uORB::Subscription	_home_position_sub{ORB_ID(home_position)};
	uORB::Subscription	_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
	uORB::Subscription	_vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
	uORB::Subscription	_vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
	uORB::Subscription 	_actuator_controls_3_sub{ORB_ID(actuator_controls_3)};

	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

	// hil_sensor and hil_state_quaternion
	enum SensorSource {
		ACCEL		= 0b111,
		GYRO		= 0b111000,
		MAG		= 0b111000000,
		BARO		= 0b1101000000000,
		DIFF_PRESS	= 0b10000000000
	};
	PX4Accelerometer *_px4_accel{nullptr};
	PX4Barometer *_px4_baro{nullptr};
	PX4Gyroscope *_px4_gyro{nullptr};
	PX4Magnetometer *_px4_mag{nullptr};

	static constexpr unsigned int	MOM_SWITCH_COUNT{8};
	uint8_t				_mom_switch_pos[MOM_SWITCH_COUNT] {};
	uint16_t			_mom_switch_state{0};

	hrt_abstime			_last_utm_global_pos_com{0};

	// Allocated if needed.
	TunePublisher *_tune_publisher{nullptr};

	hrt_abstime _last_heartbeat_check{0};

	hrt_abstime _heartbeat_type_antenna_tracker{0};
	hrt_abstime _heartbeat_type_gcs{0};
	hrt_abstime _heartbeat_type_onboard_controller{0};
	hrt_abstime _heartbeat_type_gimbal{0};
	hrt_abstime _heartbeat_type_adsb{0};
	hrt_abstime _heartbeat_type_camera{0};

	hrt_abstime _heartbeat_component_telemetry_radio{0};
	hrt_abstime _heartbeat_component_log{0};
	hrt_abstime _heartbeat_component_osd{0};
	hrt_abstime _heartbeat_component_obstacle_avoidance{0};
	hrt_abstime _heartbeat_component_visual_inertial_odometry{0};
	hrt_abstime _heartbeat_component_pairing_manager{0};
	hrt_abstime _heartbeat_component_udp_bridge{0};
	hrt_abstime _heartbeat_component_uart_bridge{0};

	DEFINE_PARAMETERS(
		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,
		(ParamFloat<px4::params::BAT_LOW_THR>)      _param_bat_low_thr,
		(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
		(ParamFloat<px4::params::SENS_FLOW_MAXR>)   _param_sens_flow_maxr,
		(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
		(ParamInt<px4::params::SENS_FLOW_ROT>)      _param_sens_flow_rot
	);

	// Disallow copy construction and move assignment.
	MavlinkReceiver(const MavlinkReceiver &) = delete;
	MavlinkReceiver operator=(const MavlinkReceiver &) = delete;
};