rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15:54:15 +01:00
Code Issues Releases Activity

Merged in alessiomorale/librepilot/LP-381_log_buffers (pull request #303)

Browse Source 
LP-381 - Adds circular buffer and run writes within a separate callback
This commit is contained in:
Philippe Renon 2016-08-28 18:47:40 +02:00
commit 2ccabe27e7
2 changed files with 86 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
flight/pios/common/pios_debuglog.c
View File

@ -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 *)&current_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 */
/**

3
shared/uavobjectdefinition/callbackinfo.xml
View File

@ -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"/>