dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Mavlink: bring mavlink log messages to life

This commit is contained in:
Julian Oes 2014-02-12 12:38:35 +01:00
parent ea2a69d8bf
commit 99b426c27c
3 changed files with 36 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/include/mavlink/mavlink_log.h
View File

@ -100,6 +100,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
*/
#define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
struct mavlink_logmessage {
char text[MAVLINK_LOG_MAXLEN + 1];
unsigned char severity;
@ -112,6 +113,7 @@ struct mavlink_logbuffer {
struct mavlink_logmessage *elems;
};
__BEGIN_DECLS
void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb);
@ -125,6 +127,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
__END_DECLS
#endif

56
src/modules/mavlink/mavlink_main.cpp
View File

@ -90,6 +90,12 @@ static const int ERROR = -1;
static Mavlink* _head = nullptr;
/* TODO: if this is a class member it crashes */
static struct file_operations fops;
static struct mavlink_logbuffer lb;
static unsigned int total_counter;
/**
* mavlink app start / stop handling function
*
@ -152,7 +158,7 @@ namespace mavlink
}
Mavlink::Mavlink() :
// _mavlink_fd(-1),
_mavlink_fd(-1),
_next(nullptr),
_task_should_exit(false),
thread_running(false),
@ -277,14 +283,15 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
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;
printf("logmsg: %s\n", txt);
//struct mavlink_logmessage msg;
//strncpy(msg.text, txt, sizeof(msg.text));
//mavlink_logbuffer_write(&lb, &msg);
//total_counter++;
return OK;
}
const char *txt = (const char *)arg;
// printf("logmsg: %s\n", txt);
struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text));
mavlink_logbuffer_write(&lb, &msg);
total_counter++;
return OK;
}
default:
return ENOTTY;
@ -1435,14 +1442,8 @@ Mavlink::task_main(int argc, char *argv[])
warnx("Initializing..");
fflush(stdout);
/* initialize logging device */
// TODO
// _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0);
//mavlink_log_info(_mavlink_fd, "[mavlink] started");
/* initialize mavlink text message buffering */
// mavlink_logbuffer_init(&lb, 10);
mavlink_logbuffer_init(&lb, 10);
int ch;
const char *device_name = "/dev/ttyS1";
@ -1519,7 +1520,12 @@ Mavlink::task_main(int argc, char *argv[])
err(1, "could not open %s", device_name);
/* create the device node that's used for sending text log messages, etc. */
//register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
/* initialize logging device */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[mavlink] started");
/* Initialize system properties */
mavlink_update_system();
@ -1693,15 +1699,15 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_pm_queued_send();
}
// /* send one string at 10 Hz */
// if (!mavlink_logbuffer_is_empty(&lb)) {
// struct mavlink_logmessage msg;
// int lb_ret = mavlink_logbuffer_read(&lb, &msg);
/* send one string at 10 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
// if (lb_ret == OK) {
// mavlink_missionlib_send_gcs_string(msg.text);
// }
// }
if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text);
}
}
perf_end(_loop_perf);
}

5
src/modules/mavlink/mavlink_main.h
View File

@ -264,11 +264,11 @@ protected:
* Pointer to the default cdev file operations table; useful for
* registering clone devices etc.
*/
struct file_operations fops;
Mavlink* _next;
private:
int _mavlink_fd;
bool _task_should_exit; /**< if true, mavlink task should exit */
bool thread_running;
int _mavlink_task; /**< task handle for sensor task */
@ -311,7 +311,6 @@ private:
*/
unsigned int mavlink_param_queue_index;
struct mavlink_logbuffer lb;
bool mavlink_link_termination_allowed;
/**