mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-13 20:48:42 +01:00
796 lines
27 KiB
C
796 lines
27 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup LibrePilotModules LibrePilot Modules
|
|
* @brief The LibrePilot Modules do the majority of the control in LibrePilot. The
|
|
* @ref SystemModule "System Module" starts all the other modules that then take care
|
|
* of all the telemetry and control algorithms and such. This is done through the @ref PIOS
|
|
* "PIOS Hardware abstraction layer" which then contains hardware specific implementations
|
|
* (currently only STM32 supported)
|
|
*
|
|
* @{
|
|
* @addtogroup SystemModule System Module
|
|
* @brief Initializes PIOS and other modules runs monitoring
|
|
* After initializing all the modules (currently selected by Makefile but in
|
|
* future controlled by configuration on SD card) runs basic monitoring and
|
|
* alarms.
|
|
* @{
|
|
*
|
|
* @file systemmod.c
|
|
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
|
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015.
|
|
* @brief System module
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include <openpilot.h>
|
|
// private includes
|
|
#include "inc/systemmod.h"
|
|
|
|
#include <notification.h>
|
|
#ifdef PIOS_INCLUDE_WS2811
|
|
#include <lednotification.h>
|
|
#endif
|
|
|
|
// UAVOs
|
|
#include <objectpersistence.h>
|
|
#include <flightstatus.h>
|
|
#include <systemstats.h>
|
|
#include <systemsettings.h>
|
|
#include <i2cstats.h>
|
|
#include <taskinfo.h>
|
|
#include <watchdogstatus.h>
|
|
#include <callbackinfo.h>
|
|
#include <hwsettings.h>
|
|
#include <pios_flashfs.h>
|
|
#include <pios_notify.h>
|
|
#include <pios_task_monitor.h>
|
|
#include <pios_board_init.h>
|
|
|
|
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
#include <instrumentation.h>
|
|
#include <pios_instrumentation.h>
|
|
#endif
|
|
|
|
#if defined(PIOS_INCLUDE_RFM22B)
|
|
#include <oplinkstatus.h>
|
|
#endif
|
|
|
|
// Flight Libraries
|
|
#include <sanitycheck.h>
|
|
|
|
|
|
// #define DEBUG_THIS_FILE
|
|
|
|
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(DEBUG_THIS_FILE)
|
|
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, format,##__VA_ARGS__)
|
|
#else
|
|
#define DEBUG_MSG(format, ...)
|
|
#endif
|
|
|
|
// Private constants
|
|
#define SYSTEM_UPDATE_PERIOD_MS 250
|
|
|
|
#if defined(PIOS_SYSTEM_STACK_SIZE)
|
|
#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
|
|
#else
|
|
#define STACK_SIZE_BYTES 1024
|
|
#endif
|
|
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
|
|
|
|
// Private types
|
|
|
|
// Private variables
|
|
static xTaskHandle systemTaskHandle;
|
|
static xQueueHandle objectPersistenceQueue;
|
|
static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
|
|
static bool mallocFailed;
|
|
static HwSettingsData bootHwSettings;
|
|
static FrameType_t bootFrameType;
|
|
|
|
volatile int initTaskDone = 0;
|
|
|
|
// Private functions
|
|
static void objectUpdatedCb(UAVObjEvent *ev);
|
|
static void checkSettingsUpdatedCb(UAVObjEvent *ev);
|
|
#ifdef DIAG_TASKS
|
|
static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context);
|
|
static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context);
|
|
#endif
|
|
static void updateStats();
|
|
static void updateSystemAlarms();
|
|
static void systemTask(void *parameters);
|
|
static void updateI2Cstats();
|
|
#ifdef DIAG_I2C_WDG_STATS
|
|
static void updateWDGstats();
|
|
#endif
|
|
|
|
#ifdef PIOS_INCLUDE_I2C
|
|
#define I2C_ERROR_ACTIVITY_TIMEOUT_SECONDS 2
|
|
#define I2C_ERROR_ACTIVITY_TIMEOUT (I2C_ERROR_ACTIVITY_TIMEOUT_SECONDS * 1000 / SYSTEM_UPDATE_PERIOD_MS)
|
|
static uint8_t i2c_error_activity[PIOS_I2C_ERROR_COUNT_NUMELEM];
|
|
#endif
|
|
|
|
#ifdef PIOS_INCLUDE_RFM22B
|
|
static uint8_t previousRFXtalCap;
|
|
static void oplinkSettingsUpdatedCb(UAVObjEvent *ev);
|
|
#endif
|
|
|
|
extern uintptr_t pios_uavo_settings_fs_id;
|
|
extern uintptr_t pios_user_fs_id;
|
|
|
|
/**
|
|
* Create the module task.
|
|
* \returns 0 on success or -1 if initialization failed
|
|
*/
|
|
int32_t SystemModStart(void)
|
|
{
|
|
// Initialize vars
|
|
stackOverflow = STACKOVERFLOW_NONE;
|
|
mallocFailed = false;
|
|
// Create system task
|
|
xTaskCreate(systemTask, "System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Initialize the module, called on startup.
|
|
* \returns 0 on success or -1 if initialization failed
|
|
*/
|
|
int32_t SystemModInitialize(void)
|
|
{
|
|
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
|
|
SystemSettingsInitialize();
|
|
SystemStatsInitialize();
|
|
FlightStatusInitialize();
|
|
ObjectPersistenceInitialize();
|
|
#ifdef DIAG_TASKS
|
|
TaskInfoInitialize();
|
|
CallbackInfoInitialize();
|
|
#endif
|
|
#ifdef DIAG_I2C_WDG_STATS
|
|
I2CStatsInitialize();
|
|
WatchdogStatusInitialize();
|
|
#endif
|
|
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
InstrumentationInit();
|
|
#endif
|
|
|
|
objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
|
|
if (objectPersistenceQueue == NULL) {
|
|
return -1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
MODULE_INITCALL(SystemModInitialize, 0);
|
|
/**
|
|
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
|
|
*/
|
|
static void systemTask(__attribute__((unused)) void *parameters)
|
|
{
|
|
/* calibrate the cpu usage monitor */
|
|
PIOS_TASK_MONITOR_CalibrateIdleCounter();
|
|
/* board driver init */
|
|
PIOS_Board_Init();
|
|
|
|
/* Initialize all modules */
|
|
MODULE_INITIALISE_ALL;
|
|
|
|
while (!initTaskDone) {
|
|
vTaskDelay(10);
|
|
}
|
|
|
|
#ifndef PIOS_INCLUDE_WDG
|
|
// if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop
|
|
#define PIOS_WDG_Clear()
|
|
#endif
|
|
/* create all modules thread */
|
|
MODULE_TASKCREATE_ALL;
|
|
|
|
/* start the delayed callback scheduler */
|
|
PIOS_CALLBACKSCHEDULER_Start();
|
|
|
|
// Register task
|
|
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
|
|
|
|
if (mallocFailed) {
|
|
/* We failed to malloc during task creation,
|
|
* system behaviour is undefined. Reset and let
|
|
* the BootFault code recover for us.
|
|
*/
|
|
PIOS_SYS_Reset();
|
|
}
|
|
#if defined(PIOS_INCLUDE_IAP)
|
|
/* Record a successful boot */
|
|
PIOS_IAP_WriteBootCount(0);
|
|
#endif
|
|
// Listen for SettingPersistance object updates, connect a callback function
|
|
ObjectPersistenceConnectQueue(objectPersistenceQueue);
|
|
|
|
// Load a copy of HwSetting active at boot time
|
|
HwSettingsGet(&bootHwSettings);
|
|
bootFrameType = GetCurrentFrameType();
|
|
// Whenever the configuration changes, make sure it is safe to fly
|
|
HwSettingsConnectCallback(checkSettingsUpdatedCb);
|
|
SystemSettingsConnectCallback(checkSettingsUpdatedCb);
|
|
|
|
#ifdef PIOS_INCLUDE_RFM22B
|
|
// Initialize previousRFXtalCap used by callback
|
|
OPLinkSettingsRFXtalCapGet(&previousRFXtalCap);
|
|
OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb);
|
|
#endif
|
|
|
|
#ifdef DIAG_TASKS
|
|
TaskInfoData taskInfoData;
|
|
memset(&taskInfoData, 0, sizeof(TaskInfoData));
|
|
CallbackInfoData callbackInfoData;
|
|
memset(&callbackInfoData, 0, sizeof(CallbackInfoData));
|
|
#endif
|
|
// Main system loop
|
|
while (1) {
|
|
NotificationUpdateStatus();
|
|
// Update the system statistics
|
|
updateStats();
|
|
|
|
// Update I2C stats
|
|
updateI2Cstats();
|
|
|
|
// Update the system alarms
|
|
updateSystemAlarms();
|
|
#ifdef DIAG_I2C_WDG_STATS
|
|
updateWDGstats();
|
|
#endif
|
|
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
InstrumentationPublishAllCounters();
|
|
#endif
|
|
|
|
#ifdef DIAG_TASKS
|
|
// Update the task status object
|
|
PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);
|
|
TaskInfoSet(&taskInfoData);
|
|
// Update the callback status object
|
|
PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData);
|
|
CallbackInfoSet(&callbackInfoData);
|
|
#endif
|
|
|
|
UAVObjEvent ev;
|
|
int delayTime = SYSTEM_UPDATE_PERIOD_MS;
|
|
|
|
#if defined(PIOS_INCLUDE_RFM22B)
|
|
|
|
// Update the OPLinkStatus UAVO
|
|
OPLinkStatusData oplinkStatus;
|
|
OPLinkStatusGet(&oplinkStatus);
|
|
oplinkStatus.HeapRemaining = xPortGetFreeHeapSize();
|
|
|
|
if (pios_rfm22b_id) {
|
|
// Get the stats from the radio device
|
|
struct rfm22b_stats radio_stats;
|
|
PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats);
|
|
|
|
// Update the OPLInk status
|
|
static bool first_time = true;
|
|
static uint16_t prev_tx_count = 0;
|
|
static uint16_t prev_rx_count = 0;
|
|
static uint16_t prev_tx_seq = 0;
|
|
static uint16_t prev_rx_seq = 0;
|
|
|
|
oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
|
|
oplinkStatus.RxGood = radio_stats.rx_good;
|
|
oplinkStatus.RxCorrected = radio_stats.rx_corrected;
|
|
oplinkStatus.RxErrors = radio_stats.rx_error;
|
|
oplinkStatus.RxMissed = radio_stats.rx_missed;
|
|
oplinkStatus.RxFailure = radio_stats.rx_failure;
|
|
oplinkStatus.TxDropped = radio_stats.tx_dropped;
|
|
oplinkStatus.TxFailure = radio_stats.tx_failure;
|
|
oplinkStatus.Resets = radio_stats.resets;
|
|
oplinkStatus.Timeouts = radio_stats.timeouts;
|
|
oplinkStatus.RSSI = radio_stats.rssi;
|
|
oplinkStatus.LinkQuality = radio_stats.link_quality;
|
|
oplinkStatus.AFCCorrection = radio_stats.afc_correction;
|
|
if (first_time) {
|
|
first_time = false;
|
|
} else {
|
|
uint16_t tx_count = radio_stats.tx_byte_count;
|
|
uint16_t rx_count = radio_stats.rx_byte_count;
|
|
uint16_t tx_packets = radio_stats.tx_seq - prev_tx_seq;
|
|
uint16_t rx_packets = radio_stats.rx_seq - prev_rx_seq;
|
|
uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count);
|
|
uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count);
|
|
oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
|
oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
|
oplinkStatus.TXPacketRate = (uint16_t)((float)(tx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
|
oplinkStatus.RXPacketRate = (uint16_t)((float)(rx_packets * 1000) / SYSTEM_UPDATE_PERIOD_MS);
|
|
prev_tx_count = tx_count;
|
|
prev_rx_count = rx_count;
|
|
prev_tx_seq = radio_stats.tx_seq;
|
|
prev_rx_seq = radio_stats.rx_seq;
|
|
}
|
|
oplinkStatus.TXSeq = radio_stats.tx_seq;
|
|
oplinkStatus.RXSeq = radio_stats.rx_seq;
|
|
|
|
oplinkStatus.LinkState = radio_stats.link_state;
|
|
} else {
|
|
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
|
|
}
|
|
OPLinkStatusSet(&oplinkStatus);
|
|
|
|
#endif /* if defined(PIOS_INCLUDE_RFM22B) */
|
|
|
|
if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
|
|
// If object persistence is updated call the callback
|
|
objectUpdatedCb(&ev);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Function called in response to object updates
|
|
*/
|
|
static void objectUpdatedCb(UAVObjEvent *ev)
|
|
{
|
|
ObjectPersistenceData objper;
|
|
UAVObjHandle obj;
|
|
|
|
// If the object updated was the ObjectPersistence execute requested action
|
|
if (ev->obj == ObjectPersistenceHandle()) {
|
|
// Get object data
|
|
ObjectPersistenceGet(&objper);
|
|
|
|
int retval = 1;
|
|
FlightStatusData flightStatus;
|
|
FlightStatusGet(&flightStatus);
|
|
|
|
// When this is called because of this method don't do anything
|
|
if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR || objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
|
|
return;
|
|
}
|
|
|
|
// Execute action if disarmed
|
|
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
|
|
retval = -1;
|
|
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
|
|
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
|
|
// Get selected object
|
|
obj = UAVObjGetByID(objper.ObjectID);
|
|
if (obj == 0) {
|
|
return;
|
|
}
|
|
// Load selected instance
|
|
retval = UAVObjLoad(obj, objper.InstanceID);
|
|
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
|
|
retval = UAVObjLoadSettings();
|
|
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
|
|
retval = UAVObjLoadMetaobjects();
|
|
}
|
|
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
|
|
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
|
|
// Get selected object
|
|
obj = UAVObjGetByID(objper.ObjectID);
|
|
if (obj == 0) {
|
|
return;
|
|
}
|
|
// Save selected instance
|
|
retval = UAVObjSave(obj, objper.InstanceID);
|
|
|
|
// Not sure why this is needed
|
|
vTaskDelay(10);
|
|
|
|
// Verify saving worked
|
|
if (retval == 0) {
|
|
retval = UAVObjLoad(obj, objper.InstanceID);
|
|
}
|
|
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
|
|
retval = UAVObjSaveSettings();
|
|
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
|
|
retval = UAVObjSaveMetaobjects();
|
|
}
|
|
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
|
|
if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
|
|
// Get selected object
|
|
obj = UAVObjGetByID(objper.ObjectID);
|
|
if (obj == 0) {
|
|
return;
|
|
}
|
|
// Delete selected instance
|
|
retval = UAVObjDelete(obj, objper.InstanceID);
|
|
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
|
|
retval = UAVObjDeleteSettings();
|
|
} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
|
|
retval = UAVObjDeleteMetaobjects();
|
|
}
|
|
} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
|
|
#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
|
|
retval = PIOS_FLASHFS_Format(0);
|
|
#else
|
|
retval = -1;
|
|
#endif
|
|
}
|
|
switch (retval) {
|
|
case 0:
|
|
objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
|
|
ObjectPersistenceSet(&objper);
|
|
break;
|
|
case -1:
|
|
objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
|
|
ObjectPersistenceSet(&objper);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Called whenever hardware settings changed
|
|
*/
|
|
static void checkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|
{
|
|
HwSettingsData currentHwSettings;
|
|
|
|
HwSettingsGet(¤tHwSettings);
|
|
FrameType_t currentFrameType = GetCurrentFrameType();
|
|
// check whether the Hw Configuration has changed from the one used at boot time
|
|
if ((memcmp(&bootHwSettings, ¤tHwSettings, sizeof(HwSettingsData)) != 0) ||
|
|
(currentFrameType != bootFrameType)) {
|
|
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
|
|
}
|
|
}
|
|
|
|
#ifdef PIOS_INCLUDE_RFM22B
|
|
/**
|
|
* Called whenever OPLink settings changed
|
|
*/
|
|
static void oplinkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|
{
|
|
uint8_t currentRFXtalCap;
|
|
|
|
OPLinkSettingsRFXtalCapGet(¤tRFXtalCap);
|
|
|
|
// Check if RFXtalCap value changed
|
|
if (currentRFXtalCap != previousRFXtalCap) {
|
|
PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, currentRFXtalCap);
|
|
PIOS_RFM22B_Reinit(pios_rfm22b_id);
|
|
previousRFXtalCap = currentRFXtalCap;
|
|
}
|
|
}
|
|
#endif /* ifdef PIOS_INCLUDE_RFM22B */
|
|
|
|
#ifdef DIAG_TASKS
|
|
static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context)
|
|
{
|
|
TaskInfoData *taskData = (TaskInfoData *)context;
|
|
|
|
// By convention, there is a direct mapping between task monitor task_id's and members
|
|
// of the TaskInfoXXXXElem enums
|
|
PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
|
|
TaskInfoRunningToArray(taskData->Running)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
|
|
((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining;
|
|
((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage;
|
|
}
|
|
|
|
static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context)
|
|
{
|
|
CallbackInfoData *callbackData = (CallbackInfoData *)context;
|
|
|
|
if (callback_id < 0) {
|
|
return;
|
|
}
|
|
// delayed callback scheduler reports callback stack overflows as remaininng: -1
|
|
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
|
|
if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) {
|
|
stackOverflow = STACKOVERFLOW_WARNING;
|
|
}
|
|
#endif
|
|
// By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members
|
|
// of the CallbackInfoXXXXElem enums
|
|
PIOS_DEBUG_Assert(callback_id < CALLBACKINFO_RUNNING_NUMELEM);
|
|
((uint8_t *)&callbackData->Running)[callback_id] = callback_info->is_running;
|
|
((uint32_t *)&callbackData->RunningTime)[callback_id] = callback_info->running_time_count;
|
|
((int16_t *)&callbackData->StackRemaining)[callback_id] = callback_info->stack_remaining;
|
|
}
|
|
#endif /* ifdef DIAG_TASKS */
|
|
|
|
/**
|
|
* Called periodically (every SYSTEM_UPDATE_PERIOD_MS milliseconds) to update the I2C statistics
|
|
*/
|
|
static void updateI2Cstats()
|
|
{
|
|
#if defined(PIOS_INCLUDE_I2C)
|
|
static uint8_t previous_error_counts[PIOS_I2C_ERROR_COUNT_NUMELEM];
|
|
|
|
struct pios_i2c_fault_history history;
|
|
uint8_t error_counts[PIOS_I2C_ERROR_COUNT_NUMELEM];
|
|
|
|
PIOS_I2C_GetDiagnostics(&history, error_counts);
|
|
|
|
// every time a counter changes, set activity timeout counter to ( I2C_ERROR_ACTIVITY_TIMEOUT ).
|
|
// every time a counter does not change, decrease activity counter.
|
|
|
|
for (uint8_t i = 0; i < PIOS_I2C_ERROR_COUNT_NUMELEM; i++) {
|
|
if (error_counts[i] != previous_error_counts[i]) {
|
|
i2c_error_activity[i] = I2C_ERROR_ACTIVITY_TIMEOUT;
|
|
} else if (i2c_error_activity[i] > 0) {
|
|
i2c_error_activity[i]--;
|
|
}
|
|
|
|
previous_error_counts[i] = error_counts[i];
|
|
}
|
|
|
|
#ifdef DIAG_I2C_WDG_STATS
|
|
I2CStatsData i2cStats;
|
|
I2CStatsGet(&i2cStats);
|
|
|
|
memcpy(&i2cStats.event_errors, &error_counts, sizeof(error_counts));
|
|
|
|
for (uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) {
|
|
i2cStats.evirq_log[i] = history.evirq[i];
|
|
i2cStats.erirq_log[i] = history.erirq[i];
|
|
i2cStats.event_log[i] = history.event[i];
|
|
i2cStats.state_log[i] = history.state[i];
|
|
}
|
|
i2cStats.last_error_type = history.type;
|
|
I2CStatsSet(&i2cStats);
|
|
#endif /* DIAG_I2C_WDG_STATS */
|
|
#endif /* PIOS_INCLUDE_I2C */
|
|
}
|
|
|
|
#ifdef DIAG_I2C_WDG_STATS
|
|
static void updateWDGstats()
|
|
{
|
|
WatchdogStatusData watchdogStatus;
|
|
|
|
watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
|
|
watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
|
|
WatchdogStatusSet(&watchdogStatus);
|
|
}
|
|
#endif /* ifdef DIAG_I2C_WDG_STATS */
|
|
|
|
/**
|
|
* Called periodically to update the system stats
|
|
*/
|
|
static uint16_t GetFreeIrqStackSize(void)
|
|
{
|
|
uint32_t i = 0x200;
|
|
|
|
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
|
|
extern uint32_t _irq_stack_top;
|
|
extern uint32_t _irq_stack_end;
|
|
uint32_t pattern = 0x0000A5A5;
|
|
uint32_t *ptr = &_irq_stack_end;
|
|
|
|
#if 1 /* the ugly way accurate but takes more time, useful for debugging */
|
|
uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3) / 4;
|
|
|
|
for (i = 0; i < stack_size; i++) {
|
|
if (ptr[i] != pattern) {
|
|
i = i * 4;
|
|
break;
|
|
}
|
|
}
|
|
#else /* faster way but not accurate */
|
|
if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) {
|
|
i = IRQSTACK_LIMIT_CRITICAL - 1;
|
|
} else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) {
|
|
i = IRQSTACK_LIMIT_WARNING - 1;
|
|
} else {
|
|
i = IRQSTACK_LIMIT_WARNING;
|
|
}
|
|
#endif
|
|
#endif /* if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) */
|
|
return i;
|
|
}
|
|
|
|
/**
|
|
* Called periodically to update the system stats
|
|
*/
|
|
static void updateStats()
|
|
{
|
|
SystemStatsData stats;
|
|
|
|
// Get stats and update
|
|
SystemStatsGet(&stats);
|
|
stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
|
|
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
|
|
// POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
|
|
stats.SystemModStackRemaining = 128;
|
|
stats.HeapRemaining = 10240;
|
|
#else
|
|
stats.HeapRemaining = xPortGetFreeHeapSize();
|
|
stats.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4;
|
|
#endif
|
|
|
|
// Get Irq stack status
|
|
stats.IRQStackRemaining = GetFreeIrqStackSize();
|
|
|
|
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
|
|
static struct PIOS_FLASHFS_Stats fsStats;
|
|
|
|
if (pios_uavo_settings_fs_id) {
|
|
PIOS_FLASHFS_GetStats(pios_uavo_settings_fs_id, &fsStats);
|
|
stats.SysSlotsFree = fsStats.num_free_slots;
|
|
stats.SysSlotsActive = fsStats.num_active_slots;
|
|
}
|
|
if (pios_user_fs_id) {
|
|
PIOS_FLASHFS_GetStats(pios_user_fs_id, &fsStats);
|
|
stats.UsrSlotsFree = fsStats.num_free_slots;
|
|
stats.UsrSlotsActive = fsStats.num_active_slots;
|
|
}
|
|
#endif
|
|
stats.CPUIdleTicks = PIOS_TASK_MONITOR_GetIdleTicksCount();
|
|
stats.CPUZeroLoadTicks = PIOS_TASK_MONITOR_GetZeroLoadTicksCount();
|
|
stats.CPULoad = 100 - (uint8_t)((100 * stats.CPUIdleTicks) / stats.CPUZeroLoadTicks);
|
|
|
|
#if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
|
|
float temp_voltage = PIOS_ADC_PinGetVolt(PIOS_ADC_TEMPERATURE_PIN);
|
|
stats.CPUTemp = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
|
|
#endif
|
|
SystemStatsSet(&stats);
|
|
}
|
|
|
|
/**
|
|
* Update system alarms
|
|
*/
|
|
static void updateSystemAlarms()
|
|
{
|
|
SystemStatsData stats;
|
|
UAVObjStats objStats;
|
|
EventStats evStats;
|
|
|
|
SystemStatsGet(&stats);
|
|
|
|
// Check heap, IRQ stack and malloc failures
|
|
if (mallocFailed || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
|
|
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
|
|
|| (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
|
|
#endif
|
|
) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
|
|
} else if ((stats.HeapRemaining < HEAP_LIMIT_WARNING)
|
|
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
|
|
|| (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
|
|
#endif
|
|
) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
|
|
} else {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
|
|
}
|
|
|
|
// Check CPU load
|
|
if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
|
|
} else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
|
|
} else {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
|
|
}
|
|
|
|
// Check for stack overflow
|
|
switch (stackOverflow) {
|
|
case STACKOVERFLOW_NONE:
|
|
AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
|
|
break;
|
|
case STACKOVERFLOW_WARNING:
|
|
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_WARNING);
|
|
break;
|
|
default:
|
|
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
|
|
}
|
|
|
|
// Check for event errors
|
|
UAVObjGetStats(&objStats);
|
|
EventGetStats(&evStats);
|
|
UAVObjClearStats();
|
|
EventClearStats();
|
|
if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
|
|
} else {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
|
|
}
|
|
|
|
if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
|
|
SystemStatsData sysStats;
|
|
SystemStatsGet(&sysStats);
|
|
sysStats.EventSystemWarningID = evStats.lastErrorID;
|
|
sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
|
|
sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID;
|
|
SystemStatsSet(&sysStats);
|
|
}
|
|
|
|
#ifdef PIOS_INCLUDE_I2C
|
|
if (AlarmsGet(SYSTEMALARMS_ALARM_I2C) != SYSTEMALARMS_ALARM_UNINITIALISED) {
|
|
static const SystemAlarmsAlarmOptions i2c_alarm_by_error[] = {
|
|
[PIOS_I2C_BAD_EVENT_COUNTER] = SYSTEMALARMS_ALARM_ERROR,
|
|
[PIOS_I2C_FSM_FAULT_COUNT] = SYSTEMALARMS_ALARM_ERROR,
|
|
[PIOS_I2C_ERROR_INTERRUPT_COUNTER] = SYSTEMALARMS_ALARM_ERROR,
|
|
[PIOS_I2C_NACK_COUNTER] = SYSTEMALARMS_ALARM_CRITICAL,
|
|
[PIOS_I2C_TIMEOUT_COUNTER] = SYSTEMALARMS_ALARM_ERROR,
|
|
};
|
|
|
|
SystemAlarmsAlarmOptions i2c_alarm = SYSTEMALARMS_ALARM_OK;
|
|
|
|
for (uint8_t i = 0; i < PIOS_I2C_ERROR_COUNT_NUMELEM; i++) {
|
|
if ((i2c_error_activity[i] > 0) && (i2c_alarm < i2c_alarm_by_error[i])) {
|
|
i2c_alarm = i2c_alarm_by_error[i];
|
|
}
|
|
}
|
|
|
|
AlarmsSet(SYSTEMALARMS_ALARM_I2C, i2c_alarm);
|
|
}
|
|
#endif /* PIOS_INCLUDE_I2C */
|
|
}
|
|
|
|
/**
|
|
* Called by the RTOS when the CPU is idle,
|
|
*/
|
|
void vApplicationIdleHook(void)
|
|
{
|
|
PIOS_TASK_MONITOR_IdleHook();
|
|
NotificationOnboardLedsRun();
|
|
#ifdef PIOS_INCLUDE_WS2811
|
|
LedNotificationExtLedsRun();
|
|
#endif
|
|
}
|
|
/**
|
|
* Called by the RTOS when a stack overflow is detected.
|
|
*/
|
|
#define DEBUG_STACK_OVERFLOW 0
|
|
void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
|
|
__attribute__((unused)) signed portCHAR *pcTaskName)
|
|
{
|
|
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
|
|
stackOverflow = STACKOVERFLOW_CRITICAL;
|
|
#if DEBUG_STACK_OVERFLOW
|
|
static volatile bool wait_here = true;
|
|
while (wait_here) {
|
|
;
|
|
}
|
|
wait_here = true;
|
|
#endif
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
* Called by the RTOS when a malloc call fails.
|
|
*/
|
|
#define DEBUG_MALLOC_FAILURES 0
|
|
void vApplicationMallocFailedHook(void)
|
|
{
|
|
mallocFailed = true;
|
|
#if DEBUG_MALLOC_FAILURES
|
|
static volatile bool wait_here = true;
|
|
while (wait_here) {
|
|
;
|
|
}
|
|
wait_here = true;
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|