forked from Archive/PX4-Autopilot
logger: subscribe to mavlink_log messages and write them to the log
This commit is contained in:
parent
659ac8faf2
commit
069dd01cb0
|
@ -1,3 +1,3 @@
|
|||
|
||||
uint8[50] text
|
||||
uint8 severity
|
||||
uint8 severity # log level (same as in the linux kernel, starting with 0)
|
||||
|
|
|
@ -41,6 +41,8 @@
|
|||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/uORBTopics.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/mavlink_log.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
@ -439,6 +441,7 @@ void Logger::run()
|
|||
|
||||
uORB::Subscription<vehicle_status_s> vehicle_status_sub(ORB_ID(vehicle_status));
|
||||
uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
|
||||
uORB::Subscription<mavlink_log_s> mavlink_log_sub(ORB_ID(mavlink_log));
|
||||
|
||||
|
||||
add_topic("sensor_gyro", 0);
|
||||
|
@ -547,38 +550,37 @@ void Logger::run()
|
|||
|
||||
//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
|
||||
|
||||
if (_writer.write(buffer, msg_size, _dropout_start)) {
|
||||
if (write(buffer, msg_size)) {
|
||||
|
||||
#ifdef DBGPRINT
|
||||
total_bytes += msg_size;
|
||||
#endif /* DBGPRINT */
|
||||
|
||||
if (_dropout_start) {
|
||||
float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f;
|
||||
|
||||
if (dropout_duration > _max_dropout_duration) {
|
||||
_max_dropout_duration = dropout_duration;
|
||||
}
|
||||
|
||||
_dropout_start = 0;
|
||||
}
|
||||
|
||||
data_written = true;
|
||||
|
||||
} else {
|
||||
|
||||
if (!_dropout_start) {
|
||||
_dropout_start = hrt_absolute_time();
|
||||
++_write_dropouts;
|
||||
_high_water = 0;
|
||||
}
|
||||
|
||||
break; // Write buffer overflow, skip this record
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//check for new mavlink log message
|
||||
if (mavlink_log_sub.check_updated()) {
|
||||
mavlink_log_sub.update();
|
||||
message_logging_s log_msg;
|
||||
log_msg.log_level = mavlink_log_sub.get().severity + '0';
|
||||
log_msg.timestamp = mavlink_log_sub.get().timestamp;
|
||||
const char *message = (const char *)mavlink_log_sub.get().text;
|
||||
int message_len = strlen(message);
|
||||
|
||||
if (message_len > 0) {
|
||||
strncpy(log_msg.message, message, sizeof(log_msg.message));
|
||||
log_msg.msg_size = sizeof(log_msg) - sizeof(log_msg.message) - MSG_HEADER_LEN + message_len;
|
||||
write(&log_msg, log_msg.msg_size + MSG_HEADER_LEN);
|
||||
}
|
||||
}
|
||||
|
||||
if (!_dropout_start && _writer.get_buffer_fill_count() > _high_water) {
|
||||
_high_water = _writer.get_buffer_fill_count();
|
||||
}
|
||||
|
@ -642,6 +644,32 @@ void Logger::run()
|
|||
}
|
||||
}
|
||||
|
||||
bool Logger::write(void *ptr, size_t size)
|
||||
{
|
||||
if (_writer.write(ptr, size, _dropout_start)) {
|
||||
|
||||
if (_dropout_start) {
|
||||
float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f;
|
||||
|
||||
if (dropout_duration > _max_dropout_duration) {
|
||||
_max_dropout_duration = dropout_duration;
|
||||
}
|
||||
|
||||
_dropout_start = 0;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!_dropout_start) {
|
||||
_dropout_start = hrt_absolute_time();
|
||||
++_write_dropouts;
|
||||
_high_water = 0;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int Logger::create_log_dir(tm *tt)
|
||||
{
|
||||
/* create dir on sdcard if needed */
|
||||
|
|
|
@ -164,6 +164,13 @@ private:
|
|||
*/
|
||||
bool write_wait(void *ptr, size_t size);
|
||||
|
||||
/**
|
||||
* Write data to the logger and handle dropouts.
|
||||
* Must be called with _writer.lock() held.
|
||||
* @return true if data written, false otherwise (on overflow)
|
||||
*/
|
||||
bool write(void *ptr, size_t size);
|
||||
|
||||
/**
|
||||
* Get the time for log file name
|
||||
* @param tt returned time
|
||||
|
|
|
@ -42,6 +42,7 @@ enum class MessageType : uint8_t {
|
|||
REMOVE_LOGGED_MSG = 'R',
|
||||
SYNC = 'S',
|
||||
DROPOUT = 'O',
|
||||
LOGGING = 'L',
|
||||
};
|
||||
|
||||
|
||||
|
@ -108,6 +109,15 @@ struct message_info_header_s {
|
|||
char key[255];
|
||||
};
|
||||
|
||||
struct message_logging_s {
|
||||
uint16_t msg_size; //size of message - MSG_HEADER_LEN
|
||||
uint8_t msg_type = static_cast<uint8_t>(MessageType::LOGGING);
|
||||
|
||||
uint8_t log_level; //same levels as in the linux kernel
|
||||
uint64_t timestamp;
|
||||
char message[255];
|
||||
};
|
||||
|
||||
struct message_parameter_header_s {
|
||||
uint16_t msg_size;
|
||||
uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
|
||||
|
|
|
@ -64,6 +64,7 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
|
|||
struct mavlink_log_s log_msg;
|
||||
|
||||
log_msg.severity = severity;
|
||||
log_msg.timestamp = hrt_absolute_time();
|
||||
|
||||
va_list ap;
|
||||
|
||||
|
|
|
@ -49,6 +49,7 @@
|
|||
#include "topics/position_setpoint_triplet.h"
|
||||
#include "topics/vehicle_status.h"
|
||||
#include "topics/manual_control_setpoint.h"
|
||||
#include "topics/mavlink_log.h"
|
||||
#include "topics/vehicle_local_position_setpoint.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
|
@ -165,6 +166,7 @@ template class __EXPORT Subscription<encoders_s>;
|
|||
template class __EXPORT Subscription<position_setpoint_triplet_s>;
|
||||
template class __EXPORT Subscription<vehicle_status_s>;
|
||||
template class __EXPORT Subscription<manual_control_setpoint_s>;
|
||||
template class __EXPORT Subscription<mavlink_log_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
|
||||
|
|
Loading…
Reference in New Issue