diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index d9ba17ea6c..4a504107a2 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -61,6 +61,8 @@ uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started +uint32 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data +uint32 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b7be1a8ccc..55c7c15781 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1122,6 +1122,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND: case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: + case vehicle_command_s::VEHICLE_CMD_LOGGING_START: + case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP: /* ignore commands that handled in low prio loop */ break; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index a4babddf51..593c0d7e7e 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -47,6 +47,8 @@ #include #include #include +#include +#include #include #include @@ -655,6 +657,13 @@ void Logger::run() add_default_topics(); } + int vehicle_command_sub = -1; + orb_advert_t vehicle_command_ack_pub = nullptr; + + if (_writer.backend() & LogWriter::BackendMavlink) { + vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + } + //all topics added. Get required message buffer size int max_msg_size = 0; @@ -738,6 +747,39 @@ void Logger::run() } } + /* check for logging command from MAVLink */ + if (vehicle_command_sub != -1) { + bool command_updated = false; + int ret = orb_check(vehicle_command_sub, &command_updated); + + if (ret == 0 && command_updated) { + vehicle_command_s command; + orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &command); + + if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + if ((int)(command.param1 + 0.5f) != 0) { + ack_vehicle_command(vehicle_command_ack_pub, command.command, + vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + + } else if (can_start_mavlink_log()) { + ack_vehicle_command(vehicle_command_ack_pub, command.command, + vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + start_log_mavlink(); + + } else { + ack_vehicle_command(vehicle_command_ack_pub, command.command, + vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + } + + } else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) { + stop_log_mavlink(); + ack_vehicle_command(vehicle_command_ack_pub, command.command, + vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + } + } + } + + if (_writer.is_started()) { bool data_written = false; @@ -871,6 +913,14 @@ void Logger::run() orb_unadvertise(_mavlink_log_pub); _mavlink_log_pub = nullptr; } + + if (vehicle_command_ack_pub) { + orb_unadvertise(vehicle_command_ack_pub); + } + + if (vehicle_command_sub != -1) { + orb_unsubscribe(vehicle_command_sub); + } } bool Logger::write_message(void *ptr, size_t size) @@ -1101,6 +1151,7 @@ void Logger::start_log_file() mavlink_log_info(&_mavlink_log_pub, "[logger] file: %s", file_name); _writer.start_log_file(file_name); + _writer.select_write_backend(LogWriter::BackendFile); _writer.set_need_reliable_transfer(true); write_header(); write_version(); @@ -1108,6 +1159,7 @@ void Logger::start_log_file() write_parameters(); write_all_add_logged_msg(); _writer.set_need_reliable_transfer(false); + _writer.unselect_write_backend(); _writer.notify(); _start_time_file = hrt_absolute_time(); @@ -1118,6 +1170,33 @@ void Logger::stop_log_file() _writer.stop_log_file(); } +void Logger::start_log_mavlink() +{ + if (!can_start_mavlink_log()) { + return; + } + + PX4_INFO("Start mavlink log"); + + _writer.start_log_mavlink(); + _writer.select_write_backend(LogWriter::BackendMavlink); + _writer.set_need_reliable_transfer(true); + write_header(); + write_version(); + write_formats(); + write_parameters(); + write_all_add_logged_msg(); + _writer.set_need_reliable_transfer(false); + _writer.unselect_write_backend(); + _writer.notify(); +} + +void Logger::stop_log_mavlink() +{ + PX4_INFO("Stop mavlink log"); + _writer.stop_log_mavlink(); +} + void Logger::write_formats() { _writer.lock(); @@ -1393,5 +1472,22 @@ int Logger::check_free_space() return PX4_OK; } +void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result) +{ + vehicle_command_ack_s vehicle_command_ack; + vehicle_command_ack.timestamp = hrt_absolute_time(); + vehicle_command_ack.command = command; + vehicle_command_ack.result = result; + + if (vehicle_command_ack_pub == nullptr) { + vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack, + vehicle_command_ack_s::ORB_QUEUE_LENGTH); + + } else { + orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack); + } + +} + } } diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 8de1e286b4..019256d6d8 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -160,6 +160,15 @@ private: void stop_log_file(); + void start_log_mavlink(); + + void stop_log_mavlink(); + + /** check if mavlink logging can be started */ + bool can_start_mavlink_log() const + { + return !_writer.is_started(LogWriter::BackendMavlink) && (_writer.backend() & LogWriter::BackendMavlink) != 0; + } /** get the configured backend as string */ const char *configured_backend_mode() const; @@ -206,6 +215,8 @@ private: void add_default_topics(); + void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result); + static constexpr size_t MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */ static constexpr unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */