systemmod.c

Go to the documentation of this file.

/*
 * 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, see <http://www.gnu.org/licenses/>
 */

#include "openpilot.h"
#include <eventdispatcher.h>
#include <utlist.h>

#include "systemmod.h"
#include "sanitycheck.h"
#include "taskinfo.h"
#include "taskmonitor.h"
#include "pios_thread.h"
#include "pios_mutex.h"
#include "pios_queue.h"
#include "misc_math.h"
#include "morsel.h"

#include "annunciatorsettings.h"
#include "flightstatus.h"
#include "manualcontrolcommand.h"
#include "manualcontrolsettings.h"
#include "objectpersistence.h"
#include "stabilizationsettings.h"
#include "stateestimation.h"
#include "systemsettings.h"
#include "systemstats.h"
#include "watchdogstatus.h"

#ifdef SYSTEMMOD_RGBLED_SUPPORT
#include "rgbledsettings.h"
#include "rgbleds.h"
#endif

#ifdef PIOS_INCLUDE_DAC_ANNUNCIATOR
#include "pios_annuncdac.h"

extern annuncdac_dev_t pios_dac_annunciator_id;
#endif

#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

#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998   // calibrated by running tests/test_cpuload.c
// must be updated if the FreeRTOS or compiler
// optimisation options are changed.
#endif

#if defined(PIOS_SYSTEM_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
#else
#define STACK_SIZE_BYTES 1024
#endif

#define TASK_PRIORITY PIOS_THREAD_PRIO_NORMAL

/* When we're blinking morse code, this works out to 10.6 WPM.  It's also
 * nice and relatively prime to most other rates of things, so we don't get
 * bad beat frequencies. */

#ifdef SYSTEMMOD_RGBLED_SUPPORT
#define SYSTEM_RAPID_UPDATES

#define SYSTEM_UPDATE_PERIOD_MS 29
#define SYSTEM_UPDATE_PERIOD_MS4TH (SYSTEM_UPDATE_PERIOD_MS * 4)

#else

#define SYSTEM_UPDATE_PERIOD_MS 117
#define SYSTEM_UPDATE_PERIOD_MS4TH (SYSTEM_UPDATE_PERIOD_MS)

#endif

// Private types

typedef struct {
        UAVObjEvent ev; 
        UAVObjEventCallback cb; 
        struct pios_queue *queue; 
} EventCallbackInfo;

struct PeriodicObjectListStruct {
        EventCallbackInfo evInfo; 
        uint16_t updatePeriodMs; 
        int32_t timeToNextUpdateMs; 
        struct PeriodicObjectListStruct* next; 
};
typedef struct PeriodicObjectListStruct PeriodicObjectList;

// Private types

// Private variables
static PeriodicObjectList* objList;
static struct pios_recursive_mutex *mutex;
static EventStats stats;

static volatile uint32_t idleCounter;
static volatile uint32_t idleCounterClear;
static struct pios_thread *systemTaskHandle;
static struct pios_queue *objectPersistenceQueue;

static volatile bool config_check_needed;
static const char * volatile custom_blink_string;

// Private functions
static void systemPeriodicCb(const UAVObjEvent *ev,
                void *ctx, void *obj_data, int len);
static void objectUpdatedCb(const UAVObjEvent *ev,
                void *ctx, void *obj, int len);
static uint32_t processPeriodicUpdates();
static int32_t eventPeriodicCreate(UAVObjEvent *ev,
                UAVObjEventCallback cb, struct pios_queue *queue,
                uint16_t periodMs);
static int32_t eventPeriodicUpdate(UAVObjEvent *ev,
                UAVObjEventCallback cb, struct pios_queue *queue,
                uint16_t periodMs);

#ifndef NO_SENSORS
static void configurationUpdatedCb(const UAVObjEvent *ev,
                void *ctx, void *obj, int len);
#endif

void system_task();

static inline void updateStats();
static inline void updateSystemAlarms();
#if defined(WDG_STATS_DIAGNOSTICS)
static inline void updateWDGstats();
#endif

