fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 15:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink: Code style

Browse Source
This commit is contained in:
Lorenz Meier
2016-08-26 23:43:33 +02:00
parent e67a6bdfc2
commit 720c445f21
17 changed files with 922 additions and 627 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_log_handler.cpp
+144 -57
View File
@@ -64,13 +64,18 @@ static const char *kTmpData = MOUNTPOINT "/$log$.txt";
//-------------------------------------------------------------------
static bool
stat_file(const char* file, time_t* date = 0, uint32_t* size = 0) {
stat_file(const char *file, time_t *date = 0, uint32_t *size = 0)
{
struct stat st;
if (stat(file, &st) == 0) {
if (date) *date = st.st_mtime;
if (size) *size = st.st_size;
if (date) { *date = st.st_mtime; }
if (size) { *size = st.st_size; }
return true;
}
return false;
}
@@ -97,12 +102,15 @@ MavlinkLogHandler::handle_message(const mavlink_message_t *msg)
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
_log_request_list(msg);
break;
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
_log_request_data(msg);
break;
case MAVLINK_MSG_ID_LOG_ERASE:
_log_request_erase(msg);
break;
case MAVLINK_MSG_ID_LOG_REQUEST_END:
_log_request_end(msg);
break;
@@ -110,7 +118,7 @@ MavlinkLogHandler::handle_message(const mavlink_message_t *msg)
}
//-------------------------------------------------------------------
const char*
const char *
MavlinkLogHandler::get_name(void) const
{
return "MAVLINK_LOG_HANDLER";
@@ -130,10 +138,12 @@ MavlinkLogHandler::get_size(void)
//-- Sending Log Entries
if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING) {
return MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
//-- Sending Log Data (contents of one of the log files)
//-- Sending Log Data (contents of one of the log files)
} else if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA) {
return MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
//-- Idle
//-- Idle
} else {
return 0;
}
@@ -144,14 +154,18 @@ void
MavlinkLogHandler::send(const hrt_abstime /*t*/)
{
//-- An arbitrary count of max bytes in one go (one of the two below but never both)
#define MAX_BYTES_SEND 64 * 1024
#define MAX_BYTES_SEND 64 * 1024
size_t count = 0;
//-- Log Entries
while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) {
while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING
&& _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) {
count += _log_send_listing();
}
//-- Log Data
while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) {
while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA
&& _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) {
count += _log_send_data();
}
}
@@ -162,29 +176,35 @@ MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg)
{
mavlink_log_request_list_t request;
mavlink_msg_log_request_list_decode(msg, &request);
//-- Check for re-requests (data loss) or new request
if(_pLogHandlerHelper) {
if (_pLogHandlerHelper) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
//-- Is this a new request?
if((request.end - request.start) > _pLogHandlerHelper->log_count) {
if ((request.end - request.start) > _pLogHandlerHelper->log_count) {
delete _pLogHandlerHelper;
_pLogHandlerHelper = NULL;
}
}
if(!_pLogHandlerHelper) {
if (!_pLogHandlerHelper) {
//-- Prepare new request
_pLogHandlerHelper = new LogListHelper;
}
if (_pLogHandlerHelper->log_count)
{
if (_pLogHandlerHelper->log_count) {
//-- Define (and clamp) range
_pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1;
_pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end : _pLogHandlerHelper->log_count - 1;
_pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start :
_pLogHandlerHelper->log_count - 1;
_pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end :
_pLogHandlerHelper->log_count - 1;
}
PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %u last: %u count: %u\n",
_pLogHandlerHelper->next_entry,
_pLogHandlerHelper->last_entry,
_pLogHandlerHelper->log_count);
_pLogHandlerHelper->next_entry,
_pLogHandlerHelper->last_entry,
_pLogHandlerHelper->log_count);
//-- Enable streaming
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_LISTING;
}
@@ -194,44 +214,54 @@ void
MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg)
{
//-- If we haven't listed, we can't do much
if(!_pLogHandlerHelper) {
if (!_pLogHandlerHelper) {
PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested.\n");
return;
}
mavlink_log_request_data_t request;
mavlink_msg_log_request_data_decode(msg, &request);
//-- Does the requested log exist?
if (request.id >= _pLogHandlerHelper->log_count) {
PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %u but we only have %u.\n", request.id, _pLogHandlerHelper->log_count);
PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %u but we only have %u.\n", request.id,
_pLogHandlerHelper->log_count);
return;
}
//-- If we were sending log entries, stop it
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
if (_pLogHandlerHelper->current_log_index != request.id) {
//-- Init send log dataset
_pLogHandlerHelper->current_log_filename[0] = 0;
_pLogHandlerHelper->current_log_index = request.id;
uint32_t time_utc = 0;
_pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename);
_pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc,
_pLogHandlerHelper->current_log_filename);
_pLogHandlerHelper->open_for_transmit();
}
_pLogHandlerHelper->current_log_data_offset = request.ofs;
if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) {
_pLogHandlerHelper->current_log_data_remaining = 0;
} else {
_pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs;
}
if (_pLogHandlerHelper->current_log_data_remaining > request.count) {
_pLogHandlerHelper->current_log_data_remaining = request.count;
}
//-- Enable streaming
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA;
}
//-------------------------------------------------------------------
void
MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/)
MavlinkLogHandler::_log_request_erase(const mavlink_message_t * /*msg*/)
{
/*
mavlink_log_erase_t request;
@@ -241,36 +271,43 @@ MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/)
delete _pLogHandlerHelper;
_pLogHandlerHelper = 0;
}
//-- Delete all logs
LogListHelper::delete_all(kLogRoot);
//-- Now delete all "msgs_*" from root
DIR* dp = opendir(kSDRoot);
DIR *dp = opendir(kSDRoot);
if (dp) {
struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) {
// no more entries?
if (!result) {
break;
}
if (entry.d_type == PX4LOG_REGULAR_FILE) {
if(!memcmp(entry.d_name, "msgs_", 5)) {
if (!memcmp(entry.d_name, "msgs_", 5)) {
char msg_path[128];
snprintf(msg_path, sizeof(msg_path), "%s%s", kSDRoot, entry.d_name);
if(unlink(msg_path)) {
if (unlink(msg_path)) {
PX4LOG_WARN("MavlinkLogHandler::_log_request_erase Error deleting %s\n", msg_path);
}
}
}
}
closedir(dp);
}
}
//-------------------------------------------------------------------
void
MavlinkLogHandler::_log_request_end(const mavlink_message_t* /*msg*/)
MavlinkLogHandler::_log_request_end(const mavlink_message_t * /*msg*/)
{
PX4LOG_WARN("MavlinkLogHandler::_log_request_end\n");
if (_pLogHandlerHelper) {
delete _pLogHandlerHelper;
_pLogHandlerHelper = 0;
@@ -290,19 +327,22 @@ MavlinkLogHandler::_log_send_listing()
response.num_logs = _pLogHandlerHelper->log_count;
response.last_log_num = _pLogHandlerHelper->last_entry;
mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response);
//-- If we're done listing, flag it.
if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
} else {
_pLogHandlerHelper->next_entry++;
}
PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %u count: %u last: %u size: %u date: %u status: %d\n",
response.id,
response.num_logs,
response.last_log_num,
response.size,
response.time_utc,
_pLogHandlerHelper->current_status);
response.id,
response.num_logs,
response.last_log_num,
response.size,
response.time_utc,
_pLogHandlerHelper->current_status);
return sizeof(response);
}
@@ -313,9 +353,11 @@ MavlinkLogHandler::_log_send_data()
mavlink_log_data_t response;
memset(&response, 0, sizeof(response));
uint32_t len = _pLogHandlerHelper->current_log_data_remaining;
if (len > sizeof(response.data)) {
len = sizeof(response.data);
}
size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data);
response.ofs = _pLogHandlerHelper->current_log_data_offset;
response.id = _pLogHandlerHelper->current_log_index;
@@ -323,9 +365,11 @@ MavlinkLogHandler::_log_send_data()
mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response);
_pLogHandlerHelper->current_log_data_offset += read_size;
_pLogHandlerHelper->current_log_data_remaining -= read_size;
if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) {
_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
}
return sizeof(response);
}
@@ -354,33 +398,39 @@ LogListHelper::~LogListHelper()
//-------------------------------------------------------------------
bool
LogListHelper::get_entry(int idx, uint32_t& size, uint32_t& date, char* filename)
LogListHelper::get_entry(int idx, uint32_t &size, uint32_t &date, char *filename)
{
//-- Find log file in log list file created during init()
size = 0;
date = 0;
bool result = false;
//-- Open list of log files
FILE* f = ::fopen(kLogData, "r");
FILE *f = ::fopen(kLogData, "r");
if (f) {
//--- Find requested entry
char line[160];
int count = 0;
while (fgets(line, sizeof(line), f)) {
//-- Found our "index"
if(count++ == idx) {
if (count++ == idx) {
char file[128];
if(sscanf(line, "%u %u %s", &date, &size, file) == 3) {
if(filename) {
if (sscanf(line, "%u %u %s", &date, &size, file) == 3) {
if (filename) {
strcpy(filename, file);
}
result = true;
break;
}
}
}
fclose(f);
}
return result;
}
@@ -392,30 +442,38 @@ LogListHelper::open_for_transmit()
::fclose(current_log_filep);
current_log_filep = 0;
}
current_log_filep = ::fopen(current_log_filename, "rb");
if (!current_log_filep) {
PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s\n", current_log_filename);
return false;
}
return true;
}
//-------------------------------------------------------------------
size_t
LogListHelper::get_log_data(uint8_t len, uint8_t* buffer)
LogListHelper::get_log_data(uint8_t len, uint8_t *buffer)
{
if(!current_log_filename[0])
if (!current_log_filename[0]) {
return 0;
}
if (!current_log_filep) {
PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s\n", current_log_filename);
return 0;
}
long int offset = current_log_data_offset - ftell(current_log_filep);
if(offset && fseek(current_log_filep, offset, SEEK_CUR)) {
if (offset && fseek(current_log_filep, offset, SEEK_CUR)) {
fclose(current_log_filep);
PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename);
return 0;
}
size_t result = fread(buffer, 1, len, current_log_filep);
return result;
}
@@ -436,36 +494,44 @@ LogListHelper::_init()
unlink(kLogData);
// Open log directory
DIR *dp = opendir(kLogRoot);
if (dp == nullptr) {
// No log directory. Nothing to do.
return;
}
// Create work file
FILE* f = ::fopen(kTmpData, "w");
FILE *f = ::fopen(kTmpData, "w");
if (!f) {
PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData);
closedir(dp);
return;
}
// Scan directory and collect log files
struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) {
// no more entries?
if (result == nullptr) {
break;
}
if (entry.d_type == PX4LOG_DIRECTORY)
{
if (entry.d_type == PX4LOG_DIRECTORY) {
time_t tt = 0;
char log_path[128];
snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name);
if (_get_session_date(log_path, entry.d_name, tt)) {
_scan_logs(f, log_path, tt);
}
}
}
closedir(dp);
fclose(f);
// Rename temp file to data file
if (rename(kTmpData, kLogData)) {
PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s\n", kTmpData);
@@ -475,20 +541,23 @@ LogListHelper::_init()
//-------------------------------------------------------------------
bool
LogListHelper::_get_session_date(const char* path, const char* dir, time_t& date)
LogListHelper::_get_session_date(const char *path, const char *dir, time_t &date)
{
if(strlen(dir) > 4) {
if (strlen(dir) > 4) {
// Convert "sess000" to 00:00 Jan 1 1970 (day per session)
if (strncmp(dir, "sess", 4) == 0) {
unsigned u;
if(sscanf(&dir[4], "%u", &u) == 1) {
if (sscanf(&dir[4], "%u", &u) == 1) {
date = u * 60 * 60 * 24;
return true;
}
} else {
if (stat_file(path, &date)) {
return true;
}
/* strptime not available for some reason
// Get date from directory name
struct tm tt;
@@ -499,27 +568,32 @@ LogListHelper::_get_session_date(const char* path, const char* dir, time_t& date
*/
}
}
return false;
}
//-------------------------------------------------------------------
void
LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date)
LogListHelper::_scan_logs(FILE *f, const char *dir, time_t &date)
{
DIR *dp = opendir(dir);
if (dp) {
struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) {
// no more entries?
if (result == nullptr) {
break;
}
if (entry.d_type == PX4LOG_REGULAR_FILE) {
time_t ldate = date;
uint32_t size = 0;
char log_file_path[128];
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, entry.d_name);
if(_get_log_time_size(log_file_path, entry.d_name, ldate, size)) {
if (_get_log_time_size(log_file_path, entry.d_name, ldate, size)) {
//-- Write entry out to list file
fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path);
log_count++;
@@ -531,23 +605,27 @@ LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date)
//-------------------------------------------------------------------
bool
LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& date, uint32_t& size)
LogListHelper::_get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size)
{
if(file && file[0]) {
if(strstr(file, ".px4log") || strstr(file, ".ulg")) {
if (file && file[0]) {
if (strstr(file, ".px4log") || strstr(file, ".ulg")) {
// Convert "log000" to 00:00 (minute per flight in session)
if (strncmp(file, "log", 3) == 0) {
unsigned u;
if(sscanf(&file[3], "%u", &u) == 1) {
if (sscanf(&file[3], "%u", &u) == 1) {
date += (u * 60);
if (stat_file(path, 0, &size)) {
return true;
}
}
} else {
if (stat_file(path, &date, &size)) {
return true;
}
/* strptime not available for some reason
// Get time from file name
struct tm tt;
@@ -559,39 +637,48 @@ LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& da
}
}
}
return false;
}
//-------------------------------------------------------------------
void
LogListHelper::delete_all(const char* dir)
LogListHelper::delete_all(const char *dir)
{
//-- Open log directory
DIR* dp = opendir(dir);
DIR *dp = opendir(dir);
if (dp == nullptr) {
return;
}
struct dirent entry, *result = nullptr;
while (readdir_r(dp, &entry, &result) == 0) {
// no more entries?
if (result == nullptr) {
break;
}
if (entry.d_type == PX4LOG_DIRECTORY && entry.d_name[0] != '.') {
char log_path[128];
snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name);
LogListHelper::delete_all(log_path);
if(rmdir(log_path)) {
if (rmdir(log_path)) {
PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s\n", log_path);
}
}
if (entry.d_type == PX4LOG_REGULAR_FILE) {
char log_path[128];
snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name);
if(unlink(log_path)) {
if (unlink(log_path)) {
PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s\n", log_path);
}
}
}
closedir(dp);
}