mavlink: receiver lazily allocate handlers on first use

src/modules/mavlink/mavlink_receiver.cpp

@@ -72,8 +72,19 @@
 
 using matrix::wrap_2pi;
 
+MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
+	ModuleParams(nullptr),
+	_mavlink(parent)
+{
+}
+
 MavlinkReceiver::~MavlinkReceiver()
 {
+	delete _mavlink_ftp;
+	delete _mavlink_log_handler;
+	delete _mavlink_timesync;
+	delete _mission_manager;
+	delete _parameters_manager;
 	delete _tune_publisher;
 	delete _px4_accel;
 	delete _px4_baro;
@@ -81,17 +92,6 @@ MavlinkReceiver::~MavlinkReceiver()
 	delete _px4_mag;
 }
 
-MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
-	ModuleParams(nullptr),
-	_mavlink(parent),
-	_mavlink_ftp(parent),
-	_mavlink_log_handler(parent),
-	_mission_manager(parent),
-	_parameters_manager(parent),
-	_mavlink_timesync(parent)
-{
-}
-
 void
 MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
 {
@@ -258,6 +258,123 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_statustext(msg);
 		break;
 
+	case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
+		if (_mavlink->ftp_enabled()) {
+			if (_mavlink_ftp == nullptr) {
+				if (!_armed) {
+					_mavlink_ftp = new MavlinkFTP(_mavlink);
+
+					if (_mavlink_ftp == nullptr) {
+						_mavlink->send_statustext_critical("failed to start Mavlink FTP");
+					}
+
+				} else {
+					_mavlink->send_statustext_critical("must be disarmed to start Mavlink FTP");
+				}
+			}
+
+			if (_mavlink_ftp != nullptr) {
+				_mavlink_ftp->handle_message(msg);
+			}
+		}
+
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_ACK:
+	case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
+	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+	case MAVLINK_MSG_ID_MISSION_REQUEST:
+	case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
+	case MAVLINK_MSG_ID_MISSION_COUNT:
+	case MAVLINK_MSG_ID_MISSION_ITEM:
+	case MAVLINK_MSG_ID_MISSION_ITEM_INT:
+	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+		if (_mission_manager == nullptr) {
+			if (!_armed) {
+				_mission_manager = new MavlinkMissionManager(_mavlink);
+
+				if (_mission_manager == nullptr) {
+					_mavlink->send_statustext_critical("failed to start Mavlink mission manager");
+				}
+
+			} else {
+				_mavlink->send_statustext_critical("must be disarmed to start Mavlink mission manager");
+			}
+		}
+
+		if (_mission_manager != nullptr) {
+			_mission_manager->handle_message(msg);
+		}
+
+		break;
+
+	case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
+	case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
+	case MAVLINK_MSG_ID_LOG_ERASE:
+	case MAVLINK_MSG_ID_LOG_REQUEST_END:
+		if (_mavlink_log_handler == nullptr) {
+			if (!_armed) {
+				_mavlink_log_handler = new MavlinkLogHandler(_mavlink);
+
+				if (_mavlink_log_handler == nullptr) {
+					_mavlink->send_statustext_critical("failed to start Mavlink log handler");
+				}
+
+			} else {
+				_mavlink->send_statustext_critical("must be disarmed to start Mavlink log handler");
+			}
+		}
+
+		if (_mavlink_log_handler != nullptr) {
+			_mavlink_log_handler->handle_message(msg);
+		}
+
+		break;
+
+	case MAVLINK_MSG_ID_TIMESYNC:
+	case MAVLINK_MSG_ID_SYSTEM_TIME:
+		if (_mavlink_timesync == nullptr) {
+			if (!_armed) {
+				_mavlink_timesync = new MavlinkTimesync(_mavlink);
+
+				if (_mavlink_timesync == nullptr) {
+					_mavlink->send_statustext_critical("failed to start Mavlink time sync");
+				}
+
+			} else {
+				_mavlink->send_statustext_critical("must be disarmed to start Mavlink time sync");
+			}
+		}
+
+		if (_mavlink_timesync != nullptr) {
+			_mavlink_timesync->handle_message(msg);
+		}
+
+		break;
+
+	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+	case MAVLINK_MSG_ID_PARAM_SET:
+	case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
+	case MAVLINK_MSG_ID_PARAM_MAP_RC:
+		if (_parameters_manager == nullptr) {
+			if (!_armed) {
+				_parameters_manager = new MavlinkParametersManager(_mavlink);
+
+				if (_parameters_manager == nullptr) {
+					_mavlink->send_statustext_critical("failed to start Mavlink parameter manager");
+				}
+
+			} else {
+				_mavlink->send_statustext_critical("must be disarmed to start Mavlink parameter manager");
+			}
+		}
+
+		if (_parameters_manager != nullptr) {
+			_parameters_manager->handle_message(msg);
+		}
+
+		break;
+
 #if !defined(CONSTRAINED_FLASH)
 
 	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
@@ -753,7 +870,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 	vehicle_odometry_s mocap_odom{};
 
 	mocap_odom.timestamp = hrt_absolute_time();
-	mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec);
+	mocap_odom.timestamp_sample = sync_timestamp(mocap.time_usec);
 
 	mocap_odom.x = mocap.x;
 	mocap_odom.y = mocap.y;
