mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
288 lines
8.3 KiB
C
288 lines
8.3 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
|
* @{
|
|
* @defgroup PIOS_DEBUGLOG Flash log debugging Functions
|
|
* @brief Debugging functionality
|
|
* @{
|
|
*
|
|
* @file pios_debuglog.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
|
* @brief Debugging Functions
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
/* Project Includes */
|
|
#include "pios.h"
|
|
#include "uavobjectmanager.h"
|
|
#include "debuglogentry.h"
|
|
|
|
// global definitions
|
|
|
|
|
|
// 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
|
|
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 LOG_ENTRY_MAX_DATA_SIZE (sizeof(((DebugLogEntryData *)0)->Data))
|
|
#define LOG_ENTRY_HEADER_SIZE (sizeof(DebugLogEntryData) - LOG_ENTRY_MAX_DATA_SIZE)
|
|
// build the obj_id as a DEBUGLOGENTRY ID with least significant byte zeroed and filled with flight number
|
|
#define LOG_GET_FLIGHT_OBJID(x) ((DEBUGLOGENTRY_OBJID & ~0xFF) | (x & 0xFF))
|
|
|
|
static uint32_t used_buffer_space = 0;
|
|
|
|
/* Private Function Prototypes */
|
|
static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data);
|
|
static bool write_current_buffer();
|
|
/**
|
|
* @brief Initialize the log facility
|
|
*/
|
|
void PIOS_DEBUGLOG_Initialize()
|
|
{
|
|
#if defined(PIOS_INCLUDE_FREERTOS)
|
|
if (!mutex) {
|
|
mutex = xSemaphoreCreateRecursiveMutex();
|
|
buffer = pios_malloc(sizeof(DebugLogEntryData));
|
|
}
|
|
#else
|
|
buffer = &staticbuffer;
|
|
#endif
|
|
if (!buffer) {
|
|
return;
|
|
}
|
|
mutexlock();
|
|
lognum = 0;
|
|
flightnum = 0;
|
|
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) {
|
|
flightnum++;
|
|
}
|
|
mutexunlock();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Enables or Disables logging globally
|
|
* @param[in] enable or disable logging
|
|
*/
|
|
void PIOS_DEBUGLOG_Enable(uint8_t enabled)
|
|
{
|
|
// increase the flight num as soon as logging is disabled
|
|
if (logging_enabled && !enabled) {
|
|
flightnum++;
|
|
lognum = 0;
|
|
}
|
|
logging_enabled = enabled;
|
|
}
|
|
|
|
/**
|
|
* @brief Write a debug log entry with a uavobject
|
|
* @param[in] objectid
|
|
* @param[in] instanceid
|
|
* @param[in] instanceid
|
|
* @param[in] size of object
|
|
* @param[in] data buffer
|
|
*/
|
|
void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
|
|
{
|
|
if (!logging_enabled || !buffer || log_is_full) {
|
|
return;
|
|
}
|
|
mutexlock();
|
|
|
|
enqueue_data(objid, instid, size, data);
|
|
|
|
mutexunlock();
|
|
}
|
|
/**
|
|
* @brief Write a debug log entry with text
|
|
* @param[in] format - as in printf
|
|
* @param[in] variable arguments for printf
|
|
* @param...
|
|
*/
|
|
void PIOS_DEBUGLOG_Printf(char *format, ...)
|
|
{
|
|
if (!logging_enabled || !buffer || log_is_full) {
|
|
return;
|
|
}
|
|
|
|
va_list args;
|
|
va_start(args, format);
|
|
mutexlock();
|
|
// flush any pending buffer before writing debug text
|
|
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;
|
|
|
|
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);
|
|
|
|
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
|
|
lognum++;
|
|
}
|
|
mutexunlock();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Load one object instance from the filesystem
|
|
* @param[out] buffer where to store the uavobject
|
|
* @param[in] log entry from which flight
|
|
* @param[in] log entry sequence number
|
|
* @return 0 if success or error code
|
|
* @retval -1 if fs_id is not a valid filesystem instance
|
|
* @retval -2 if failed to start transaction
|
|
* @retval -3 if object not found in filesystem
|
|
* @retval -4 if object size in filesystem does not exactly match buffer size
|
|
* @retval -5 if reading the object data from flash fails
|
|
*/
|
|
int32_t PIOS_DEBUGLOG_Read(void *mybuffer, uint16_t flight, uint16_t inst)
|
|
{
|
|
PIOS_Assert(mybuffer);
|
|
return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flight), inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData));
|
|
}
|
|
|
|
/**
|
|
* @brief Retrieve run time info of logging system
|
|
* @param[out] current flight number
|
|
* @param[out] next entry number
|
|
* @param[out] free slots in filesystem
|
|
* @param[out] used slots in filesystem
|
|
*/
|
|
void PIOS_DEBUGLOG_Info(uint16_t *flight, uint16_t *entry, uint16_t *free, uint16_t *used)
|
|
{
|
|
if (flight) {
|
|
*flight = flightnum;
|
|
}
|
|
if (entry) {
|
|
*entry = lognum;
|
|
}
|
|
struct PIOS_FLASHFS_Stats stats = { 0, 0 };
|
|
PIOS_FLASHFS_GetStats(pios_user_fs_id, &stats);
|
|
if (free) {
|
|
*free = stats.num_free_slots;
|
|
}
|
|
if (used) {
|
|
*used = stats.num_active_slots;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Format entire flash memory!!!
|
|
*/
|
|
void PIOS_DEBUGLOG_Format(void)
|
|
{
|
|
mutexlock();
|
|
PIOS_FLASHFS_Format(pios_user_fs_id);
|
|
lognum = 0;
|
|
flightnum = 0;
|
|
log_is_full = false;
|
|
fails_count = 0;
|
|
used_buffer_space = 0;
|
|
mutexunlock();
|
|
}
|
|
|
|
void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
|
|
{
|
|
DebugLogEntryData *entry;
|
|
|
|
// start a new block
|
|
if (!used_buffer_space) {
|
|
entry = buffer;
|
|
memset(buffer->Data, 0xff, sizeof(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;
|
|
if (!write_current_buffer()) {
|
|
return;
|
|
}
|
|
entry = buffer;
|
|
memset(buffer->Data, 0xff, sizeof(buffer->Data));
|
|
used_buffer_space += size;
|
|
} else {
|
|
entry = (DebugLogEntryData *)&buffer->Data[used_buffer_space];
|
|
used_buffer_space += size + LOG_ENTRY_HEADER_SIZE;
|
|
}
|
|
}
|
|
|
|
entry->Flight = flightnum;
|
|
entry->FlightTime = PIOS_DELAY_GetuS();
|
|
entry->Entry = lognum;
|
|
entry->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT;
|
|
entry->ObjectID = objid;
|
|
entry->InstanceID = instid;
|
|
if (size > sizeof(buffer->Data)) {
|
|
size = sizeof(buffer->Data);
|
|
}
|
|
entry->Size = size;
|
|
|
|
memcpy(entry->Data, data, size);
|
|
}
|
|
|
|
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;
|
|
used_buffer_space = 0;
|
|
} else {
|
|
if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) {
|
|
log_is_full = true;
|
|
}
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|