flight/modules/System/systemmod.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup LibrePilotModules LibrePilot Modules
   4  * @brief The LibrePilot Modules do the majority of the control in LibrePilot.  The
   5  * @ref SystemModule "System Module" starts all the other modules that then take care
   6  * of all the telemetry and control algorithms and such.  This is done through the @ref PIOS
   7  * "PIOS Hardware abstraction layer" which then contains hardware specific implementations
   8  * (currently only STM32 supported)
   9  *
  10  * @{
  11  * @addtogroup SystemModule System Module
  12  * @brief Initializes PIOS and other modules runs monitoring
  13  * After initializing all the modules (currently selected by Makefile but in
  14  * future controlled by configuration on SD card) runs basic monitoring and
  15  * alarms.
  16  * @{
  17  *
  18  * @file       systemmod.c
  19  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
  20  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015.
  21  * @brief      System module
  22  *
  23  * @see        The GNU Public License (GPL) Version 3
  24  *
  25  *****************************************************************************/
  26 /*
  27  * This program is free software; you can redistribute it and/or modify
  28  * it under the terms of the GNU General Public License as published by
  29  * the Free Software Foundation; either version 3 of the License, or
  30  * (at your option) any later version.
  31  *
  32  * This program is distributed in the hope that it will be useful, but
  33  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  34  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  35  * for more details.
  36  *
  37  * You should have received a copy of the GNU General Public License along
  38  * with this program; if not, write to the Free Software Foundation, Inc.,
  39  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  40  */
  41
  42 #include <openpilot.h>
  43 // private includes
  44 #include "inc/systemmod.h"
  45
  46 #include <notification.h>
  47 #ifdef PIOS_INCLUDE_WS2811
  48 #include <lednotification.h>
  49 #endif
  50
  51 // UAVOs
  52 #include <objectpersistence.h>
  53 #include <flightstatus.h>
  54 #include <systemstats.h>
  55 #include <systemsettings.h>
  56 #include <i2cstats.h>
  57 #include <taskinfo.h>
  58 #include <watchdogstatus.h>
  59 #include <callbackinfo.h>
  60 #include <hwsettings.h>
  61 #include <pios_flashfs.h>
  62 #include <pios_notify.h>
  63 #include <pios_task_monitor.h>
  64 #include <pios_board_init.h>
  65
  66
  67 #ifdef PIOS_INCLUDE_INSTRUMENTATION
  68 #include <instrumentation.h>
  69 #include <pios_instrumentation.h>
  70 #endif
  71
  72 #if defined(PIOS_INCLUDE_RFM22B)
  73 #include <oplinkstatus.h>
  74 #endif
  75
  76 // Flight Libraries
  77 #include <sanitycheck.h>
  78
  79
  80 // #define DEBUG_THIS_FILE
  81
  82 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(DEBUG_THIS_FILE)
  83 #define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, format,##__VA_ARGS__)
  84 #else
  85 #define DEBUG_MSG(format, ...)
  86 #endif
  87
  88 // Private constants
  89 #define SYSTEM_UPDATE_PERIOD_MS 250
  90
  91 #if defined(PIOS_SYSTEM_STACK_SIZE)
  92 #define STACK_SIZE_BYTES        PIOS_SYSTEM_STACK_SIZE
  93 #else
  94 #define STACK_SIZE_BYTES        1024
  95 #endif
  96
  97 #define TASK_PRIORITY           (tskIDLE_PRIORITY + 1)
  98
  99 // Private types
 100
 101 // Private variables
 102 static xTaskHandle systemTaskHandle;
 103 static xQueueHandle objectPersistenceQueue;
 104 static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
 105 static bool mallocFailed;
 106 static HwSettingsData bootHwSettings;
 107 static FrameType_t bootFrameType;
 108
 109 volatile int initTaskDone = 0;
 110
 111 // Private functions
 112 static void objectUpdatedCb(UAVObjEvent *ev);
 113 static void checkSettingsUpdatedCb(UAVObjEvent *ev);
 114 #ifdef DIAG_TASKS
 115 static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context);
 116 static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context);
 117 #endif
 118 static void updateStats();
 119 static void updateSystemAlarms();
 120 static void systemTask(void *parameters);
 121 #ifdef DIAG_I2C_WDG_STATS
 122 static void updateI2Cstats();
 123 static void updateWDGstats();
 124 #endif
 125
 126 extern uintptr_t pios_uavo_settings_fs_id;
 127 extern uintptr_t pios_user_fs_id;
 128
 129 /**
 130  * Create the module task.
 131  * \returns 0 on success or -1 if initialization failed
 132  */
 133 int32_t SystemModStart(void)
 134 {
 135     // Initialize vars
 136     stackOverflow = STACKOVERFLOW_NONE;
 137     mallocFailed  = false;
 138     // Create system task
 139     xTaskCreate(systemTask, "System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
 140
 141     return 0;
 142 }
 143
 144 /**
 145  * Initialize the module, called on startup.
 146  * \returns 0 on success or -1 if initialization failed
 147  */
 148 int32_t SystemModInitialize(void)
 149 {
 150     // Must registers objects here for system thread because ObjectManager started in OpenPilotInit
 151     SystemSettingsInitialize();
 152     SystemStatsInitialize();
 153     FlightStatusInitialize();
 154     ObjectPersistenceInitialize();
 155 #ifdef DIAG_TASKS
 156     TaskInfoInitialize();
 157     CallbackInfoInitialize();
 158 #endif
 159 #ifdef DIAG_I2C_WDG_STATS
 160     I2CStatsInitialize();
 161     WatchdogStatusInitialize();
 162 #endif
 163
 164 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 165     InstrumentationInit();
 166 #endif
 167
 168     objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 169     if (objectPersistenceQueue == NULL) {
 170         return -1;
 171     }
 172
 173     return 0;
 174 }
 175
 176 MODULE_INITCALL(SystemModInitialize, 0);
 177 /**
 178  * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
 179  */
 180 static void systemTask(__attribute__((unused)) void *parameters)
 181 {
 182     /* calibrate the cpu usage monitor */
 183     PIOS_TASK_MONITOR_CalibrateIdleCounter();
 184     /* board driver init */
 185     PIOS_Board_Init();
 186
 187     /* Initialize all modules */
 188     MODULE_INITIALISE_ALL;
 189
 190     while (!initTaskDone) {
 191         vTaskDelay(10);
 192     }
 193
 194 #ifndef PIOS_INCLUDE_WDG
 195 // if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop
 196 #define PIOS_WDG_Clear()
 197 #endif
 198     /* create all modules thread */
 199     MODULE_TASKCREATE_ALL;
 200
 201     /* start the delayed callback scheduler */
 202     PIOS_CALLBACKSCHEDULER_Start();
 203
 204     // Register task
 205     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
 206
 207     if (mallocFailed) {
 208         /* We failed to malloc during task creation,
 209          * system behaviour is undefined.  Reset and let
 210          * the BootFault code recover for us.
 211          */
 212         PIOS_SYS_Reset();
 213     }
 214 #if defined(PIOS_INCLUDE_IAP)
 215     /* Record a successful boot */
 216     PIOS_IAP_WriteBootCount(0);
 217 #endif
 218     // Listen for SettingPersistance object updates, connect a callback function
 219     ObjectPersistenceConnectQueue(objectPersistenceQueue);
 220
 221     // Load a copy of HwSetting active at boot time
 222     HwSettingsGet(&bootHwSettings);
 223     bootFrameType = GetCurrentFrameType();
 224     // Whenever the configuration changes, make sure it is safe to fly
 225     HwSettingsConnectCallback(checkSettingsUpdatedCb);
 226     SystemSettingsConnectCallback(checkSettingsUpdatedCb);
 227
 228 #ifdef DIAG_TASKS
 229     TaskInfoData taskInfoData;
 230     CallbackInfoData callbackInfoData;
 231 #endif
 232     // Main system loop
 233     while (1) {
 234         NotificationUpdateStatus();
 235         // Update the system statistics
 236         updateStats();
 237         // Update the system alarms
 238         updateSystemAlarms();
 239 #ifdef DIAG_I2C_WDG_STATS
 240         updateI2Cstats();
 241         updateWDGstats();
 242 #endif
 243
 244 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 245         InstrumentationPublishAllCounters();
 246 #endif
 247
 248 #ifdef DIAG_TASKS
 249         // Update the task status object
 250         PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);
 251         TaskInfoSet(&taskInfoData);
 252         // Update the callback status object
 253 // if(FALSE){
 254         PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData);
 255         CallbackInfoSet(&callbackInfoData);
 256 // }
 257 #endif
 258 // }
 259
 260
 261         UAVObjEvent ev;
 262         int delayTime = SYSTEM_UPDATE_PERIOD_MS;
 263
 264 #if defined(PIOS_INCLUDE_RFM22B)
 265
 266         // Update the OPLinkStatus UAVO
 267         OPLinkStatusData oplinkStatus;
 268         OPLinkStatusGet(&oplinkStatus);
 269
 270         if (pios_rfm22b_id) {
 271             // Get the other device stats.
 272             PIOS_RFM22B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM);
 273
 274             // Get the stats from the radio device
 275             struct rfm22b_stats radio_stats;
 276             PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
 277
 278             // Update the OPLInk status
 279             static bool first_time = true;
 280             static uint16_t prev_tx_count = 0;
 281             static uint16_t prev_rx_count = 0;
 282             static uint16_t prev_tx_seq   = 0;
 283             static uint16_t prev_rx_seq   = 0;
 284
 285             oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
 286             oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
 287             oplinkStatus.RxGood = radio_stats.rx_good;
 288             oplinkStatus.RxCorrected   = radio_stats.rx_corrected;
 289             oplinkStatus.RxErrors = radio_stats.rx_error;
 290             oplinkStatus.RxMissed = radio_stats.rx_missed;
 291             oplinkStatus.RxFailure     = radio_stats.rx_failure;
 292             oplinkStatus.TxDropped     = radio_stats.tx_dropped;
 293             oplinkStatus.TxFailure     = radio_stats.tx_failure;
 294             oplinkStatus.Resets      = radio_stats.resets;
 295             oplinkStatus.Timeouts    = radio_stats.timeouts;
 296             oplinkStatus.RSSI        = radio_stats.rssi;
 297             oplinkStatus.LinkQuality = radio_stats.link_quality;
 298             if (first_time) {
 299                 first_time = false;
 300             } else {
 301                 uint16_t tx_count   = radio_stats.tx_byte_count;
 302                 uint16_t rx_count   = radio_stats.rx_byte_count;
 303                 uint16_t tx_packets = radio_stats.tx_seq - prev_tx_seq;
 304                 uint16_t rx_packets = radio_stats.rx_seq - prev_rx_seq;
 305                 uint16_t tx_bytes   = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
 306                 uint16_t rx_bytes   = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
 307                 oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 308                 oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 309                 oplinkStatus.TXPacketRate = (uint16_t)((float)(tx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 310                 oplinkStatus.RXPacketRate = (uint16_t)((float)(rx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 311                 prev_tx_count = tx_count;
 312                 prev_rx_count = rx_count;
 313                 prev_tx_seq   = radio_stats.tx_seq;
 314                 prev_rx_seq   = radio_stats.rx_seq;
 315             }
 316             oplinkStatus.TXSeq     = radio_stats.tx_seq;
 317             oplinkStatus.RXSeq     = radio_stats.rx_seq;
 318
 319             oplinkStatus.LinkState = radio_stats.link_state;
 320         } else {
 321             oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
 322         }
 323         OPLinkStatusSet(&oplinkStatus);
 324
 325 #endif /* if defined(PIOS_INCLUDE_RFM22B) */
 326
 327         if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
 328             // If object persistence is updated call the callback
 329             objectUpdatedCb(&ev);
 330         }
 331     }
 332 }
 333
 334 /**
 335  * Function called in response to object updates
 336  */
 337 static void objectUpdatedCb(UAVObjEvent *ev)
 338 {
 339     ObjectPersistenceData objper;
 340     UAVObjHandle obj;
 341
 342     // If the object updated was the ObjectPersistence execute requested action
 343     if (ev->obj == ObjectPersistenceHandle()) {
 344         // Get object data
 345         ObjectPersistenceGet(&objper);
 346
 347         int retval = 1;
 348         FlightStatusData flightStatus;
 349         FlightStatusGet(&flightStatus);
 350
 351         // When this is called because of this method don't do anything
 352         if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR || objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
 353             return;
 354         }
 355
 356         // Execute action if disarmed
 357         if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
 358             retval = -1;
 359         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
 360             if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
 361                 // Get selected object
 362                 obj = UAVObjGetByID(objper.ObjectID);
 363                 if (obj == 0) {
 364                     return;
 365                 }
 366                 // Load selected instance
 367                 retval = UAVObjLoad(obj, objper.InstanceID);
 368             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 369                 retval = UAVObjLoadSettings();
 370             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 371                 retval = UAVObjLoadMetaobjects();
 372             }
 373         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
 374             if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
 375                 // Get selected object
 376                 obj = UAVObjGetByID(objper.ObjectID);
 377                 if (obj == 0) {
 378                     return;
 379                 }
 380                 // Save selected instance
 381                 retval = UAVObjSave(obj, objper.InstanceID);
 382
 383                 // Not sure why this is needed
 384                 vTaskDelay(10);
 385
 386                 // Verify saving worked
 387                 if (retval == 0) {
 388                     retval = UAVObjLoad(obj, objper.InstanceID);
 389                 }
 390             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 391                 retval = UAVObjSaveSettings();
 392             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 393                 retval = UAVObjSaveMetaobjects();
 394             }
 395         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
 396             if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
 397                 // Get selected object
 398                 obj = UAVObjGetByID(objper.ObjectID);
 399                 if (obj == 0) {
 400                     return;
 401                 }
 402                 // Delete selected instance
 403                 retval = UAVObjDelete(obj, objper.InstanceID);
 404             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 405                 retval = UAVObjDeleteSettings();
 406             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 407                 retval = UAVObjDeleteMetaobjects();
 408             }
 409         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
 410 #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
 411             retval = PIOS_FLASHFS_Format(0);
 412 #else
 413             retval = -1;
 414 #endif
 415         }
 416         switch (retval) {
 417         case 0:
 418             objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
 419             ObjectPersistenceSet(&objper);
 420             break;
 421         case -1:
 422             objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
 423             ObjectPersistenceSet(&objper);
 424             break;
 425         default:
 426             break;
 427         }
 428     }
 429 }
 430
 431 /**
 432  * Called whenever hardware settings changed
 433  */
 434 static void checkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 435 {
 436     HwSettingsData currentHwSettings;
 437
 438     HwSettingsGet(&currentHwSettings);
 439     FrameType_t currentFrameType = GetCurrentFrameType();
 440     // check whether the Hw Configuration has changed from the one used at boot time
 441     if ((memcmp(&bootHwSettings, &currentHwSettings, sizeof(HwSettingsData)) != 0) ||
 442         (currentFrameType != bootFrameType)) {
 443         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
 444     }
 445 }
 446
 447 #ifdef DIAG_TASKS
 448 static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context)
 449 {
 450     TaskInfoData *taskData = (TaskInfoData *)context;
 451
 452     // By convention, there is a direct mapping between task monitor task_id's and members
 453     // of the TaskInfoXXXXElem enums
 454     PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
 455     TaskInfoRunningToArray(taskData->Running)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
 456     ((uint16_t *)&taskData->StackRemaining)[task_id]   = task_info->stack_remaining;
 457     ((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage;
 458 }
 459
 460 static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context)
 461 {
 462     CallbackInfoData *callbackData = (CallbackInfoData *)context;
 463
 464     if (callback_id < 0) {
 465         return;
 466     }
 467     // delayed callback scheduler reports callback stack overflows as remaininng: -1
 468 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
 469     if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) {
 470         stackOverflow = STACKOVERFLOW_WARNING;
 471     }
 472 #endif
 473     // By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members
 474     // of the CallbackInfoXXXXElem enums
 475     PIOS_DEBUG_Assert(callback_id < CALLBACKINFO_RUNNING_NUMELEM);
 476     ((uint8_t *)&callbackData->Running)[callback_id] = callback_info->is_running;
 477     ((uint32_t *)&callbackData->RunningTime)[callback_id]   = callback_info->running_time_count;
 478     ((int16_t *)&callbackData->StackRemaining)[callback_id] = callback_info->stack_remaining;
 479 }
 480 #endif /* ifdef DIAG_TASKS */
 481
 482 /**
 483  * Called periodically to update the I2C statistics
 484  */
 485 #ifdef DIAG_I2C_WDG_STATS
 486 static void updateI2Cstats()
 487 {
 488 #if defined(PIOS_INCLUDE_I2C)
 489     I2CStatsData i2cStats;
 490     I2CStatsGet(&i2cStats);
 491
 492     struct pios_i2c_fault_history history;
 493     PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors);
 494
 495     for (uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) {
 496         i2cStats.evirq_log[i] = history.evirq[i];
 497         i2cStats.erirq_log[i] = history.erirq[i];
 498         i2cStats.event_log[i] = history.event[i];
 499         i2cStats.state_log[i] = history.state[i];
 500     }
 501     i2cStats.last_error_type = history.type;
 502     I2CStatsSet(&i2cStats);
 503 #endif
 504 }
 505
 506 static void updateWDGstats()
 507 {
 508     WatchdogStatusData watchdogStatus;
 509
 510     watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
 511     watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
 512     WatchdogStatusSet(&watchdogStatus);
 513 }
 514 #endif /* ifdef DIAG_I2C_WDG_STATS */
 515
 516 /**
 517  * Called periodically to update the system stats
 518  */
 519 static uint16_t GetFreeIrqStackSize(void)
 520 {
 521     uint32_t i = 0x200;
 522
 523 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
 524     extern uint32_t _irq_stack_top;
 525     extern uint32_t _irq_stack_end;
 526     uint32_t pattern    = 0x0000A5A5;
 527     uint32_t *ptr       = &_irq_stack_end;
 528
 529 #if 1 /* the ugly way accurate but takes more time, useful for debugging */
 530     uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3) / 4;
 531
 532     for (i = 0; i < stack_size; i++) {
 533         if (ptr[i] != pattern) {
 534             i = i * 4;
 535             break;
 536         }
 537     }
 538 #else /* faster way but not accurate */
 539     if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) {
 540         i = IRQSTACK_LIMIT_CRITICAL - 1;
 541     } else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) {
 542         i = IRQSTACK_LIMIT_WARNING - 1;
 543     } else {
 544         i = IRQSTACK_LIMIT_WARNING;
 545     }
 546 #endif
 547 #endif /* if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) */
 548     return i;
 549 }
 550
 551 /**
 552  * Called periodically to update the system stats
 553  */
 554 static void updateStats()
 555 {
 556     SystemStatsData stats;
 557
 558     // Get stats and update
 559     SystemStatsGet(&stats);
 560     stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
 561 #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
 562     // POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
 563     stats.SystemModStackRemaining = 128;
 564     stats.HeapRemaining = 10240;
 565 #else
 566     stats.HeapRemaining = xPortGetFreeHeapSize();
 567     stats.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4;
 568 #endif
 569
 570     // Get Irq stack status
 571     stats.IRQStackRemaining = GetFreeIrqStackSize();
 572
 573 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
 574     static struct PIOS_FLASHFS_Stats fsStats;
 575
 576     if (pios_uavo_settings_fs_id) {
 577         PIOS_FLASHFS_GetStats(pios_uavo_settings_fs_id, &fsStats);
 578         stats.SysSlotsFree   = fsStats.num_free_slots;
 579         stats.SysSlotsActive = fsStats.num_active_slots;
 580     }
 581     if (pios_user_fs_id) {
 582         PIOS_FLASHFS_GetStats(pios_user_fs_id, &fsStats);
 583         stats.UsrSlotsFree   = fsStats.num_free_slots;
 584         stats.UsrSlotsActive = fsStats.num_active_slots;
 585     }
 586 #endif
 587     stats.CPUIdleTicks     = PIOS_TASK_MONITOR_GetIdleTicksCount();
 588     stats.CPUZeroLoadTicks = PIOS_TASK_MONITOR_GetZeroLoadTicksCount();
 589     stats.CPULoad = 100 - (uint8_t)((100 * stats.CPUIdleTicks) / stats.CPUZeroLoadTicks);
 590
 591 #if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
 592     float temp_voltage = PIOS_ADC_PinGetVolt(PIOS_ADC_TEMPERATURE_PIN);
 593     stats.CPUTemp = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
 594 #endif
 595     SystemStatsSet(&stats);
 596 }
 597
 598 /**
 599  * Update system alarms
 600  */
 601 static void updateSystemAlarms()
 602 {
 603     SystemStatsData stats;
 604     UAVObjStats objStats;
 605     EventStats evStats;
 606
 607     SystemStatsGet(&stats);
 608
 609     // Check heap, IRQ stack and malloc failures
 610     if (mallocFailed || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
 611 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
 612         || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
 613 #endif
 614         ) {
 615         AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
 616     } else if ((stats.HeapRemaining < HEAP_LIMIT_WARNING)
 617 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
 618                || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
 619 #endif
 620                ) {
 621         AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
 622     } else {
 623         AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
 624     }
 625
 626     // Check CPU load
 627     if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
 628         AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
 629     } else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) {
 630         AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
 631     } else {
 632         AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
 633     }
 634
 635     // Check for stack overflow
 636     switch (stackOverflow) {
 637     case STACKOVERFLOW_NONE:
 638         AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
 639         break;
 640     case STACKOVERFLOW_WARNING:
 641         AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_WARNING);
 642         break;
 643     default:
 644         AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
 645     }
 646
 647     // Check for event errors
 648     UAVObjGetStats(&objStats);
 649     EventGetStats(&evStats);
 650     UAVObjClearStats();
 651     EventClearStats();
 652     if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
 653         AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
 654     } else {
 655         AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
 656     }
 657
 658     if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
 659         SystemStatsData sysStats;
 660         SystemStatsGet(&sysStats);
 661         sysStats.EventSystemWarningID    = evStats.lastErrorID;
 662         sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
 663         sysStats.ObjectManagerQueueID    = objStats.lastQueueErrorID;
 664         SystemStatsSet(&sysStats);
 665     }
 666 }
 667
 668 /**
 669  * Called by the RTOS when the CPU is idle,
 670  */
 671 void vApplicationIdleHook(void)
 672 {
 673     PIOS_TASK_MONITOR_IdleHook();
 674     NotificationOnboardLedsRun();
 675 #ifdef PIOS_INCLUDE_WS2811
 676     LedNotificationExtLedsRun();
 677 #endif
 678 }
 679 /**
 680  * Called by the RTOS when a stack overflow is detected.
 681  */
 682 #define DEBUG_STACK_OVERFLOW 0
 683 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
 684                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 685 {
 686 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
 687     stackOverflow = STACKOVERFLOW_CRITICAL;
 688 #if DEBUG_STACK_OVERFLOW
 689     static volatile bool wait_here = true;
 690     while (wait_here) {
 691         ;
 692     }
 693     wait_here = true;
 694 #endif
 695 #endif
 696 }
 697
 698 /**
 699  * Called by the RTOS when a malloc call fails.
 700  */
 701 #define DEBUG_MALLOC_FAILURES 0
 702 void vApplicationMallocFailedHook(void)
 703 {
 704     mallocFailed = true;
 705 #if DEBUG_MALLOC_FAILURES
 706     static volatile bool wait_here = true;
 707     while (wait_here) {
 708         ;
 709     }
 710     wait_here = true;
 711 #endif
 712 }
 713
 714 /**
 715  * @}
 716  * @}
 717  */