int32_t SystemModStart(void)
{
        // Create periodic event that will be used to update the telemetry stats
        UAVObjEvent ev;
        memset(&ev, 0, sizeof(UAVObjEvent));
        EventPeriodicCallbackCreate(&ev, systemPeriodicCb, SYSTEM_UPDATE_PERIOD_MS);

        EventClearStats();

        return 0;
}

int32_t SystemModInitialize(void)
{
        // Create mutex
        mutex = PIOS_Recursive_Mutex_Create();
        if (mutex == NULL)
                return -1;

        if (SystemSettingsInitialize() == -1
                        || SystemStatsInitialize() == -1
                        || FlightStatusInitialize() == -1
                        || ObjectPersistenceInitialize() == -1
                        || AnnunciatorSettingsInitialize() == -1
#ifdef SYSTEMMOD_RGBLED_SUPPORT
                        || RGBLEDSettingsInitialize() == -1
#endif
           )    {
                return -1;
        }
#if defined(DIAG_TASKS)
        if (TaskInfoInitialize() == -1)
                return -1;
#endif
#if defined(WDG_STATS_DIAGNOSTICS)
        if (WatchdogStatusInitialize() == -1)
                return -1;
#endif

        objectPersistenceQueue = PIOS_Queue_Create(1, sizeof(UAVObjEvent));
        if (objectPersistenceQueue == NULL)
                return -1;

        return 0;
}

