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

LP-381 - Adds circular buffer and run writes within a separate callback

Browse Source
This commit is contained in:
Alessio Morale 2016-08-17 19:57:29 +02:00
parent 19fa385f28
commit cba2184caf
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 "pios.h"
#include "uavobjectmanager.h" #include "uavobjectmanager.h"
#include "debuglogentry.h" #include "debuglogentry.h"
#include "callbackinfo.h"
// global definitions // global definitions
#ifdef PIOS_INCLUDE_DEBUGLOG #ifdef PIOS_INCLUDE_DEBUGLOG
@ -39,14 +40,9 @@
// Global variables // Global variables
extern uintptr_t pios_user_fs_id; // flash filesystem for logging extern uintptr_t pios_user_fs_id; // flash filesystem for logging
#if defined(PIOS_INCLUDE_FREERTOS)
static xSemaphoreHandle mutex = 0; static xSemaphoreHandle mutex = 0;
#define mutexlock() xSemaphoreTakeRecursive(mutex, portMAX_DELAY) #define mutexlock() xSemaphoreTakeRecursive(mutex, portMAX_DELAY)
#define mutexunlock() xSemaphoreGiveRecursive(mutex) #define mutexunlock() xSemaphoreGiveRecursive(mutex)
#else
#define mutexlock()
#define mutexunlock()
#endif
static bool logging_enabled = false; static bool logging_enabled = false;
#define MAX_CONSECUTIVE_FAILS_COUNT 10 #define MAX_CONSECUTIVE_FAILS_COUNT 10
@ -54,10 +50,12 @@ static bool log_is_full = false;
static uint8_t fails_count = 0; static uint8_t fails_count = 0;
static uint16_t flightnum = 0; static uint16_t flightnum = 0;
static uint16_t lognum = 0; static uint16_t lognum = 0;
static DebugLogEntryData *buffer = 0;
#if !defined(PIOS_INCLUDE_FREERTOS) #define BUFFERS_COUNT 2
static DebugLogEntryData staticbuffer; static DebugLogEntryData *current_buffer = 0;
#endif 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_MAX_DATA_SIZE (sizeof(((DebugLogEntryData *)0)->Data))
#define LOG_ENTRY_HEADER_SIZE (sizeof(DebugLogEntryData) - LOG_ENTRY_MAX_DATA_SIZE) #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; 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 */ /* Private Function Prototypes */
static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data); static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data);
static bool write_current_buffer(); static bool write_current_buffer();
static void writeTask();
static uint8_t get_blocks_free();
/** /**
* @brief Initialize the log facility * @brief Initialize the log facility
*/ */
void PIOS_DEBUGLOG_Initialize() void PIOS_DEBUGLOG_Initialize()
{ {
#if defined(PIOS_INCLUDE_FREERTOS)
if (!mutex) { if (!mutex) {
mutex = xSemaphoreCreateRecursiveMutex(); mutex = xSemaphoreCreateRecursiveMutex();
buffer = pios_malloc(sizeof(DebugLogEntryData)); 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; if (!current_buffer) {
#endif
if (!buffer) {
return; return;
} }
mutexlock(); mutexlock();
@ -91,10 +99,12 @@ void PIOS_DEBUGLOG_Initialize()
fails_count = 0; fails_count = 0;
used_buffer_space = 0; used_buffer_space = 0;
log_is_full = false; 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++; flightnum++;
} }
mutexunlock(); 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) 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; return;
} }
mutexlock(); 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, ...) void PIOS_DEBUGLOG_Printf(char *format, ...)
{ {
if (!logging_enabled || !buffer || log_is_full) { if (!logging_enabled || !current_buffer || log_is_full) {
return; return;
} }
@ -150,19 +160,19 @@ void PIOS_DEBUGLOG_Printf(char *format, ...)
if (used_buffer_space) { if (used_buffer_space) {
write_current_buffer(); write_current_buffer();
} }
memset(buffer->Data, 0xff, sizeof(buffer->Data)); memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
vsnprintf((char *)buffer->Data, sizeof(buffer->Data), (char *)format, args); vsnprintf((char *)current_buffer->Data, sizeof(current_buffer->Data), (char *)format, args);
buffer->Flight = flightnum; current_buffer->Flight = flightnum;
buffer->FlightTime = PIOS_DELAY_GetuS(); current_buffer->FlightTime = PIOS_DELAY_GetuS();
buffer->Entry = lognum; current_buffer->Entry = lognum;
buffer->Type = DEBUGLOGENTRY_TYPE_TEXT; current_buffer->Type = DEBUGLOGENTRY_TYPE_TEXT;
buffer->ObjectID = 0; current_buffer->ObjectID = 0;
buffer->InstanceID = 0; current_buffer->InstanceID = 0;
buffer->Size = strlen((const char *)buffer->Data); 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++; lognum++;
} }
mutexunlock(); mutexunlock();
@ -233,21 +243,21 @@ void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
// start a new block // start a new block
if (!used_buffer_space) { if (!used_buffer_space) {
entry = buffer; entry = current_buffer;
memset(buffer->Data, 0xff, sizeof(buffer->Data)); memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
used_buffer_space += size; used_buffer_space += size;
} else { } else {
// if an instance is being filled and there is enough space, does enqueues new data. // 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) { 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()) { if (!write_current_buffer()) {
return; return;
} }
entry = buffer; entry = current_buffer;
memset(buffer->Data, 0xff, sizeof(buffer->Data)); memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
used_buffer_space += size; used_buffer_space += size;
} else { } else {
entry = (DebugLogEntryData *)&buffer->Data[used_buffer_space]; entry = (DebugLogEntryData *)&current_buffer->Data[used_buffer_space];
used_buffer_space += size + LOG_ENTRY_HEADER_SIZE; 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->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT;
entry->ObjectID = objid; entry->ObjectID = objid;
entry->InstanceID = instid; entry->InstanceID = instid;
if (size > sizeof(buffer->Data)) { if (size > sizeof(current_buffer->Data)) {
size = sizeof(buffer->Data); size = sizeof(current_buffer->Data);
} }
entry->Size = size; 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() bool write_current_buffer()
{ {
// not enough space, write the block and start a new one PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { // Check if queue is full
lognum++;
fails_count = 0; 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; used_buffer_space = 0;
return true;
} else { } else {
if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) {
log_is_full = true;
}
return false; 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 */ #endif /* ifdef PIOS_INCLUDE_DEBUGLOG */
/** /**

3
shared/uavobjectdefinition/callbackinfo.xml
View File

@ -12,6 +12,7 @@
<elementname>PathPlanner0</elementname> <elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname> <elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname> <elementname>ManualControl</elementname>
<elementname>DebugLog</elementname>
</elementnames> </elementnames>
</field> </field>
<field name="Running" units="bool" type="enum"> <field name="Running" units="bool" type="enum">
@ -25,6 +26,7 @@
<elementname>PathPlanner0</elementname> <elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname> <elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname> <elementname>ManualControl</elementname>
<elementname>DebugLog</elementname>
</elementnames> </elementnames>
<options> <options>
<option>False</option> <option>False</option>
@ -42,6 +44,7 @@
<elementname>PathPlanner0</elementname> <elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname> <elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname> <elementname>ManualControl</elementname>
<elementname>DebugLog</elementname>
</elementnames> </elementnames>
</field> </field>
<access gcs="readonly" flight="readwrite"/> <access gcs="readonly" flight="readwrite"/>