-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathdebug.cpp
48 lines (44 loc) · 1.01 KB
/
debug.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#include <stdarg.h>
#include <FreeRTOS_SAMD21.h>
#include <Arduino.h>
#include <semphr.h>
#include <ros.h>
#include "delay.h"
#include "debug.h"
static char buf[256];
static char isr_buf[64];
static bool is_inited;
static SemaphoreHandle_t debug_print_mutex;
static ros::NodeHandle *nh;
void debug_init(ros::NodeHandle &node_handle)
{
nh = &node_handle;
debug_print_mutex = xSemaphoreCreateMutex();
is_inited = true;
}
int debug(const char *fmt, ...)
{
xSemaphoreTake(debug_print_mutex, portMAX_DELAY);
va_list args;
va_start(args, fmt);
int len = vsnprintf(buf, sizeof(buf), fmt, args);
va_end(args);
//Serial.print(buf);
if (len && buf[len - 1] == '\n') // strip superfluous newline
{
buf[len - 1] = '\0';
--len;
}
nh->loginfo(buf);
xSemaphoreGive(debug_print_mutex);
return len;
}
int debugFromISR(const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
int count = vsnprintf(isr_buf, sizeof(isr_buf), fmt, args);
va_end(args);
//Serial.print(isr_buf);
return count;
}