MODULE_HIPRI_INITCALL(SystemModInitialize, SystemModStart)
void system_task()
{
        if (PIOS_heap_malloc_failed_p()) {
                /* 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

        systemTaskHandle = PIOS_Thread_WrapCurrentThread("system");
        PIOS_Thread_ChangePriority(TASK_PRIORITY);

        TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);

        // Listen for SettingPersistance object updates, connect a callback function
        ObjectPersistenceConnectQueue(objectPersistenceQueue);

#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_SYSTEM)
        PIOS_WDG_RegisterFlag(PIOS_WDG_SYSTEM);
#endif

#ifndef NO_SENSORS
        // Run this initially to make sure the configuration is checked
        configuration_check();

        // Whenever the configuration changes, make sure it is safe to fly
        if (StabilizationSettingsHandle())
                StabilizationSettingsConnectCallback(configurationUpdatedCb);
        if (SystemSettingsHandle())
                SystemSettingsConnectCallback(configurationUpdatedCb);
        if (ManualControlSettingsHandle())
                ManualControlSettingsConnectCallback(configurationUpdatedCb);
        if (FlightStatusHandle())
                FlightStatusConnectCallback(configurationUpdatedCb);
#endif

        // Main system loop
        while (1) {
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_SYSTEM)
                PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM);
#endif

                int32_t delayTime = processPeriodicUpdates();

                UAVObjEvent ev;

                if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) {
                        // If object persistence is updated call the callback
                        objectUpdatedCb(&ev, NULL, NULL, 0);
                }
        }
}

#if defined(PIOS_INCLUDE_ANNUNC)

#define BLINK_STRING_RADIO "r "

static inline uint8_t indicate_error(const char **sequence)
{
#ifdef PIPXTREME
        *sequence="t";  /* Dashes */
        return SYSTEMALARMS_ALARM_WARNING;
#else

        SystemAlarmsData alarms;
        SystemAlarmsGet(&alarms);

        *sequence = NULL;

        uint8_t worst_sev = 0;

        bool generic = false;

        for (uint32_t i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
                // If this is less severe than the current alarm, continue.
                if (alarms.Alarm[i] < worst_sev) {
                        continue;
                }

                // If there's a tie and the previous answer is not the
                // 'generic' alarm, continue.
                if ((!generic) && (alarms.Alarm[i] == worst_sev)) {
                        continue;
                }

                uint8_t thresh = SYSTEMALARMS_ALARM_WARNING;

                if (i == SYSTEMALARMS_ALARM_TELEMETRY) {
                        // Suppress most alarms from telemetry.
                        // The user can identify if present from GCS.
                        thresh = SYSTEMALARMS_ALARM_CRITICAL;
                }

                if (i == SYSTEMALARMS_ALARM_EVENTSYSTEM) {
                        // Skip event system alarms-- nuisance
                        continue;
                }

                if (alarms.Alarm[i] < thresh) {
                        continue;
                }

                // OK, we have a candidate alarm.

                worst_sev = alarms.Alarm[i];
                generic = false;

                switch (i) {
                        case SYSTEMALARMS_ALARM_BATTERY:
                                // -...    b for battery
                                *sequence = "b ";
                                break;
                        case SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION:
                                // -.-.    c for configuration
                                *sequence = "c ";
                                break;
                        case SYSTEMALARMS_ALARM_GPS:
                                // --.     g for GPS
                                *sequence = "g ";
                                break;
                        case SYSTEMALARMS_ALARM_MANUALCONTROL:
                                // .-.     r for radio
                                *sequence = BLINK_STRING_RADIO;
                                break;
                        default:
                                // .-      a for alarm
                                *sequence = "a ";

                                generic = true;
                                break;
                }
        }

        return worst_sev;
#endif /* PIPXTREME */
}
#endif

DONT_BUILD_IF(ANNUNCIATORSETTINGS_ANNUNCIATEAFTERARMING_NUMELEM !=
        ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_NUMELEM,
        AnnuncSettingsMismatch1);
DONT_BUILD_IF(ANNUNCIATORSETTINGS_ANNUNCIATEAFTERARMING_MAXOPTVAL !=
        ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_MAXOPTVAL,
        AnnuncSettingsMismatch2);

#if defined(PIOS_INCLUDE_ANNUNC)
static inline bool should_annunc(AnnunciatorSettingsData *annunciatorSettings,
                bool been_armed, bool is_manual_control, uint8_t blink_prio,
                uint8_t cfg_field) {
        PIOS_Assert(cfg_field <
                        ANNUNCIATORSETTINGS_ANNUNCIATEAFTERARMING_NUMELEM);

        if (been_armed) {
                if (blink_prio >= annunciatorSettings->AnnunciateAfterArming[cfg_field]) {
                        return true;
                } else if (is_manual_control && annunciatorSettings->AnnunciateAfterArming[cfg_field] == ANNUNCIATORSETTINGS_ANNUNCIATEAFTERARMING_MANUALCONTROLONLY) {
                        return true;
                }
        }

        if (blink_prio >= annunciatorSettings->AnnunciateAnytime[cfg_field]) {
                return true;
        } else if (is_manual_control && annunciatorSettings->AnnunciateAnytime[cfg_field] == ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_MANUALCONTROLONLY) {
                return true;
        }

        return false;
}

static inline void consider_annunc(AnnunciatorSettingsData *annunciatorSettings,
                bool is_active, bool been_armed, bool is_manual_control,
                uint8_t blink_prio, uint32_t annunc_id,
                uint8_t cfg_field) {
        if (
                        is_active &&
                        should_annunc(annunciatorSettings, been_armed,
                                is_manual_control, blink_prio, cfg_field)
           ) {
                PIOS_ANNUNC_On(annunc_id);
        } else {
                PIOS_ANNUNC_Off(annunc_id);
        }
}
#endif

void system_annunc_custom_string(const char *string)
{
        custom_blink_string = string;
}

static void systemPeriodicCb(const UAVObjEvent *ev,
                void *ctx, void *obj_data, int len)
{
        (void) ev; (void) ctx; (void) obj_data; (void) len;

        static unsigned int counter=0;

        counter++;

#ifndef NO_SENSORS
        if (config_check_needed) {
                configuration_check();
                config_check_needed = false;
        }
#endif

#ifdef SYSTEM_RAPID_UPDATES
        bool fourth = (counter & 3) == 0;
#else
        bool fourth = true;     // All of this stuff, do each time
#endif

        if (fourth) {
                // Update the system statistics
                updateStats();

#ifndef PIPXTREME
                // Update the system alarms
                updateSystemAlarms();
#endif /* PIPXTREME */

#if defined(WDG_STATS_DIAGNOSTICS)
                updateWDGstats();
#endif

#if defined(DIAG_TASKS)
                // Update the task status object
                TaskMonitorUpdateAll();
#endif
        }

#if defined(PIOS_INCLUDE_ANNUNC)
        // Figure out what we should be doing.

        static const char *blink_string = NULL;
        static uint32_t blink_state = 0;
        static uint8_t blink_prio = 0;
        static bool ever_armed = false;
        static uint8_t armed_status = FLIGHTSTATUS_ARMED_DISARMED;
        static bool is_manual_control = false;
        static int morse;

#ifdef SYSTEMMOD_RGBLED_SUPPORT
        static bool led_override = false;
#endif

        // Evaluate all our possible annunciator sources.
        // The most important: indicate_error / alarms

        if (fourth) {
                const char *candidate_blink;
                uint8_t candidate_prio;

                if (custom_blink_string) {
                        candidate_prio = ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_HAIRONFIRE;

                        candidate_blink = custom_blink_string;
                } else {
                        candidate_prio = indicate_error(&candidate_blink);
                }

                if (candidate_prio > blink_prio) {
                        // Preempt!
                        blink_state = 0;
                        blink_string = candidate_blink;
                        blink_prio = candidate_prio;

                        if (blink_prio == ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_HAIRONFIRE) {
                                custom_blink_string = NULL;
                        }

                        if (blink_string &&
                                        !strcmp(blink_string, BLINK_STRING_RADIO)) {
                                /* XXX do this in a more robust way */
                                is_manual_control = true;
                        } else {
                                is_manual_control = false;
                        }
                }

                FlightStatusArmedGet(&armed_status);

                if (armed_status == FLIGHTSTATUS_ARMED_ARMED) {
                        ever_armed = true;
                } else {
                        if ((counter & 15) == 0) {
                                /* Every 16 cycles through here, kick off
                                 * a config check next cycle when disarmed.
                                 * Works around #1776 where tasks race
                                 * through startup... but it also just
                                 * seems prudent to check this stuff
                                 * every half second or so while disarmed.
                                 * (for lost events, etc.)
                                 */
                                config_check_needed = true;
                        }
                }

                if ((blink_prio == 0) && (blink_state == 0)) {
                        // Nothing else to do-- show armed status
                        if (armed_status == FLIGHTSTATUS_ARMED_ARMED) {
                                blink_string = "I";     // .. pairs of blinks.
                        } else {
                                blink_string = "T";     // - single long blinks
                        }

                        blink_prio = SHAREDDEFS_ALARMLEVELS_OK;
                }

                morse = morse_send(&blink_string, &blink_state);

                AnnunciatorSettingsData annunciatorSettings;
                AnnunciatorSettingsGet(&annunciatorSettings);

#ifdef PIOS_LED_HEARTBEAT
                consider_annunc(&annunciatorSettings, morse > 0, ever_armed,
                                is_manual_control,
                                blink_prio, PIOS_LED_HEARTBEAT,
                                ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_LED_HEARTBEAT);
#endif

#ifdef PIOS_LED_ALARM
                consider_annunc(&annunciatorSettings, morse > 0, ever_armed,
                                is_manual_control,
                                blink_prio, PIOS_LED_ALARM,
                                ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_LED_ALARM);
#endif

                uint8_t buzzer_prio = blink_prio;

#ifndef PIPXTREME
                if (annunciatorSettings.ManualBuzzer !=
                                ANNUNCIATORSETTINGS_MANUALBUZZER_DISABLED) {
                        float acc[MANUALCONTROLCOMMAND_ACCESSORY_NUMELEM];
                        ManualControlCommandAccessoryGet(acc);
                        /* DONT_BUILD_IF to protect this at end of file */
                        if (acc[annunciatorSettings.ManualBuzzer - 1] > 0.0f)
                                buzzer_prio = ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_HAIRONFIRE;
                }
#endif

                (void) buzzer_prio;

#ifdef PIOS_ANNUNCIATOR_BUZZER
                consider_annunc(&annunciatorSettings, morse > 0, ever_armed,
                                is_manual_control,
                                buzzer_prio, PIOS_ANNUNCIATOR_BUZZER,
                                ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_BUZZER);
#endif

#ifdef PIOS_INCLUDE_DAC_ANNUNCIATOR
                if (pios_dac_annunciator_id) {
                        if (should_annunc(&annunciatorSettings, ever_armed,
                                        is_manual_control, buzzer_prio,
                                        ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_DAC)) {
                                PIOS_ANNUNCDAC_SetValue(pios_dac_annunciator_id,
                                                true, morse > 0);
                        } else {
                                PIOS_ANNUNCDAC_SetValue(pios_dac_annunciator_id,
                                                false, false);
                        }
                }
#endif

#ifdef PIOS_INCLUDE_ACTUATOR_ANNUNCIATOR
                if (should_annunc(&annunciatorSettings, ever_armed,
                                is_manual_control, buzzer_prio,
                                ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_ACTUATORS)) {
                        actuator_send_dshot_command(DSHOT_CMD_BEACON3,
                                        1, 0xffff);
                }
#endif

#ifdef SYSTEMMOD_RGBLED_SUPPORT
                led_override = should_annunc(&annunciatorSettings,
                                ever_armed, is_manual_control, blink_prio,
                                ANNUNCIATORSETTINGS_ANNUNCIATEANYTIME_RGB_LEDS);
#endif

                if (morse < 0) {
                        // This means we were told "completed"
                        blink_string = NULL;
                        blink_prio = 0;

                        is_manual_control = false;
                }
        }

#ifdef SYSTEMMOD_RGBLED_SUPPORT
        systemmod_process_rgb_leds(led_override, morse > 0, blink_prio,
                FLIGHTSTATUS_ARMED_DISARMED != armed_status,
                (FLIGHTSTATUS_ARMED_ARMING == armed_status) &&
                ((counter & 3) < 2));
#endif /* SYSTEMMOD_RGBLED_SUPPORT */

#endif  /* PIOS_INCLUDE_ANNUNC */
}


static void objectUpdatedCb(const UAVObjEvent *ev,
                void *ctx, void *obj_data, int len)
{
        (void) ctx; (void) obj_data; (void) len;

        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;

                // When this is called because of this method don't do anything
                if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR ||
                                objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
                        return;
                }

                if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
                        // Get selected object
                        obj = UAVObjGetByID(objper.ObjectID);
                        if (obj == 0) {
                                return;
                        }
                        // Load selected instance
                        retval = UAVObjLoad(obj, objper.InstanceID);
                } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
                        // Get selected object
                        obj = UAVObjGetByID(objper.ObjectID);
                        if (obj == 0) {
                                return;
                        }

                        // Save selected instance
                        retval = UAVObjSave(obj, objper.InstanceID);
                } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
                        // Delete selected instance
                        retval = UAVObjDeleteById(objper.ObjectID, objper.InstanceID);
                } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
                        retval = -1;