@@ -1241,7 +1358,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	vehicle_odometry_s visual_odom{};
 
 	visual_odom.timestamp = hrt_absolute_time();
-	visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec);
+	visual_odom.timestamp_sample = sync_timestamp(ev.usec);
 
 	visual_odom.x = ev.x;
 	visual_odom.y = ev.y;
@@ -1280,7 +1397,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 	vehicle_odometry_s odometry{};
 
 	odometry.timestamp = hrt_absolute_time();
-	odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec);
+	odometry.timestamp_sample = sync_timestamp(odom.time_usec);
 
 	/* The position is in a local FRD frame */
 	odometry.x = odom.x;
@@ -1838,7 +1955,7 @@ MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message
 
 	vehicle_trajectory_bezier_s trajectory_bezier{};
 
-	trajectory_bezier.timestamp =  _mavlink_timesync.sync_stamp(trajectory.time_usec);
+	trajectory_bezier.timestamp = hrt_absolute_time();
 
 	for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) {
 		trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i];
@@ -2369,7 +2486,7 @@ MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg)
 	if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) {
 		landing_target_pose_s landing_target_pose{};
 
-		landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec);
+		landing_target_pose.timestamp = sync_timestamp(landing_target.time_usec);
 		landing_target_pose.abs_pos_valid = true;
 		landing_target_pose.x_abs = landing_target.x;
 		landing_target_pose.y_abs = landing_target.y;
@@ -2888,6 +3005,15 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
 	}
 }
 
+uint64_t MavlinkReceiver::sync_timestamp(uint64_t timestamp)
+{
+	if (_mavlink_timesync != nullptr) {
+		return _mavlink_timesync->sync_stamp(timestamp);
+	}
+
+	return hrt_absolute_time();
+}
+
 /**
  * Receive data from UART/UDP
  */
@@ -2959,6 +3085,14 @@ MavlinkReceiver::Run()
 			updateParams();
 		}
 
+		if (_vehicle_status_sub.updated()) {
+			vehicle_status_s vehicle_status;
+
+			if (_vehicle_status_sub.copy(&vehicle_status)) {
+				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			}
+		}
+
 		int ret = poll(&fds[0], 1, timeout);
 
 		if (ret > 0) {
@@ -3019,24 +3153,6 @@ MavlinkReceiver::Run()
 						/* handle generic messages and commands */
 						handle_message(&msg);
 
-						/* handle packet with mission manager */
-						_mission_manager.handle_message(&msg);
-
-
-						/* handle packet with parameter component */
-						_parameters_manager.handle_message(&msg);
-
-						if (_mavlink->ftp_enabled()) {
-							/* handle packet with ftp component */
-							_mavlink_ftp.handle_message(&msg);
-						}
-
-						/* handle packet with log component */
-						_mavlink_log_handler.handle_message(&msg);
-
-						/* handle packet with timesync component */
-						_mavlink_timesync.handle_message(&msg);
-
 						/* handle packet with parent object */
 						_mavlink->handle_message(&msg);
 					}
@@ -3061,16 +3177,23 @@ MavlinkReceiver::Run()
 		CheckHeartbeats(t);
 
 		if (t - last_send_update > timeout * 1000) {
-			_mission_manager.check_active_mission();
-			_mission_manager.send();
-
-			_parameters_manager.send();
-
-			if (_mavlink->ftp_enabled()) {
-				_mavlink_ftp.send();
+			if (_mission_manager) {
+				_mission_manager->check_active_mission();
+				_mission_manager->send();
+			}
+
+			if (_parameters_manager) {
+				_parameters_manager->send();
+			}
+
+			if (_mavlink_ftp) {
+				_mavlink_ftp->send();
+			}
+
+			if (_mavlink_log_handler) {
+				_mavlink_log_handler->send();
 			}
 
-			_mavlink_log_handler.send();
 			last_send_update = t;
 		}
 

src/modules/mavlink/mavlink_receiver.h

@@ -226,13 +226,15 @@ private:
 	 */
 	void update_params();
 
+	uint64_t sync_timestamp(uint64_t timestamp);
+
 	Mavlink				*_mavlink;
 
-	MavlinkFTP			_mavlink_ftp;
-	MavlinkLogHandler		_mavlink_log_handler;
-	MavlinkMissionManager		_mission_manager;
-	MavlinkParametersManager	_parameters_manager;
-	MavlinkTimesync			_mavlink_timesync;
+	MavlinkFTP			*_mavlink_ftp{nullptr};
+	MavlinkLogHandler		*_mavlink_log_handler{nullptr};
+	MavlinkMissionManager		*_mission_manager{nullptr};
+	MavlinkParametersManager	*_parameters_manager{nullptr};
+	MavlinkTimesync			*_mavlink_timesync{nullptr};
 
 	mavlink_status_t		_status{}; ///< receiver status, used for mavlink_parse_char()
 
@@ -289,7 +291,6 @@ private:
 	uORB::PublicationQueued<vehicle_command_s>	_cmd_pub{ORB_ID(vehicle_command)};
 
 	// ORB subscriptions
-	uORB::Subscription	_actuator_armed_sub{ORB_ID(actuator_armed)};
 	uORB::Subscription	_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription	_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -342,6 +343,8 @@ private:
 	hrt_abstime _heartbeat_component_udp_bridge{0};
 	hrt_abstime _heartbeat_component_uart_bridge{0};
 
+	bool _armed{false};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,