Browse Source

logger: send post-flight perf data when stopping mavlink log streaming

- send out VEHICLE_CMD_RESULT_IN_PROGRESS
- delete the ulog object when receiving the ACK from logger, instead of
  the stop command
master
Beat Küng 4 years ago committed by Daniel Agar
parent
commit
e1ac6fe297
  1. 13
      src/modules/logger/logger.cpp
  2. 8
      src/modules/mavlink/mavlink_main.cpp
  3. 6
      src/modules/mavlink/mavlink_main.h
  4. 3
      src/modules/mavlink/mavlink_receiver.cpp

13
src/modules/logger/logger.cpp

@ -1124,7 +1124,11 @@ void Logger::handle_vehicle_command_update()
} }
} else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) { } else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) {
if (_writer.is_started(LogType::Full, LogWriter::BackendMavlink)) {
ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_IN_PROGRESS);
stop_log_mavlink(); stop_log_mavlink();
}
ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} }
} }
@ -1388,8 +1392,17 @@ void Logger::stop_log_mavlink()
{ {
// don't write perf data since a client does not expect more data after a stop command // don't write perf data since a client does not expect more data after a stop command
PX4_INFO("Stop mavlink log"); PX4_INFO("Stop mavlink log");
if (_writer.is_started(LogType::Full, LogWriter::BackendMavlink)) {
_writer.select_write_backend(LogWriter::BackendMavlink);
_writer.set_need_reliable_transfer(true);
write_perf_data(false);
_writer.set_need_reliable_transfer(false);
_writer.unselect_write_backend();
_writer.notify();
_writer.stop_log_mavlink(); _writer.stop_log_mavlink();
} }
}
struct perf_callback_data_t { struct perf_callback_data_t {
Logger *logger; Logger *logger;

8
src/modules/mavlink/mavlink_main.cpp

@ -2381,6 +2381,7 @@ Mavlink::task_main(int argc, char *argv[])
/* send command ACK */ /* send command ACK */
bool cmd_logging_start_acknowledgement = false; bool cmd_logging_start_acknowledgement = false;
bool cmd_logging_stop_acknowledgement = false;
if (_vehicle_command_ack_sub.updated()) { if (_vehicle_command_ack_sub.updated()) {
static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@ -2412,6 +2413,10 @@ Mavlink::task_main(int argc, char *argv[])
if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
cmd_logging_start_acknowledgement = true; cmd_logging_start_acknowledgement = true;
} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
&& command_ack.result == vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
cmd_logging_stop_acknowledgement = true;
} }
} }
} }
@ -2453,10 +2458,9 @@ Mavlink::task_main(int argc, char *argv[])
/* check for ulog streaming messages */ /* check for ulog streaming messages */
if (_mavlink_ulog) { if (_mavlink_ulog) {
if (_mavlink_ulog_stop_requested.load()) { if (cmd_logging_stop_acknowledgement) {
_mavlink_ulog->stop(); _mavlink_ulog->stop();
_mavlink_ulog = nullptr; _mavlink_ulog = nullptr;
_mavlink_ulog_stop_requested.store(false);
} else { } else {
if (cmd_logging_start_acknowledgement) { if (cmd_logging_start_acknowledgement) {

6
src/modules/mavlink/mavlink_main.h

@ -498,10 +498,6 @@ public:
_mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component); _mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component);
} }
void request_stop_ulog_streaming()
{
if (_mavlink_ulog) { _mavlink_ulog_stop_requested.store(true); }
}
const events::SendProtocol &get_events_protocol() const { return _events; }; const events::SendProtocol &get_events_protocol() const { return _events; };
bool ftp_enabled() const { return _ftp_on; } bool ftp_enabled() const { return _ftp_on; }
@ -575,8 +571,6 @@ private:
static events::EventBuffer *_event_buffer; static events::EventBuffer *_event_buffer;
events::SendProtocol _events{*_event_buffer, *this}; events::SendProtocol _events{*_event_buffer, *this};
px4::atomic_bool _mavlink_ulog_stop_requested{false};
MAVLINK_MODE _mode{MAVLINK_MODE_NORMAL}; MAVLINK_MODE _mode{MAVLINK_MODE_NORMAL};
mavlink_channel_t _channel{MAVLINK_COMM_0}; mavlink_channel_t _channel{MAVLINK_COMM_0};

3
src/modules/mavlink/mavlink_receiver.cpp

@ -594,9 +594,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
// from the logger. // from the logger.
_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid); _mavlink->try_start_ulog_streaming(msg->sysid, msg->compid);
} }
} else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
_mavlink->request_stop_ulog_streaming();
} }
if (!send_ack) { if (!send_ack) {

Loading…
Cancel
Save