mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Merged in alessiomorale/librepilot/LP-381_log_buffers (pull request #303)
LP-381 - Adds circular buffer and run writes within a separate callback
This commit is contained in:
commit
2ccabe27e7
@ -32,6 +32,7 @@
|
||||
#include "pios.h"
|
||||
#include "uavobjectmanager.h"
|
||||
#include "debuglogentry.h"
|
||||
#include "callbackinfo.h"
|
||||
|
||||
// global definitions
|
||||
#ifdef PIOS_INCLUDE_DEBUGLOG
|
||||
@ -39,14 +40,9 @@
|
||||
// Global variables
|
||||
extern uintptr_t pios_user_fs_id; // flash filesystem for logging
|
||||
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
static xSemaphoreHandle mutex = 0;
|
||||
#define mutexlock() xSemaphoreTakeRecursive(mutex, portMAX_DELAY)
|
||||
#define mutexunlock() xSemaphoreGiveRecursive(mutex)
|
||||
#else
|
||||
#define mutexlock()
|
||||
#define mutexunlock()
|
||||
#endif
|
||||
|
||||
static bool logging_enabled = false;
|
||||
#define MAX_CONSECUTIVE_FAILS_COUNT 10
|
||||
@ -54,10 +50,12 @@ static bool log_is_full = false;
|
||||
static uint8_t fails_count = 0;
|
||||
static uint16_t flightnum = 0;
|
||||
static uint16_t lognum = 0;
|
||||
static DebugLogEntryData *buffer = 0;
|
||||
#if !defined(PIOS_INCLUDE_FREERTOS)
|
||||
static DebugLogEntryData staticbuffer;
|
||||
#endif
|
||||
|
||||
#define BUFFERS_COUNT 2
|
||||
static DebugLogEntryData *current_buffer = 0;
|
||||
static DebugLogEntryData *buffers[BUFFERS_COUNT] = { 0, 0 };
|
||||
static uint8_t current_write_buffer_index;
|
||||
static uint8_t next_read_buffer_index;
|
||||
|
||||
#define LOG_ENTRY_MAX_DATA_SIZE (sizeof(((DebugLogEntryData *)0)->Data))
|
||||
#define LOG_ENTRY_HEADER_SIZE (sizeof(DebugLogEntryData) - LOG_ENTRY_MAX_DATA_SIZE)
|
||||
@ -66,23 +64,33 @@ static DebugLogEntryData staticbuffer;
|
||||
|
||||
static uint32_t used_buffer_space = 0;
|
||||
|
||||
#define CBTASK_PRIORITY CALLBACK_TASK_AUXILIARY
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
|
||||
#define CB_TIMEOUT 100
|
||||
#define STACK_SIZE_BYTES 512
|
||||
static DelayedCallbackInfo *callbackHandle;
|
||||
|
||||
/* Private Function Prototypes */
|
||||
static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data);
|
||||
static bool write_current_buffer();
|
||||
static void writeTask();
|
||||
static uint8_t get_blocks_free();
|
||||
/**
|
||||
* @brief Initialize the log facility
|
||||
*/
|
||||
void PIOS_DEBUGLOG_Initialize()
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
if (!mutex) {
|
||||
mutex = xSemaphoreCreateRecursiveMutex();
|
||||
buffer = pios_malloc(sizeof(DebugLogEntryData));
|
||||
mutex = xSemaphoreCreateRecursiveMutex();
|
||||
for (uint32_t i = 0; i < BUFFERS_COUNT; i++) {
|
||||
buffers[i] = pios_malloc(sizeof(DebugLogEntryData));
|
||||
}
|
||||
current_write_buffer_index = 0;
|
||||
next_read_buffer_index = 0;
|
||||
current_buffer = buffers[current_write_buffer_index];
|
||||
}
|
||||
#else
|
||||
buffer = &staticbuffer;
|
||||
#endif
|
||||
if (!buffer) {
|
||||
|
||||
if (!current_buffer) {
|
||||
return;
|
||||
}
|
||||
mutexlock();
|
||||
@ -91,10 +99,12 @@ void PIOS_DEBUGLOG_Initialize()
|
||||
fails_count = 0;
|
||||
used_buffer_space = 0;
|
||||
log_is_full = false;
|
||||
while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
|
||||
while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)current_buffer, sizeof(DebugLogEntryData)) == 0) {
|
||||
flightnum++;
|
||||
}
|
||||
mutexunlock();
|
||||
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&writeTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_DEBUGLOG, STACK_SIZE_BYTES);
|
||||
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, CB_TIMEOUT, CALLBACK_UPDATEMODE_LATER);
|
||||
}
|
||||
|
||||
|
||||
@ -122,7 +132,7 @@ void PIOS_DEBUGLOG_Enable(uint8_t enabled)
|
||||
*/
|
||||
void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
|
||||
{
|
||||
if (!logging_enabled || !buffer || log_is_full) {
|
||||
if (!logging_enabled || !current_buffer || log_is_full) {
|
||||
return;
|
||||
}
|
||||
mutexlock();
|
||||
@ -139,7 +149,7 @@ void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8
|
||||
*/
|
||||
void PIOS_DEBUGLOG_Printf(char *format, ...)
|
||||
{
|
||||
if (!logging_enabled || !buffer || log_is_full) {
|
||||
if (!logging_enabled || !current_buffer || log_is_full) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -150,19 +160,19 @@ void PIOS_DEBUGLOG_Printf(char *format, ...)
|
||||
if (used_buffer_space) {
|
||||
write_current_buffer();
|
||||
}
|
||||
memset(buffer->Data, 0xff, sizeof(buffer->Data));
|
||||
vsnprintf((char *)buffer->Data, sizeof(buffer->Data), (char *)format, args);
|
||||
buffer->Flight = flightnum;
|
||||
memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
|
||||
vsnprintf((char *)current_buffer->Data, sizeof(current_buffer->Data), (char *)format, args);
|
||||
current_buffer->Flight = flightnum;
|
||||
|
||||
buffer->FlightTime = PIOS_DELAY_GetuS();
|
||||
current_buffer->FlightTime = PIOS_DELAY_GetuS();
|
||||
|
||||
buffer->Entry = lognum;
|
||||
buffer->Type = DEBUGLOGENTRY_TYPE_TEXT;
|
||||
buffer->ObjectID = 0;
|
||||
buffer->InstanceID = 0;
|
||||
buffer->Size = strlen((const char *)buffer->Data);
|
||||
current_buffer->Entry = lognum;
|
||||
current_buffer->Type = DEBUGLOGENTRY_TYPE_TEXT;
|
||||
current_buffer->ObjectID = 0;
|
||||
current_buffer->InstanceID = 0;
|
||||
current_buffer->Size = strlen((const char *)current_buffer->Data);
|
||||
|
||||
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
|
||||
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)current_buffer, sizeof(DebugLogEntryData)) == 0) {
|
||||
lognum++;
|
||||
}
|
||||
mutexunlock();
|
||||
@ -233,21 +243,21 @@ void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
|
||||
|
||||
// start a new block
|
||||
if (!used_buffer_space) {
|
||||
entry = buffer;
|
||||
memset(buffer->Data, 0xff, sizeof(buffer->Data));
|
||||
entry = current_buffer;
|
||||
memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
|
||||
used_buffer_space += size;
|
||||
} else {
|
||||
// if an instance is being filled and there is enough space, does enqueues new data.
|
||||
if (used_buffer_space + size + LOG_ENTRY_HEADER_SIZE > LOG_ENTRY_MAX_DATA_SIZE) {
|
||||
buffer->Type = DEBUGLOGENTRY_TYPE_MULTIPLEUAVOBJECTS;
|
||||
current_buffer->Type = DEBUGLOGENTRY_TYPE_MULTIPLEUAVOBJECTS;
|
||||
if (!write_current_buffer()) {
|
||||
return;
|
||||
}
|
||||
entry = buffer;
|
||||
memset(buffer->Data, 0xff, sizeof(buffer->Data));
|
||||
entry = current_buffer;
|
||||
memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
|
||||
used_buffer_space += size;
|
||||
} else {
|
||||
entry = (DebugLogEntryData *)&buffer->Data[used_buffer_space];
|
||||
entry = (DebugLogEntryData *)¤t_buffer->Data[used_buffer_space];
|
||||
used_buffer_space += size + LOG_ENTRY_HEADER_SIZE;
|
||||
}
|
||||
}
|
||||
@ -258,8 +268,8 @@ void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
|
||||
entry->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT;
|
||||
entry->ObjectID = objid;
|
||||
entry->InstanceID = instid;
|
||||
if (size > sizeof(buffer->Data)) {
|
||||
size = sizeof(buffer->Data);
|
||||
if (size > sizeof(current_buffer->Data)) {
|
||||
size = sizeof(current_buffer->Data);
|
||||
}
|
||||
entry->Size = size;
|
||||
|
||||
@ -268,18 +278,46 @@ void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
|
||||
|
||||
bool write_current_buffer()
|
||||
{
|
||||
// not enough space, write the block and start a new one
|
||||
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
|
||||
lognum++;
|
||||
fails_count = 0;
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
// Check if queue is full
|
||||
|
||||
if (get_blocks_free() > 0) {
|
||||
current_write_buffer_index = (current_write_buffer_index + 1) % BUFFERS_COUNT;
|
||||
current_buffer = buffers[current_write_buffer_index];
|
||||
used_buffer_space = 0;
|
||||
return true;
|
||||
} else {
|
||||
if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) {
|
||||
log_is_full = true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static uint8_t get_blocks_free()
|
||||
{
|
||||
uint8_t used_blocks = current_write_buffer_index - next_read_buffer_index;
|
||||
|
||||
if (current_write_buffer_index < next_read_buffer_index) {
|
||||
used_blocks = (BUFFERS_COUNT - next_read_buffer_index) + current_write_buffer_index;
|
||||
}
|
||||
return (BUFFERS_COUNT - used_blocks) - 1;
|
||||
}
|
||||
|
||||
static void writeTask()
|
||||
{
|
||||
if (current_write_buffer_index != next_read_buffer_index) {
|
||||
// not enough space, write the block and start a new one
|
||||
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id,
|
||||
LOG_GET_FLIGHT_OBJID(flightnum), lognum,
|
||||
(uint8_t *)buffers[next_read_buffer_index],
|
||||
sizeof(DebugLogEntryData)) == 0) {
|
||||
next_read_buffer_index = (next_read_buffer_index + 1) % BUFFERS_COUNT;
|
||||
lognum++;
|
||||
fails_count = 0;
|
||||
} else {
|
||||
if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) {
|
||||
log_is_full = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif /* ifdef PIOS_INCLUDE_DEBUGLOG */
|
||||
/**
|
||||
|
@ -12,6 +12,7 @@
|
||||
<elementname>PathPlanner0</elementname>
|
||||
<elementname>PathPlanner1</elementname>
|
||||
<elementname>ManualControl</elementname>
|
||||
<elementname>DebugLog</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="Running" units="bool" type="enum">
|
||||
@ -25,6 +26,7 @@
|
||||
<elementname>PathPlanner0</elementname>
|
||||
<elementname>PathPlanner1</elementname>
|
||||
<elementname>ManualControl</elementname>
|
||||
<elementname>DebugLog</elementname>
|
||||
</elementnames>
|
||||
<options>
|
||||
<option>False</option>
|
||||
@ -42,6 +44,7 @@
|
||||
<elementname>PathPlanner0</elementname>
|
||||
<elementname>PathPlanner1</elementname>
|
||||
<elementname>ManualControl</elementname>
|
||||
<elementname>DebugLog</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<access gcs="readonly" flight="readwrite"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user