flight/pios/common/pios_debuglog.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @defgroup   PIOS_DEBUGLOG Flash log debugging Functions
   6  * @brief Debugging functionality
   7  * @{
   8  *
   9  * @file       pios_debuglog.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Debugging Functions
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 /* Project Includes */
  32 #include "pios.h"
  33 #include "uavobjectmanager.h"
  34 #include "debuglogentry.h"
  35 #include "callbackinfo.h"
  36
  37 // global definitions
  38 #ifdef PIOS_INCLUDE_DEBUGLOG
  39
  40 // Global variables
  41 extern uintptr_t pios_user_fs_id; // flash filesystem for logging
  42
  43 static xSemaphoreHandle mutex = 0;
  44 #define mutexlock()   xSemaphoreTakeRecursive(mutex, portMAX_DELAY)
  45 #define mutexunlock() xSemaphoreGiveRecursive(mutex)
  46
  47 static bool logging_enabled = false;
  48 #define MAX_CONSECUTIVE_FAILS_COUNT 10
  49 static bool log_is_full     = false;
  50 static uint8_t fails_count  = 0;
  51 static uint16_t flightnum   = 0;
  52 static uint16_t lognum = 0;
  53
  54 #define BUFFERS_COUNT 2
  55 static DebugLogEntryData *current_buffer = 0;
  56 static DebugLogEntryData *buffers[BUFFERS_COUNT] = { 0, 0 };
  57 static uint8_t current_write_buffer_index;
  58 static uint8_t next_read_buffer_index;
  59
  60 #define LOG_ENTRY_MAX_DATA_SIZE (sizeof(((DebugLogEntryData *)0)->Data))
  61 #define LOG_ENTRY_HEADER_SIZE   (sizeof(DebugLogEntryData) - LOG_ENTRY_MAX_DATA_SIZE)
  62 // build the obj_id as a DEBUGLOGENTRY ID with least significant byte zeroed and filled with flight number
  63 #define LOG_GET_FLIGHT_OBJID(x) ((DEBUGLOGENTRY_OBJID & ~0xFF) | (x & 0xFF))
  64
  65 static uint32_t used_buffer_space = 0;
  66
  67 #define CBTASK_PRIORITY   CALLBACK_TASK_AUXILIARY
  68 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
  69 #define CB_TIMEOUT        100
  70 #define STACK_SIZE_BYTES  512
  71 static DelayedCallbackInfo *callbackHandle;
  72
  73 /* Private Function Prototypes */
  74 static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data);
  75 static bool write_current_buffer();
  76 static void writeTask();
  77 static uint8_t get_blocks_free();
  78 /**
  79  * @brief Initialize the log facility
  80  */
  81 void PIOS_DEBUGLOG_Initialize()
  82 {
  83     if (!mutex) {
  84         mutex = xSemaphoreCreateRecursiveMutex();
  85         for (uint32_t i = 0; i < BUFFERS_COUNT; i++) {
  86             buffers[i] = pios_malloc(sizeof(DebugLogEntryData));
  87         }
  88         current_write_buffer_index = 0;
  89         next_read_buffer_index     = 0;
  90         current_buffer = buffers[current_write_buffer_index];
  91     }
  92
  93     if (!current_buffer) {
  94         return;
  95     }
  96     mutexlock();
  97     lognum      = 0;
  98     flightnum   = 0;
  99     fails_count = 0;
 100     used_buffer_space = 0;
 101     log_is_full = false;
 102     while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)current_buffer, sizeof(DebugLogEntryData)) == 0) {
 103         flightnum++;
 104     }
 105     mutexunlock();
 106     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&writeTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_DEBUGLOG, STACK_SIZE_BYTES);
 107     PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, CB_TIMEOUT, CALLBACK_UPDATEMODE_LATER);
 108 }
 109
 110
 111 /**
 112  * @brief Enables or Disables logging globally
 113  * @param[in] enable or disable logging
 114  */
 115 void PIOS_DEBUGLOG_Enable(uint8_t enabled)
 116 {
 117     // increase the flight num as soon as logging is disabled
 118     if (logging_enabled && !enabled) {
 119         flightnum++;
 120         lognum = 0;
 121     }
 122     logging_enabled = enabled;
 123 }
 124
 125 /**
 126  * @brief Write a debug log entry with a uavobject
 127  * @param[in] objectid
 128  * @param[in] instanceid
 129  * @param[in] instanceid
 130  * @param[in] size of object
 131  * @param[in] data buffer
 132  */
 133 void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
 134 {
 135     if (!logging_enabled || !current_buffer || log_is_full) {
 136         return;
 137     }
 138     mutexlock();
 139
 140     enqueue_data(objid, instid, size, data);
 141
 142     mutexunlock();
 143 }
 144 /**
 145  * @brief Write a debug log entry with text
 146  * @param[in] format - as in printf
 147  * @param[in] variable arguments for printf
 148  * @param...
 149  */
 150 void PIOS_DEBUGLOG_Printf(char *format, ...)
 151 {
 152     if (!logging_enabled || !current_buffer || log_is_full) {
 153         return;
 154     }
 155
 156     va_list args;
 157     va_start(args, format);
 158     mutexlock();
 159     // flush any pending buffer before writing debug text
 160     if (used_buffer_space) {
 161         write_current_buffer();
 162     }
 163     memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
 164     vsnprintf((char *)current_buffer->Data, sizeof(current_buffer->Data), (char *)format, args);
 165     current_buffer->Flight     = flightnum;
 166
 167     current_buffer->FlightTime = PIOS_DELAY_GetuS();
 168
 169     current_buffer->Entry      = lognum;
 170     current_buffer->Type       = DEBUGLOGENTRY_TYPE_TEXT;
 171     current_buffer->ObjectID   = 0;
 172     current_buffer->InstanceID = 0;
 173     current_buffer->Size       = strlen((const char *)current_buffer->Data);
 174
 175     if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)current_buffer, sizeof(DebugLogEntryData)) == 0) {
 176         lognum++;
 177     }
 178     mutexunlock();
 179 }
 180
 181
 182 /**
 183  * @brief Load one object instance from the filesystem
 184  * @param[out] buffer where to store the uavobject
 185  * @param[in] log entry from which flight
 186  * @param[in] log entry sequence number
 187  * @return 0 if success or error code
 188  * @retval -1 if fs_id is not a valid filesystem instance
 189  * @retval -2 if failed to start transaction
 190  * @retval -3 if object not found in filesystem
 191  * @retval -4 if object size in filesystem does not exactly match buffer size
 192  * @retval -5 if reading the object data from flash fails
 193  */
 194 int32_t PIOS_DEBUGLOG_Read(void *mybuffer, uint16_t flight, uint16_t inst)
 195 {
 196     PIOS_Assert(mybuffer);
 197     return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flight), inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData));
 198 }
 199
 200 /**
 201  * @brief Retrieve run time info of logging system
 202  * @param[out] current flight number
 203  * @param[out] next entry number
 204  * @param[out] free slots in filesystem
 205  * @param[out] used slots in filesystem
 206  */
 207 void PIOS_DEBUGLOG_Info(uint16_t *flight, uint16_t *entry, uint16_t *free, uint16_t *used)
 208 {
 209     if (flight) {
 210         *flight = flightnum;
 211     }
 212     if (entry) {
 213         *entry = lognum;
 214     }
 215     struct PIOS_FLASHFS_Stats stats = { 0, 0 };
 216     PIOS_FLASHFS_GetStats(pios_user_fs_id, &stats);
 217     if (free) {
 218         *free = stats.num_free_slots;
 219     }
 220     if (used) {
 221         *used = stats.num_active_slots;
 222     }
 223 }
 224
 225 /**
 226  * @brief Format entire flash memory!!!
 227  */
 228 void PIOS_DEBUGLOG_Format(void)
 229 {
 230     mutexlock();
 231     PIOS_FLASHFS_Format(pios_user_fs_id);
 232     lognum      = 0;
 233     flightnum   = 0;
 234     log_is_full = false;
 235     fails_count = 0;
 236     used_buffer_space = 0;
 237     mutexunlock();
 238 }
 239
 240 void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
 241 {
 242     DebugLogEntryData *entry;
 243
 244     // start a new block
 245     if (!used_buffer_space) {
 246         entry = current_buffer;
 247         memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
 248         used_buffer_space += size;
 249     } else {
 250         // if an instance is being filled and there is enough space, does enqueues new data.
 251         if (used_buffer_space + size + LOG_ENTRY_HEADER_SIZE > LOG_ENTRY_MAX_DATA_SIZE) {
 252             current_buffer->Type = DEBUGLOGENTRY_TYPE_MULTIPLEUAVOBJECTS;
 253             if (!write_current_buffer()) {
 254                 return;
 255             }
 256             entry = current_buffer;
 257             memset(current_buffer->Data, 0xff, sizeof(current_buffer->Data));
 258             used_buffer_space += size;
 259         } else {
 260             entry = (DebugLogEntryData *)&current_buffer->Data[used_buffer_space];
 261             used_buffer_space += size + LOG_ENTRY_HEADER_SIZE;
 262         }
 263     }
 264
 265     entry->Flight     = flightnum;
 266     entry->FlightTime = PIOS_DELAY_GetuS();
 267     entry->Entry = lognum;
 268     entry->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT;
 269     entry->ObjectID   = objid;
 270     entry->InstanceID = instid;
 271     if (size > sizeof(current_buffer->Data)) {
 272         size = sizeof(current_buffer->Data);
 273     }
 274     entry->Size = size;
 275
 276     memcpy(entry->Data, data, size);
 277 }
 278
 279 bool write_current_buffer()
 280 {
 281     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 282     // Check if queue is full
 283
 284     if (get_blocks_free() > 0) {
 285         current_write_buffer_index = (current_write_buffer_index + 1) % BUFFERS_COUNT;
 286         current_buffer    = buffers[current_write_buffer_index];
 287         used_buffer_space = 0;
 288         return true;
 289     } else {
 290         return false;
 291     }
 292 }
 293
 294 static uint8_t get_blocks_free()
 295 {
 296     uint8_t used_blocks = current_write_buffer_index - next_read_buffer_index;
 297
 298     if (current_write_buffer_index < next_read_buffer_index) {
 299         used_blocks = (BUFFERS_COUNT - next_read_buffer_index) + current_write_buffer_index;
 300     }
 301     return (BUFFERS_COUNT - used_blocks) - 1;
 302 }
 303
 304 static void writeTask()
 305 {
 306     if (current_write_buffer_index != next_read_buffer_index) {
 307         // not enough space, write the block and start a new one
 308         if (PIOS_FLASHFS_ObjSave(pios_user_fs_id,
 309                                  LOG_GET_FLIGHT_OBJID(flightnum), lognum,
 310                                  (uint8_t *)buffers[next_read_buffer_index],
 311                                  sizeof(DebugLogEntryData)) == 0) {
 312             next_read_buffer_index = (next_read_buffer_index + 1) % BUFFERS_COUNT;
 313             lognum++;
 314             fails_count = 0;
 315         } else {
 316             if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) {
 317                 log_is_full = true;
 318             }
 319         }
 320     }
 321 }
 322 #endif /* ifdef PIOS_INCLUDE_DEBUGLOG */
 323 /**
 324  * @}
 325  * @}
 326  */