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-2016.
  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 static void updateI2Cstats();
 122 #ifdef DIAG_I2C_WDG_STATS
 123 static void updateWDGstats();
 124 #endif
 125
 126 #ifdef PIOS_INCLUDE_I2C
 127 #define I2C_ERROR_ACTIVITY_TIMEOUT_SECONDS 2
 128 #define I2C_ERROR_ACTIVITY_TIMEOUT         (I2C_ERROR_ACTIVITY_TIMEOUT_SECONDS * 1000 / SYSTEM_UPDATE_PERIOD_MS)
 129 static uint8_t i2c_error_activity[PIOS_I2C_ERROR_COUNT_NUMELEM];
 130 #endif
 131
 132 #ifdef PIOS_INCLUDE_RFM22B
 133 static uint8_t previousRFXtalCap;
 134 static void oplinkSettingsUpdatedCb(UAVObjEvent *ev);
 135 #endif
 136
 137 extern uintptr_t pios_uavo_settings_fs_id;
 138 extern uintptr_t pios_user_fs_id;
 139
 140 /**
 141  * Create the module task.
 142  * \returns 0 on success or -1 if initialization failed
 143  */
 144 int32_t SystemModStart(void)
 145 {
 146     // Initialize vars
 147     stackOverflow = STACKOVERFLOW_NONE;
 148     mallocFailed  = false;
 149     // Create system task
 150     xTaskCreate(systemTask, "System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
 151
 152     return 0;
 153 }
 154
 155 /**
 156  * Initialize the module, called on startup.
 157  * \returns 0 on success or -1 if initialization failed
 158  */
 159 int32_t SystemModInitialize(void)
 160 {
 161     // Must registers objects here for system thread because ObjectManager started in OpenPilotInit
 162     SystemSettingsInitialize();
 163     SystemStatsInitialize();
 164     FlightStatusInitialize();
 165     ObjectPersistenceInitialize();
 166 #ifdef DIAG_TASKS
 167     TaskInfoInitialize();
 168     CallbackInfoInitialize();
 169 #endif
 170 #ifdef DIAG_I2C_WDG_STATS
 171     I2CStatsInitialize();
 172     WatchdogStatusInitialize();
 173 #endif
 174
 175 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 176     InstrumentationInit();
 177 #endif
 178
 179     objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 180     if (objectPersistenceQueue == NULL) {
 181         return -1;
 182     }
 183
 184     return 0;
 185 }
 186
 187 MODULE_INITCALL(SystemModInitialize, 0);
 188 /**
 189  * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
 190  */
 191 static void systemTask(__attribute__((unused)) void *parameters)
 192 {
 193     /* calibrate the cpu usage monitor */
 194     PIOS_TASK_MONITOR_CalibrateIdleCounter();
 195     /* board driver init */
 196     PIOS_Board_Init();
 197
 198     /* Initialize all modules */
 199     MODULE_INITIALISE_ALL;
 200
 201     while (!initTaskDone) {
 202         vTaskDelay(10);
 203     }
 204
 205 #ifndef PIOS_INCLUDE_WDG
 206 // if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop
 207 #define PIOS_WDG_Clear()
 208 #endif
 209     /* create all modules thread */
 210     MODULE_TASKCREATE_ALL;
 211
 212     /* start the delayed callback scheduler */
 213     PIOS_CALLBACKSCHEDULER_Start();
 214
 215     // Register task
 216     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
 217
 218     if (mallocFailed) {
 219         /* We failed to malloc during task creation,
 220          * system behaviour is undefined.  Reset and let
 221          * the BootFault code recover for us.
 222          */
 223         PIOS_SYS_Reset();
 224     }
 225 #if defined(PIOS_INCLUDE_IAP)
 226     /* Record a successful boot */
 227     PIOS_IAP_WriteBootCount(0);
 228 #endif
 229     // Listen for SettingPersistance object updates, connect a callback function
 230     ObjectPersistenceConnectQueue(objectPersistenceQueue);
 231
 232     // Load a copy of HwSetting active at boot time
 233     HwSettingsGet(&bootHwSettings);
 234     bootFrameType = GetCurrentFrameType();
 235     // Whenever the configuration changes, make sure it is safe to fly
 236     HwSettingsConnectCallback(checkSettingsUpdatedCb);
 237     SystemSettingsConnectCallback(checkSettingsUpdatedCb);
 238
 239 #ifdef PIOS_INCLUDE_RFM22B
 240     // Initialize previousRFXtalCap used by callback
 241     OPLinkSettingsRFXtalCapGet(&previousRFXtalCap);
 242     OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb);
 243 #endif
 244
 245 #ifdef DIAG_TASKS
 246     TaskInfoData taskInfoData;
 247     memset(&taskInfoData, 0, sizeof(TaskInfoData));
 248     CallbackInfoData callbackInfoData;
 249     memset(&callbackInfoData, 0, sizeof(CallbackInfoData));
 250 #endif
 251     // Main system loop
 252     while (1) {
 253         NotificationUpdateStatus();
 254         // Update the system statistics
 255         updateStats();
 256
 257         // Update I2C stats
 258         updateI2Cstats();
 259
 260         // Update the system alarms
 261         updateSystemAlarms();
 262 #ifdef DIAG_I2C_WDG_STATS
 263         updateWDGstats();
 264 #endif
 265
 266 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 267         InstrumentationPublishAllCounters();
 268 #endif
 269
 270 #ifdef DIAG_TASKS
 271         // Update the task status object
 272         PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);
 273         TaskInfoSet(&taskInfoData);
 274         // Update the callback status object
 275         PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData);
 276         CallbackInfoSet(&callbackInfoData);
 277 #endif
 278
 279         UAVObjEvent ev;
 280         int delayTime = SYSTEM_UPDATE_PERIOD_MS;
 281
 282 #if defined(PIOS_INCLUDE_RFM22B)
 283
 284         // Update the OPLinkStatus UAVO
 285         OPLinkStatusData oplinkStatus;
 286         OPLinkStatusGet(&oplinkStatus);
 287         oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
 288
 289         if (pios_rfm22b_id) {
 290             // Get the other device stats.
 291             PIOS_RFM22B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM);
 292
 293             // Get the stats from the radio device
 294             struct rfm22b_stats radio_stats;
 295             PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
 296
 297             // Update the OPLInk status
 298             static bool first_time = true;
 299             static uint16_t prev_tx_count = 0;
 300             static uint16_t prev_rx_count = 0;
 301             static uint16_t prev_tx_seq   = 0;
 302             static uint16_t prev_rx_seq   = 0;
 303
 304             oplinkStatus.DeviceID      = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
 305             oplinkStatus.RxGood        = radio_stats.rx_good;
 306             oplinkStatus.RxCorrected   = radio_stats.rx_corrected;
 307             oplinkStatus.RxErrors      = radio_stats.rx_error;
 308             oplinkStatus.RxMissed      = radio_stats.rx_missed;
 309             oplinkStatus.RxFailure     = radio_stats.rx_failure;
 310             oplinkStatus.TxDropped     = radio_stats.tx_dropped;
 311             oplinkStatus.TxFailure     = radio_stats.tx_failure;
 312             oplinkStatus.Resets        = radio_stats.resets;
 313             oplinkStatus.Timeouts      = radio_stats.timeouts;
 314             oplinkStatus.RSSI = radio_stats.rssi;
 315             oplinkStatus.LinkQuality   = radio_stats.link_quality;
 316             oplinkStatus.AFCCorrection = radio_stats.afc_correction;
 317             if (first_time) {
 318                 first_time = false;
 319             } else {
 320                 uint16_t tx_count   = radio_stats.tx_byte_count;
 321                 uint16_t rx_count   = radio_stats.rx_byte_count;
 322                 uint16_t tx_packets = radio_stats.tx_seq - prev_tx_seq;
 323                 uint16_t rx_packets = radio_stats.rx_seq - prev_rx_seq;
 324                 uint16_t tx_bytes   = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
 325                 uint16_t rx_bytes   = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
 326                 oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 327                 oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 328                 oplinkStatus.TXPacketRate = (uint16_t)((float)(tx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 329                 oplinkStatus.RXPacketRate = (uint16_t)((float)(rx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
 330                 prev_tx_count = tx_count;
 331                 prev_rx_count = rx_count;
 332                 prev_tx_seq   = radio_stats.tx_seq;
 333                 prev_rx_seq   = radio_stats.rx_seq;
 334             }
 335             oplinkStatus.TXSeq     = radio_stats.tx_seq;
 336             oplinkStatus.RXSeq     = radio_stats.rx_seq;
 337
 338             oplinkStatus.LinkState = radio_stats.link_state;
 339         } else {
 340             oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
 341         }
 342         OPLinkStatusSet(&oplinkStatus);
 343
 344 #endif /* if defined(PIOS_INCLUDE_RFM22B) */
 345
 346         if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
 347             // If object persistence is updated call the callback
 348             objectUpdatedCb(&ev);
 349         }
 350     }
 351 }
 352
 353 /**
 354  * Function called in response to object updates
 355  */
 356 static void objectUpdatedCb(UAVObjEvent *ev)
 357 {
 358     ObjectPersistenceData objper;
 359     UAVObjHandle obj;
 360
 361     // If the object updated was the ObjectPersistence execute requested action
 362     if (ev->obj == ObjectPersistenceHandle()) {
 363         // Get object data
 364         ObjectPersistenceGet(&objper);
 365
 366         int retval = 1;
 367         FlightStatusData flightStatus;
 368         FlightStatusGet(&flightStatus);
 369
 370         // When this is called because of this method don't do anything
 371         if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR || objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
 372             return;
 373         }
 374
 375         // Execute action if disarmed
 376         if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
 377             retval = -1;
 378         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
 379             if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
 380                 // Get selected object
 381                 obj = UAVObjGetByID(objper.ObjectID);
 382                 if (obj == 0) {
 383                     return;
 384                 }
 385                 // Load selected instance
 386                 retval = UAVObjLoad(obj, objper.InstanceID);
 387             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 388                 retval = UAVObjLoadSettings();
 389             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 390                 retval = UAVObjLoadMetaobjects();
 391             }
 392         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
 393             if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
 394                 // Get selected object
 395                 obj = UAVObjGetByID(objper.ObjectID);
 396                 if (obj == 0) {
 397                     return;
 398                 }
 399                 // Save selected instance
 400                 retval = UAVObjSave(obj, objper.InstanceID);
 401
 402                 // Not sure why this is needed
 403                 vTaskDelay(10);
 404
 405                 // Verify saving worked
 406                 if (retval == 0) {
 407                     retval = UAVObjLoad(obj, objper.InstanceID);
 408                 }
 409             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 410                 retval = UAVObjSaveSettings();
 411             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 412                 retval = UAVObjSaveMetaobjects();
 413             }
 414         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
 415             if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
 416                 // Get selected object
 417                 obj = UAVObjGetByID(objper.ObjectID);
 418                 if (obj == 0) {
 419                     return;
 420                 }
 421                 // Delete selected instance
 422                 retval = UAVObjDelete(obj, objper.InstanceID);
 423             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 424                 retval = UAVObjDeleteSettings();
 425             } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 426                 retval = UAVObjDeleteMetaobjects();
 427             }
 428         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
 429 #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
 430             retval = PIOS_FLASHFS_Format(0);
 431 #else
 432             retval = -1;
 433 #endif
 434         }
 435         switch (retval) {
 436         case 0:
 437             objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
 438             ObjectPersistenceSet(&objper);
 439             break;
 440         case -1:
 441             objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
 442             ObjectPersistenceSet(&objper);
 443             break;
 444         default:
 445             break;
 446         }
 447     }
 448 }
 449
 450 /**
 451  * Called whenever hardware settings changed
 452  */
 453 static void checkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 454 {
 455     HwSettingsData currentHwSettings;
 456
 457     HwSettingsGet(&currentHwSettings);
 458     FrameType_t currentFrameType = GetCurrentFrameType();
 459     // check whether the Hw Configuration has changed from the one used at boot time
 460     if ((memcmp(&bootHwSettings, &currentHwSettings, sizeof(HwSettingsData)) != 0) ||
 461         (currentFrameType != bootFrameType)) {
 462         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
 463     }
 464 }
 465
 466 #ifdef PIOS_INCLUDE_RFM22B
 467 /**
 468  * Called whenever OPLink settings changed
 469  */
 470 static void oplinkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 471 {
 472     uint8_t currentRFXtalCap;
 473
 474     OPLinkSettingsRFXtalCapGet(&currentRFXtalCap);
 475
 476     // Check if RFXtalCap value changed
 477     if (currentRFXtalCap != previousRFXtalCap) {
 478         PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, currentRFXtalCap);
 479         PIOS_RFM22B_Reinit(pios_rfm22b_id);
 480         previousRFXtalCap = currentRFXtalCap;
 481     }
 482 }
 483 #endif /* ifdef PIOS_INCLUDE_RFM22B */
 484
 485 #ifdef DIAG_TASKS
 486 static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context)
 487 {
 488     TaskInfoData *taskData = (TaskInfoData *)context;
 489
 490     // By convention, there is a direct mapping between task monitor task_id's and members
 491     // of the TaskInfoXXXXElem enums
 492     PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
 493     TaskInfoRunningToArray(taskData->Running)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
 494     ((uint16_t *)&taskData->StackRemaining)[task_id]   = task_info->stack_remaining;
 495     ((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage;
 496 }
 497
 498 static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context)
 499 {
 500     CallbackInfoData *callbackData = (CallbackInfoData *)context;
 501
 502     if (callback_id < 0) {
 503         return;
 504     }
 505     // delayed callback scheduler reports callback stack overflows as remaininng: -1
 506 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
 507     if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) {
 508         stackOverflow = STACKOVERFLOW_WARNING;
 509     }
 510 #endif
 511     // By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members
 512     // of the CallbackInfoXXXXElem enums
 513     PIOS_DEBUG_Assert(callback_id < CALLBACKINFO_RUNNING_NUMELEM);
 514     ((uint8_t *)&callbackData->Running)[callback_id] = callback_info->is_running;
 515     ((uint32_t *)&callbackData->RunningTime)[callback_id]   = callback_info->running_time_count;
 516     ((int16_t *)&callbackData->StackRemaining)[callback_id] = callback_info->stack_remaining;
 517 }
 518 #endif /* ifdef DIAG_TASKS */
 519
 520 /**
 521  * Called periodically (every SYSTEM_UPDATE_PERIOD_MS milliseconds) to update the I2C statistics
 522  */
 523 static void updateI2Cstats()
 524 {
 525 #if defined(PIOS_INCLUDE_I2C)
 526     static uint8_t previous_error_counts[PIOS_I2C_ERROR_COUNT_NUMELEM];
 527
 528     struct pios_i2c_fault_history history;
 529     uint8_t error_counts[PIOS_I2C_ERROR_COUNT_NUMELEM];
 530
 531     PIOS_I2C_GetDiagnostics(&history, error_counts);
 532
 533     // every time a counter changes, set activity timeout counter to ( I2C_ERROR_ACTIVITY_TIMEOUT ).
 534     // every time a counter does not change, decrease activity counter.
 535
 536     for (uint8_t i = 0; i < PIOS_I2C_ERROR_COUNT_NUMELEM; i++) {
 537         if (error_counts[i] != previous_error_counts[i]) {
 538             i2c_error_activity[i] = I2C_ERROR_ACTIVITY_TIMEOUT;
 539         } else if (i2c_error_activity[i] > 0) {
 540             i2c_error_activity[i]--;
 541         }
 542
 543         previous_error_counts[i] = error_counts[i];
 544     }
 545
 546 #ifdef DIAG_I2C_WDG_STATS
 547     I2CStatsData i2cStats;
 548     I2CStatsGet(&i2cStats);
 549
 550     memcpy(&i2cStats.event_errors, &error_counts, sizeof(error_counts));
 551
 552     for (uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) {
 553         i2cStats.evirq_log[i] = history.evirq[i];
 554         i2cStats.erirq_log[i] = history.erirq[i];
 555         i2cStats.event_log[i] = history.event[i];
 556         i2cStats.state_log[i] = history.state[i];
 557     }
 558     i2cStats.last_error_type = history.type;
 559     I2CStatsSet(&i2cStats);
 560 #endif /* DIAG_I2C_WDG_STATS */
 561 #endif /* PIOS_INCLUDE_I2C */
 562 }
 563
 564 #ifdef DIAG_I2C_WDG_STATS
 565 static void updateWDGstats()
 566 {
 567     WatchdogStatusData watchdogStatus;
 568
 569     watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
 570     watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
 571     WatchdogStatusSet(&watchdogStatus);
 572 }
 573 #endif /* ifdef DIAG_I2C_WDG_STATS */
 574
 575 /**
 576  * Called periodically to update the system stats
 577  */
 578 static uint16_t GetFreeIrqStackSize(void)
 579 {
 580     uint32_t i = 0x200;
 581
 582 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
 583     extern uint32_t _irq_stack_top;
 584     extern uint32_t _irq_stack_end;
 585     uint32_t pattern    = 0x0000A5A5;
 586     uint32_t *ptr       = &_irq_stack_end;
 587
 588 #if 1 /* the ugly way accurate but takes more time, useful for debugging */
 589     uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3) / 4;
 590
 591     for (i = 0; i < stack_size; i++) {
 592         if (ptr[i] != pattern) {
 593             i = i * 4;
 594             break;
 595         }
 596     }
 597 #else /* faster way but not accurate */
 598     if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) {
 599         i = IRQSTACK_LIMIT_CRITICAL - 1;
 600     } else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) {
 601         i = IRQSTACK_LIMIT_WARNING - 1;
 602     } else {
 603         i = IRQSTACK_LIMIT_WARNING;
 604     }
 605 #endif
 606 #endif /* if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) */
 607     return i;
 608 }
 609
 610 /**
 611  * Called periodically to update the system stats
 612  */
 613 static void updateStats()
 614 {
 615     SystemStatsData stats;
 616
 617     // Get stats and update
 618     SystemStatsGet(&stats);
 619     stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
 620 #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
 621     // POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
 622     stats.SystemModStackRemaining = 128;
 623     stats.HeapRemaining = 10240;
 624 #else
 625     stats.HeapRemaining = xPortGetFreeHeapSize();
 626     stats.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4;
 627 #endif
 628
 629     // Get Irq stack status
 630     stats.IRQStackRemaining = GetFreeIrqStackSize();
 631
 632 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
 633     static struct PIOS_FLASHFS_Stats fsStats;
 634
 635     if (pios_uavo_settings_fs_id) {
 636         PIOS_FLASHFS_GetStats(pios_uavo_settings_fs_id, &fsStats);
 637         stats.SysSlotsFree   = fsStats.num_free_slots;
 638         stats.SysSlotsActive = fsStats.num_active_slots;
 639     }
 640     if (pios_user_fs_id) {
 641         PIOS_FLASHFS_GetStats(pios_user_fs_id, &fsStats);
 642         stats.UsrSlotsFree   = fsStats.num_free_slots;
 643         stats.UsrSlotsActive = fsStats.num_active_slots;
 644     }
 645 #endif
 646     stats.CPUIdleTicks     = PIOS_TASK_MONITOR_GetIdleTicksCount();
 647     stats.CPUZeroLoadTicks = PIOS_TASK_MONITOR_GetZeroLoadTicksCount();
 648     stats.CPULoad = 100 - (uint8_t)((100 * stats.CPUIdleTicks) / stats.CPUZeroLoadTicks);
 649
 650 #if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
 651     float temp_voltage = PIOS_ADC_PinGetVolt(PIOS_ADC_TEMPERATURE_PIN);
 652     stats.CPUTemp = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
 653 #endif
 654     SystemStatsSet(&stats);
 655 }
 656
 657 /**
 658  * Update system alarms
 659  */
 660 static void updateSystemAlarms()
 661 {
 662     SystemStatsData stats;
 663     UAVObjStats objStats;
 664     EventStats evStats;
 665
 666     SystemStatsGet(&stats);
 667
 668     // Check heap, IRQ stack and malloc failures
 669     if (mallocFailed || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
 670 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
 671         || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
 672 #endif
 673         ) {
 674         AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
 675     } else if ((stats.HeapRemaining < HEAP_LIMIT_WARNING)
 676 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
 677                || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
 678 #endif
 679                ) {
 680         AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
 681     } else {
 682         AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
 683     }
 684
 685     // Check CPU load
 686     if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
 687         AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
 688     } else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) {
 689         AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
 690     } else {
 691         AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
 692     }
 693
 694     // Check for stack overflow
 695     switch (stackOverflow) {
 696     case STACKOVERFLOW_NONE:
 697         AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
 698         break;
 699     case STACKOVERFLOW_WARNING:
 700         AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_WARNING);
 701         break;
 702     default:
 703         AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
 704     }
 705
 706     // Check for event errors
 707     UAVObjGetStats(&objStats);
 708     EventGetStats(&evStats);
 709     UAVObjClearStats();
 710     EventClearStats();
 711     if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
 712         AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
 713     } else {
 714         AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
 715     }
 716
 717     if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
 718         SystemStatsData sysStats;
 719         SystemStatsGet(&sysStats);
 720         sysStats.EventSystemWarningID    = evStats.lastErrorID;
 721         sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
 722         sysStats.ObjectManagerQueueID    = objStats.lastQueueErrorID;
 723         SystemStatsSet(&sysStats);
 724     }
 725
 726 #ifdef PIOS_INCLUDE_I2C
 727     if (AlarmsGet(SYSTEMALARMS_ALARM_I2C) != SYSTEMALARMS_ALARM_UNINITIALISED) {
 728         static const SystemAlarmsAlarmOptions i2c_alarm_by_error[] = {
 729             [PIOS_I2C_BAD_EVENT_COUNTER] = SYSTEMALARMS_ALARM_ERROR,
 730             [PIOS_I2C_FSM_FAULT_COUNT]   = SYSTEMALARMS_ALARM_ERROR,
 731             [PIOS_I2C_ERROR_INTERRUPT_COUNTER] = SYSTEMALARMS_ALARM_ERROR,
 732             [PIOS_I2C_NACK_COUNTER] = SYSTEMALARMS_ALARM_CRITICAL,
 733             [PIOS_I2C_TIMEOUT_COUNTER]   = SYSTEMALARMS_ALARM_ERROR,
 734         };
 735
 736         SystemAlarmsAlarmOptions i2c_alarm = SYSTEMALARMS_ALARM_OK;
 737
 738         for (uint8_t i = 0; i < PIOS_I2C_ERROR_COUNT_NUMELEM; i++) {
 739             if ((i2c_error_activity[i] > 0) && (i2c_alarm < i2c_alarm_by_error[i])) {
 740                 i2c_alarm = i2c_alarm_by_error[i];
 741             }
 742         }
 743
 744         AlarmsSet(SYSTEMALARMS_ALARM_I2C, i2c_alarm);
 745     }
 746 #endif /* PIOS_INCLUDE_I2C */
 747 }
 748
 749 /**
 750  * Called by the RTOS when the CPU is idle,
 751  */
 752 void vApplicationIdleHook(void)
 753 {
 754     PIOS_TASK_MONITOR_IdleHook();
 755     NotificationOnboardLedsRun();
 756 #ifdef PIOS_INCLUDE_WS2811
 757     LedNotificationExtLedsRun();
 758 #endif
 759 }
 760 /**
 761  * Called by the RTOS when a stack overflow is detected.
 762  */
 763 #define DEBUG_STACK_OVERFLOW 0
 764 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
 765                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 766 {
 767 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
 768     stackOverflow = STACKOVERFLOW_CRITICAL;
 769 #if DEBUG_STACK_OVERFLOW
 770     static volatile bool wait_here = true;
 771     while (wait_here) {
 772         ;
 773     }
 774     wait_here = true;
 775 #endif
 776 #endif
 777 }
 778
 779 /**
 780  * Called by the RTOS when a malloc call fails.
 781  */
 782 #define DEBUG_MALLOC_FAILURES 0
 783 void vApplicationMallocFailedHook(void)
 784 {
 785     mallocFailed = true;
 786 #if DEBUG_MALLOC_FAILURES
 787     static volatile bool wait_here = true;
 788     while (wait_here) {
 789         ;
 790     }
 791     wait_here = true;
 792 #endif
 793 }
 794
 795 /**
 796  * @}
 797  * @}
 798  */