Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

logger move non-logged subscriptions to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-08-06 02:02:24 -04:00
parent 9d701a077d
commit 5b511eaa1a
3 changed files with 43 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/logger/logger.cpp
+24 -62
View File
@@ -46,12 +46,8 @@
#include <uORB/PublicationQueued.hpp>
#include <uORB/uORBTopics.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/log_message.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <drivers/drv_hrt.h>
#include <mathlib/math/Limits.hpp>
@@ -898,10 +894,7 @@ void Logger::run()
}
}
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
uORB::SubscriptionData<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
int log_message_sub = orb_subscribe(ORB_ID(log_message));
orb_set_interval(log_message_sub, 20);
uORB::Subscription parameter_update_sub(ORB_ID(parameter_update));
// mission log topics if enabled (must be added first)
int32_t mission_log_mode = 0;
@@ -919,12 +912,6 @@ void Logger::run()
}
}
int manual_control_sp_sub = -1;
if (_log_mode == LogMode::rc_aux1) {
manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
}
int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt");
if (ntopics > 0) {
@@ -934,15 +921,8 @@ void Logger::run()
initialize_configured_topics();
}
int vehicle_command_sub = -1;
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;
int ret = 0;
for (const auto &subscription : _subscriptions) {
//use o_size, because that's what orb_copy will use
@@ -1029,8 +1009,7 @@ void Logger::run()
while (!should_exit()) {
// Start/stop logging (depending on logging mode, by default when arming/disarming)
const bool logging_started = start_stop_logging(vehicle_status_sub, manual_control_sp_sub,
(MissionLogType)mission_log_mode);
const bool logging_started = start_stop_logging((MissionLogType)mission_log_mode);
if (logging_started) {
#ifdef DBGPRINT
@@ -1040,10 +1019,7 @@ void Logger::run()
}
/* check for logging command from MAVLink (start/stop streaming) */
if (vehicle_command_sub >= 0) {
handle_vehicle_command_update(vehicle_command_sub);
}
handle_vehicle_command_update();
if (timer_callback_data.watchdog_triggered) {
timer_callback_data.watchdog_triggered = false;
@@ -1071,7 +1047,9 @@ void Logger::run()
/* Check if parameters have changed */
if (!_should_stop_file_log) { // do not record param changes after disarming
if (parameter_update_sub.update()) {
parameter_update_s param_update;
if (parameter_update_sub.update(&param_update)) {
write_changed_parameters(LogType::Full);
}
}
@@ -1136,13 +1114,10 @@ void Logger::run()
++sub_idx;
}
//check for new logging message(s)
bool log_message_updated = false;
ret = orb_check(log_message_sub, &log_message_updated);
// check for new logging message(s)
log_message_s log_message;
if (ret == 0 && log_message_updated) {
log_message_s log_message;
orb_copy(ORB_ID(log_message), log_message_sub, &log_message);
if (_log_message_sub.update(&log_message)) {
const char *message = (const char *)log_message.text;
int message_len = strlen(message);
@@ -1183,7 +1158,7 @@ void Logger::run()
// update buffer statistics
for (int i = 0; i < (int)LogType::Count; ++i) {
if (!_statistics[i].dropout_start && _writer.get_buffer_fill_count_file((LogType)i) > _statistics[i].high_water) {
if (!_statistics[i].dropout_start && (_writer.get_buffer_fill_count_file((LogType)i) > _statistics[i].high_water)) {
_statistics[i].high_water = _writer.get_buffer_fill_count_file((LogType)i);
}
}
@@ -1279,10 +1254,6 @@ void Logger::run()
}
}
if (manual_control_sp_sub != -1) {
orb_unsubscribe(manual_control_sp_sub);
}
if (polling_topic_sub >= 0) {
orb_unsubscribe(polling_topic_sub);
}
@@ -1292,10 +1263,6 @@ void Logger::run()
_mavlink_log_pub = nullptr;
}
if (vehicle_command_sub != -1) {
orb_unsubscribe(vehicle_command_sub);
}
px4_unregister_shutdown_hook(&Logger::request_stop_static);
}
@@ -1320,21 +1287,20 @@ void Logger::debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start)
#endif /* DBGPRINT */
}
bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type)
bool Logger::start_stop_logging(MissionLogType mission_log_type)
{
bool bret = false;
bool want_start = false;
bool want_stop = false;
if (_log_mode == LogMode::rc_aux1) {
//aux1-based logging
bool manual_control_setpoint_updated;
int ret = orb_check(manual_control_sp_sub, &manual_control_setpoint_updated);
if (ret == 0 && manual_control_setpoint_updated) {
manual_control_setpoint_s manual_sp;
orb_copy(ORB_ID(manual_control_setpoint), manual_control_sp_sub, &manual_sp);
bool should_start = manual_sp.aux1 > 0.3f || _manually_logging_override;
// aux1-based logging
manual_control_setpoint_s manual_sp;
if (_manual_control_sp_sub.update(&manual_sp)) {
bool should_start = ((manual_sp.aux1 > 0.3f) || _manually_logging_override);
if (_prev_state != should_start) {
_prev_state = should_start;
@@ -1350,12 +1316,10 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su
} else {
// arming-based logging
bool vehicle_status_updated;
int ret = orb_check(vehicle_status_sub, &vehicle_status_updated);
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
if (ret == 0 && vehicle_status_updated) {
vehicle_status_s vehicle_status;
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || _manually_logging_override;
if (_prev_state != armed && _log_mode != LogMode::boot_until_shutdown) {
@@ -1397,16 +1361,14 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su
return bret;
}
void Logger::handle_vehicle_command_update(int vehicle_command_sub)
void Logger::handle_vehicle_command_update()
{
bool command_updated = false;
int ret = orb_check(vehicle_command_sub, &command_updated);
vehicle_command_s command;
if (ret == 0 && command_updated) {
vehicle_command_s command;
orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &command);
if (_vehicle_command_sub.update(&command)) {
if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
if ((int)(command.param1 + 0.5f) != 0) {
ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
src/modules/logger/logger.h
+13 -4
View File
@@ -39,17 +39,21 @@
#include "util.h"
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <version/version.h>
#include <parameters/param.h>
#include <systemlib/printload.h>
#include <px4_module.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/log_message.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
extern "C" __EXPORT int logger_main(int argc, char *argv[]);
#define TRY_SUBSCRIBE_INTERVAL 1000*1000 // interval in microseconds at which we try to subscribe to a topic
static constexpr hrt_abstime TRY_SUBSCRIBE_INTERVAL{1000 * 1000}; // interval in microseconds at which we try to subscribe to a topic
// if we haven't succeeded before
namespace px4
@@ -355,9 +359,9 @@ private:
* @param mission_log_type
* @return true if log started
*/
bool start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type);
bool start_stop_logging(MissionLogType mission_log_type);
void handle_vehicle_command_update(int vehicle_command_sub);
void handle_vehicle_command_update();
void ack_vehicle_command(vehicle_command_s *cmd, uint32_t result);
/**
@@ -408,6 +412,11 @@ private:
hrt_abstime _next_load_print{0}; ///< timestamp when to print the process load
PrintLoadReason _print_load_reason {PrintLoadReason::Preflight};
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20};
param_t _sdlog_profile_handle{PARAM_INVALID};
param_t _log_utc_offset{PARAM_INVALID};
param_t _log_dirs_max{PARAM_INVALID};
src/modules/logger/util.cpp
+6 -10
View File
@@ -39,6 +39,7 @@
#include <stdlib.h>
#include <unistd.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h>
@@ -72,18 +73,15 @@ bool file_exist(const char *filename)
bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time)
{
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
if (vehicle_gps_position_sub < 0) {
return false;
}
/* Get the latest GPS publication */
vehicle_gps_position_s gps_pos;
time_t utc_time_sec;
bool use_clock_time = true;
if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) == 0) {
/* Get the latest GPS publication */
vehicle_gps_position_s gps_pos;
if (vehicle_gps_position_sub.copy(&gps_pos)) {
utc_time_sec = gps_pos.time_utc_usec / 1e6;
if (gps_pos.fix_type >= 2 && utc_time_sec >= GPS_EPOCH_SECS) {
@@ -91,8 +89,6 @@ bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time)
}
}
orb_unsubscribe(vehicle_gps_position_sub);
if (use_clock_time) {
/* take clock time if there's no fix (yet) */
struct timespec ts = {};