#if defined(PIOS_INCLUDE_LOGFS_SETTINGS)
                        extern uintptr_t pios_uavo_settings_fs_id;
                        retval = PIOS_FLASHFS_Format(pios_uavo_settings_fs_id);
#endif
                }

                // Yield when saving, so if there's a ton of updates we don't
                // prevent other threads from updatin'.
                PIOS_Thread_Sleep(25);

                switch(retval) {
                        case 0:
                                objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
                                ObjectPersistenceSet(&objper);
                                break;
                        case -1:
                                objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
                                ObjectPersistenceSet(&objper);
                                break;
                        default:
                                break;
                }
        }
}

#ifndef NO_SENSORS

static void configurationUpdatedCb(const UAVObjEvent *ev,
                void *ctx, void *obj, int len)
{
        (void) ev; (void) ctx; (void) obj; (void) len;
        config_check_needed = true;
}
#endif

#if defined(WDG_STATS_DIAGNOSTICS)
static WatchdogStatusData watchdogStatus;
static void updateWDGstats()
{
        // Only update if something has changed
        if (watchdogStatus.ActiveFlags != PIOS_WDG_GetActiveFlags() ||
                        watchdogStatus.BootupFlags != PIOS_WDG_GetBootupFlags()) {
                watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
                watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
                WatchdogStatusSet(&watchdogStatus);
        }
}
#endif

static void updateStats()
{
        static uint32_t lastTickCount = 0;
        SystemStatsData stats;

        // Get stats and update
        SystemStatsGet(&stats);
        stats.FlightTime = PIOS_Thread_Systime();
        stats.HeapRemaining = PIOS_heap_get_free_size();
        stats.FastHeapRemaining = PIOS_fastheap_get_free_size();

        // Get Irq stack status
        stats.IRQStackRemaining = (uint16_t)PIOS_SYS_IrqStackUnused();
        stats.OSStackRemaining = (uint16_t)PIOS_SYS_OsStackUnused();

        // When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
        if (idleCounterClear) {
                idleCounter = 0;
        }

        uint32_t now = PIOS_Thread_Systime();
        if (now > lastTickCount) {
                float dT = (now - lastTickCount) / 1000.0f;

                // In the case of a slightly miscalibrated max idle count, make sure CPULoad does
                // not go negative and set an alarm inappropriately.
                float idleFraction = ((float)idleCounter / dT) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD;
                if (idleFraction > 1)
                        stats.CPULoad = 0;
                else
                        stats.CPULoad = 100 - roundf(100.0f * idleFraction);
        } //else: TickCount has wrapped, do not calc now

        lastTickCount = now;
        idleCounterClear = 1;

        SystemStatsSet(&stats);
}

