forked from Archive/PX4-Autopilot
Mavlink: bring mavlink log messages to life
This commit is contained in:
Julian Oes
parent
ea2a69d8bf
commit
99b426c27c
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue