flight/modules/System/systemmod.c

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