fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 10:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: refactor mavlink_log

Browse Source 
This moves the mavlink_log interface from ioctl to uORB and enables the
mavlink statusmessage output for Snapdragon. The API changes will lead
to changes in all modules that are using it.
This commit is contained in:
Julian Oes
2016-03-24 13:09:16 +01:00
parent 790fa7667c
commit 2d4179a35b
9 changed files with 203 additions and 309 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+10 -106
View File
@@ -69,13 +69,14 @@
#include <systemlib/systemlib.h>
#include <systemlib/mcu_version.h>
#include <systemlib/git_version.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <dataman/dataman.h>
//#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/mavlink_log.h>
#include "mavlink_bridge_header.h"
#include "mavlink_main.h"
@@ -101,11 +102,6 @@ static const int ERROR = -1;
static Mavlink *_mavlink_instances = nullptr;
#ifdef __PX4_NUTTX
/* TODO: if this is a class member it crashes */
static struct file_operations fops;
#endif
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
@@ -123,14 +119,10 @@ static void usage(void);
bool Mavlink::_boot_complete = false;
Mavlink::Mavlink() :
#ifndef __PX4_NUTTX
VDev("mavlink-log", MAVLINK_LOG_DEVICE),
#endif
_device_name(DEFAULT_DEVICE_NAME),
_task_should_exit(false),
next(nullptr),
_instance_id(0),
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
_task_running(false),
_hil_enabled(false),
_generate_rc(false),
@@ -149,7 +141,7 @@ Mavlink::Mavlink() :
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_radio_id(0),
_logbuffer {},
_logbuffer(5, sizeof(mavlink_log_s)),
_total_counter(0),
_receive_thread {},
_verbose(false),
@@ -203,10 +195,6 @@ Mavlink::Mavlink() :
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
#ifdef __PX4_NUTTX
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
#endif
_instance_id = Mavlink::instance_count();
/* set channel according to instance id */
@@ -475,60 +463,6 @@ Mavlink::get_channel()
return _channel;
}
/****************************************************************************
* MAVLink text message logger
****************************************************************************/
int
#ifdef __PX4_NUTTX
Mavlink::mavlink_dev_ioctl(struct file *filp, int cmd, unsigned long arg)
#else
Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg)
#endif
{
switch (cmd) {
case (int)MAVLINK_IOC_SEND_TEXT_INFO:
case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
const char *txt = (const char *)arg;
struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text));
switch (cmd) {
case MAVLINK_IOC_SEND_TEXT_INFO:
msg.severity = MAV_SEVERITY_INFO;
break;
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
msg.severity = MAV_SEVERITY_CRITICAL;
break;
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
msg.severity = MAV_SEVERITY_EMERGENCY;
break;
default:
msg.severity = MAV_SEVERITY_INFO;
break;
}
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (!inst->_task_should_exit) {
mavlink_logbuffer_write(&inst->_logbuffer, &msg);
inst->_total_counter++;
}
}
return OK;
}
default:
return ENOTTY;
}
}
void Mavlink::mavlink_update_system(void)
{
if (!_param_initialized) {
@@ -1088,29 +1022,19 @@ Mavlink::handle_message(const mavlink_message_t *msg)
void
Mavlink::send_statustext_info(const char *string)
{
send_statustext(MAV_SEVERITY_INFO, string);
mavlink_log_info(&_mavlink_log_pub, string);
}
void
Mavlink::send_statustext_critical(const char *string)
{
send_statustext(MAV_SEVERITY_CRITICAL, string);
mavlink_log_critical(&_mavlink_log_pub, string);
}
void
Mavlink::send_statustext_emergency(const char *string)
{
send_statustext(MAV_SEVERITY_EMERGENCY, string);
}
void
Mavlink::send_statustext(unsigned char severity, const char *string)
{
struct mavlink_logmessage logmsg;
strncpy(logmsg.text, string, sizeof(logmsg.text));
logmsg.severity = severity;
mavlink_logbuffer_write(&_logbuffer, &logmsg);
mavlink_log_emergency(&_mavlink_log_pub, string);
}
void Mavlink::send_autopilot_capabilites()
@@ -1686,9 +1610,6 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize send mutex */
pthread_mutex_init(&_send_mutex, NULL);
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_forwarding_on || _ftp_on) {
/* initialize message buffer if multiplexing is on or its needed for FTP.
@@ -1704,20 +1625,6 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_init(&_message_buffer_mutex, NULL);
}
/* create the device node that's used for sending text log messages, etc. */
#ifdef __PX4_NUTTX
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
#else
int ret;
ret = VDev::init();
if (ret != OK) {
PX4_WARN("VDev setup for mavlink log device failed!\n");
}
#endif
/* initialize logging device */
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
/* Initialize system properties */
mavlink_update_system();
@@ -2112,17 +2019,11 @@ Mavlink::task_main(int argc, char *argv[])
::close(_uart_fd);
}
/* close mavlink logging device */
px4_close(_mavlink_fd);
if (_forwarding_on || _ftp_on) {
message_buffer_destroy();
pthread_mutex_destroy(&_message_buffer_mutex);
}
/* destroy log buffer */
mavlink_logbuffer_destroy(&_logbuffer);
warnx("exiting");
_task_running = false;
@@ -2390,3 +2291,6 @@ int mavlink_main(int argc, char *argv[])
return 0;
}