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