static void updateSystemAlarms()
{
#ifndef PIPXTREME
        SystemStatsData stats;
        UAVObjStats objStats;
        EventStats evStats;
        SystemStatsGet(&stats);

        // Check heap, IRQ stack and malloc failures
        if (PIOS_heap_malloc_failed_p()
                        || (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
#ifdef FLIGHT_POSIX
        // XXX do something meaningful here in the future.
        // Right now CPU monitoring on posix is worthless.
        AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
#else
        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);
        }
#endif

        // 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);
        }
#endif
}

void vApplicationIdleHook(void)
{
        // Called when the scheduler has no tasks to run
        if (idleCounterClear == 0) {
                ++idleCounter;
        } else {
                idleCounter = 0;
                idleCounterClear = 0;
        }
}

void EventGetStats(EventStats* statsOut)
{
        PIOS_Recursive_Mutex_Lock(mutex, PIOS_MUTEX_TIMEOUT_MAX);
        memcpy(statsOut, &stats, sizeof(EventStats));
        PIOS_Recursive_Mutex_Unlock(mutex);
}

void EventClearStats()
{
        PIOS_Recursive_Mutex_Lock(mutex, PIOS_MUTEX_TIMEOUT_MAX);
        memset(&stats, 0, sizeof(EventStats));
        PIOS_Recursive_Mutex_Unlock(mutex);
}

/* XXX TODO: would be nice to get context record logic in these */
int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs)
{
        return eventPeriodicCreate(ev, cb, 0, periodMs);
}

int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, uint16_t periodMs)
{
        return eventPeriodicUpdate(ev, cb, 0, periodMs);
}

int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, struct pios_queue *queue, uint16_t periodMs)
{
        return eventPeriodicCreate(ev, 0, queue, periodMs);
}

int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, struct pios_queue *queue, uint16_t periodMs)
{
        return eventPeriodicUpdate(ev, 0, queue, periodMs);
}

static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, struct pios_queue *queue, uint16_t periodMs)
{
        PeriodicObjectList* objEntry;
        // Get lock
        PIOS_Recursive_Mutex_Lock(mutex, PIOS_MUTEX_TIMEOUT_MAX);
        // Check that the object is not already connected
        LL_FOREACH(objList, objEntry)
        {
                if (objEntry->evInfo.cb == cb &&
                                objEntry->evInfo.queue == queue &&
                                objEntry->evInfo.ev.obj == ev->obj &&
                                objEntry->evInfo.ev.instId == ev->instId &&
                                objEntry->evInfo.ev.event == ev->event)
                {
                        // Already registered, do nothing
                        PIOS_Recursive_Mutex_Unlock(mutex);
                        return -1;
                }
        }
        // Create handle
        objEntry = (PeriodicObjectList*)PIOS_malloc_no_dma(sizeof(PeriodicObjectList));
        if (objEntry == NULL) return -1;
        objEntry->evInfo.ev.obj = ev->obj;
        objEntry->evInfo.ev.instId = ev->instId;
        objEntry->evInfo.ev.event = ev->event;
        objEntry->evInfo.ev.throttle = NULL;
        objEntry->evInfo.cb = cb;
        objEntry->evInfo.queue = queue;
        objEntry->updatePeriodMs = periodMs;
        objEntry->timeToNextUpdateMs = randomize_int(periodMs); // avoid bunching of updates
        // Add to list
        LL_APPEND(objList, objEntry);
        // Release lock
        PIOS_Recursive_Mutex_Unlock(mutex);
        return 0;
}

static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, struct pios_queue *queue, uint16_t periodMs)
{
        PeriodicObjectList* objEntry;
        // Get lock
        PIOS_Recursive_Mutex_Lock(mutex, PIOS_MUTEX_TIMEOUT_MAX);
        // Find object
        LL_FOREACH(objList, objEntry)
        {
                if (objEntry->evInfo.cb == cb &&
                                objEntry->evInfo.queue == queue &&
                                objEntry->evInfo.ev.obj == ev->obj &&
                                objEntry->evInfo.ev.instId == ev->instId &&
                                objEntry->evInfo.ev.event == ev->event)
                {
                        // Object found, update period
                        objEntry->updatePeriodMs = periodMs;
                        objEntry->timeToNextUpdateMs = randomize_int(periodMs); // avoid bunching of updates
                        // Release lock
                        PIOS_Recursive_Mutex_Unlock(mutex);
                        return 0;
                }
        }
        // If this point is reached the object was not found
        PIOS_Recursive_Mutex_Unlock(mutex);
        return -1;
}

/* It can take this long before a "first callback" on a registration,
 * so it's advantageous for it to not be too long.  (e.g. we don't have a
 * mechanism to wakeup on list change */
#define MAX_UPDATE_PERIOD_MS 350

static uint32_t processPeriodicUpdates()
{
        PeriodicObjectList* objEntry;
        int32_t timeNow;
        int32_t offset;

        // Get lock
        PIOS_Recursive_Mutex_Lock(mutex, PIOS_MUTEX_TIMEOUT_MAX);

        // Iterate through each object and update its timer, if zero then transmit object.
        // Also calculate smallest delay to next update.
        uint32_t now = PIOS_Thread_Systime();
        uint32_t timeToNextUpdate = PIOS_Thread_Systime() + MAX_UPDATE_PERIOD_MS;
        LL_FOREACH(objList, objEntry)
        {
                // If object is configured for periodic updates
                if (objEntry->updatePeriodMs > 0)
                {
                        // Check if time for the next update
                        timeNow = PIOS_Thread_Systime();
                        if (objEntry->timeToNextUpdateMs <= timeNow)
                        {
                                // Reset timer
                                offset = ( timeNow - objEntry->timeToNextUpdateMs ) % objEntry->updatePeriodMs;
                                objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset;
                                // Invoke callback, if one
                                if ( objEntry->evInfo.cb != 0)
                                {
                                        objEntry->evInfo.cb(&objEntry->evInfo.ev, NULL, NULL, 0); // the function is expected to copy the event information
                                }
                                // Push event to queue, if one
                                if ( objEntry->evInfo.queue != 0)
                                {
                                        if (PIOS_Queue_Send(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != true ) // do not block if queue is full
                                        {
                                                if (objEntry->evInfo.ev.obj != NULL)
                                                        stats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj);
                                                ++stats.eventErrors;
                                        }
                                }
                        }
                        // Update minimum delay
                        if (objEntry->timeToNextUpdateMs < timeToNextUpdate)
                        {
                                timeToNextUpdate = objEntry->timeToNextUpdateMs;
                        }
                }
        }

        // Done
        PIOS_Recursive_Mutex_Unlock(mutex);
        return timeToNextUpdate - now;
}

DONT_BUILD_IF(ANNUNCIATORSETTINGS_MANUALBUZZER_MAXOPTVAL >
        MANUALCONTROLCOMMAND_ACCESSORY_NUMELEM, TooManyManualBuzzers);