diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5e99247a31..355c5529ad 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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; } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9873d7a9a8..f89fa5509d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/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 _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) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr,