mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
proper floats + formatter
This commit is contained in:
sambas
parent
a40f8f5ebd
commit
36d6c01864
@ -64,8 +64,8 @@
|
||||
|
||||
#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.
|
||||
// must be updated if the FreeRTOS or compiler
|
||||
// optimisation options are changed.
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_SYSTEM_STACK_SIZE)
|
||||
@ -102,15 +102,15 @@ static void updateWDGstats();
|
||||
*/
|
||||
int32_t SystemModStart(void)
|
||||
{
|
||||
// Initialize vars
|
||||
stackOverflow = false;
|
||||
mallocFailed = false;
|
||||
// Create system task
|
||||
xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle);
|
||||
// Register task
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
|
||||
// Initialize vars
|
||||
stackOverflow = false;
|
||||
mallocFailed = false;
|
||||
// Create system task
|
||||
xTaskCreate(systemTask, (signed char *) "System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
|
||||
// Register task
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -120,26 +120,26 @@ int32_t SystemModStart(void)
|
||||
int32_t SystemModInitialize(void)
|
||||
{
|
||||
|
||||
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
|
||||
SystemSettingsInitialize();
|
||||
SystemStatsInitialize();
|
||||
FlightStatusInitialize();
|
||||
ObjectPersistenceInitialize();
|
||||
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
|
||||
SystemSettingsInitialize();
|
||||
SystemStatsInitialize();
|
||||
FlightStatusInitialize();
|
||||
ObjectPersistenceInitialize();
|
||||
#ifdef DIAG_TASKS
|
||||
TaskInfoInitialize();
|
||||
TaskInfoInitialize();
|
||||
#endif
|
||||
#ifdef DIAG_I2C_WDG_STATS
|
||||
I2CStatsInitialize();
|
||||
WatchdogStatusInitialize();
|
||||
I2CStatsInitialize();
|
||||
WatchdogStatusInitialize();
|
||||
#endif
|
||||
|
||||
objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
|
||||
if (objectPersistenceQueue == NULL)
|
||||
return -1;
|
||||
objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
|
||||
if (objectPersistenceQueue == NULL)
|
||||
return -1;
|
||||
|
||||
SystemModStart();
|
||||
SystemModStart();
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(SystemModInitialize, 0)
|
||||
@ -148,77 +148,77 @@ MODULE_INITCALL(SystemModInitialize, 0)
|
||||
*/
|
||||
static void systemTask(void *parameters)
|
||||
{
|
||||
/* create all modules thread */
|
||||
MODULE_TASKCREATE_ALL;
|
||||
/* create all modules thread */
|
||||
MODULE_TASKCREATE_ALL;
|
||||
|
||||
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 (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);
|
||||
/* Record a successful boot */
|
||||
PIOS_IAP_WriteBootCount(0);
|
||||
#endif
|
||||
|
||||
// Initialize vars
|
||||
idleCounter = 0;
|
||||
idleCounterClear = 0;
|
||||
// Initialize vars
|
||||
idleCounter = 0;
|
||||
idleCounterClear = 0;
|
||||
|
||||
// Listen for SettingPersistance object updates, connect a callback function
|
||||
ObjectPersistenceConnectQueue(objectPersistenceQueue);
|
||||
// Listen for SettingPersistance object updates, connect a callback function
|
||||
ObjectPersistenceConnectQueue(objectPersistenceQueue);
|
||||
|
||||
// Whenever the configuration changes, make sure it is safe to fly
|
||||
HwSettingsConnectCallback(hwSettingsUpdatedCb);
|
||||
// Whenever the configuration changes, make sure it is safe to fly
|
||||
HwSettingsConnectCallback(hwSettingsUpdatedCb);
|
||||
|
||||
// Main system loop
|
||||
while (1) {
|
||||
// Update the system statistics
|
||||
updateStats();
|
||||
// Main system loop
|
||||
while (1) {
|
||||
// Update the system statistics
|
||||
updateStats();
|
||||
|
||||
// Update the system alarms
|
||||
updateSystemAlarms();
|
||||
// Update the system alarms
|
||||
updateSystemAlarms();
|
||||
#ifdef DIAG_I2C_WDG_STATS
|
||||
updateI2Cstats();
|
||||
updateWDGstats();
|
||||
updateI2Cstats();
|
||||
updateWDGstats();
|
||||
#endif
|
||||
|
||||
#ifdef DIAG_TASKS
|
||||
// Update the task status object
|
||||
TaskMonitorUpdateAll();
|
||||
// Update the task status object
|
||||
TaskMonitorUpdateAll();
|
||||
#endif
|
||||
|
||||
// Flash the heartbeat LED
|
||||
// Flash the heartbeat LED
|
||||
#if defined(PIOS_LED_HEARTBEAT)
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
|
||||
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
|
||||
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
|
||||
#endif /* PIOS_LED_HEARTBEAT */
|
||||
|
||||
// Turn on the error LED if an alarm is set
|
||||
// Turn on the error LED if an alarm is set
|
||||
#if defined (PIOS_LED_ALARM)
|
||||
if (AlarmsHasWarnings()) {
|
||||
PIOS_LED_On(PIOS_LED_ALARM);
|
||||
} else {
|
||||
PIOS_LED_Off(PIOS_LED_ALARM);
|
||||
}
|
||||
if (AlarmsHasWarnings()) {
|
||||
PIOS_LED_On(PIOS_LED_ALARM);
|
||||
} else {
|
||||
PIOS_LED_Off(PIOS_LED_ALARM);
|
||||
}
|
||||
#endif /* PIOS_LED_ALARM */
|
||||
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
UAVObjEvent ev;
|
||||
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
|
||||
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
|
||||
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
|
||||
UAVObjEvent ev;
|
||||
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
|
||||
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
|
||||
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
|
||||
|
||||
if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
|
||||
// If object persistence is updated call the callback
|
||||
objectUpdatedCb(&ev);
|
||||
}
|
||||
}
|
||||
if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
|
||||
// If object persistence is updated call the callback
|
||||
objectUpdatedCb(&ev);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -226,101 +226,94 @@ static void systemTask(void *parameters)
|
||||
*/
|
||||
static void objectUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
ObjectPersistenceData objper;
|
||||
UAVObjHandle obj;
|
||||
ObjectPersistenceData objper;
|
||||
UAVObjHandle obj;
|
||||
|
||||
// If the object updated was the ObjectPersistence execute requested action
|
||||
if (ev->obj == ObjectPersistenceHandle()) {
|
||||
// Get object data
|
||||
ObjectPersistenceGet(&objper);
|
||||
// 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);
|
||||
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;
|
||||
}
|
||||
// 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);
|
||||
// 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);
|
||||
// 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) {
|
||||
retval = -1;
|
||||
// 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) {
|
||||
retval = -1;
|
||||
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
|
||||
retval = PIOS_FLASHFS_Format(0);
|
||||
retval = PIOS_FLASHFS_Format(0);
|
||||
#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;
|
||||
}
|
||||
}
|
||||
}
|
||||
switch (retval) {
|
||||
case 0:
|
||||
objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
|
||||
ObjectPersistenceSet(&objper);
|
||||
break;
|
||||
case -1:
|
||||
objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
|
||||
ObjectPersistenceSet(&objper);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -328,83 +321,82 @@ static void objectUpdatedCb(UAVObjEvent * ev)
|
||||
*/
|
||||
static void hwSettingsUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT,SYSTEMALARMS_ALARM_ERROR);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_ERROR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Called periodically to update the I2C statistics
|
||||
*/
|
||||
#ifdef DIAG_I2C_WDG_STATS
|
||||
static void updateI2Cstats()
|
||||
static void updateI2Cstats()
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_I2C)
|
||||
I2CStatsData i2cStats;
|
||||
I2CStatsGet(&i2cStats);
|
||||
|
||||
struct pios_i2c_fault_history history;
|
||||
PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors);
|
||||
|
||||
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);
|
||||
I2CStatsData i2cStats;
|
||||
I2CStatsGet(&i2cStats);
|
||||
|
||||
struct pios_i2c_fault_history history;
|
||||
PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
static void updateWDGstats()
|
||||
static void updateWDGstats()
|
||||
{
|
||||
WatchdogStatusData watchdogStatus;
|
||||
watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
|
||||
watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
|
||||
WatchdogStatusSet(&watchdogStatus);
|
||||
WatchdogStatusData watchdogStatus;
|
||||
watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
|
||||
watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
|
||||
WatchdogStatusSet(&watchdogStatus);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* Called periodically to update the system stats
|
||||
*/
|
||||
static uint16_t GetFreeIrqStackSize(void)
|
||||
{
|
||||
uint32_t i = 0x200;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
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;
|
||||
}
|
||||
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
|
||||
return i;
|
||||
return i;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -412,50 +404,49 @@ uint32_t *ptr = &_irq_stack_end;
|
||||
*/
|
||||
static void updateStats()
|
||||
{
|
||||
static portTickType lastTickCount = 0;
|
||||
SystemStatsData stats;
|
||||
static portTickType lastTickCount = 0;
|
||||
SystemStatsData stats;
|
||||
|
||||
// Get stats and update
|
||||
SystemStatsGet(&stats);
|
||||
stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
// 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.HeapRemaining = 10240;
|
||||
// POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
|
||||
stats.HeapRemaining = 10240;
|
||||
#else
|
||||
stats.HeapRemaining = xPortGetFreeHeapSize();
|
||||
stats.HeapRemaining = xPortGetFreeHeapSize();
|
||||
#endif
|
||||
|
||||
// Get Irq stack status
|
||||
stats.IRQStackRemaining = GetFreeIrqStackSize();
|
||||
// Get Irq stack status
|
||||
stats.IRQStackRemaining = GetFreeIrqStackSize();
|
||||
|
||||
// When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
|
||||
if (idleCounterClear) {
|
||||
idleCounter = 0;
|
||||
}
|
||||
// When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
|
||||
if (idleCounterClear) {
|
||||
idleCounter = 0;
|
||||
}
|
||||
|
||||
portTickType now = xTaskGetTickCount();
|
||||
if (now > lastTickCount) {
|
||||
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
|
||||
stats.CPULoad =
|
||||
100 - (uint8_t) roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
|
||||
} //else: TickCount has wrapped, do not calc now
|
||||
lastTickCount = now;
|
||||
idleCounterClear = 1;
|
||||
portTickType now = xTaskGetTickCount();
|
||||
if (now > lastTickCount) {
|
||||
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
|
||||
stats.CPULoad = 100 - (uint8_t) roundf(100.0f * ((float) idleCounter / ((float) dT / 1000.0f)) / (float) IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
|
||||
} //else: TickCount has wrapped, do not calc now
|
||||
lastTickCount = now;
|
||||
idleCounterClear = 1;
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
|
||||
#if defined(STM32F4XX)
|
||||
float temp_voltage = 3.3 * PIOS_ADC_PinGet(3) / ((1 << 12) - 1);
|
||||
const float STM32_TEMP_V25 = 0.76; /* V */
|
||||
const float STM32_TEMP_AVG_SLOPE = 2.5; /* mV/C */
|
||||
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;
|
||||
float temp_voltage = 3.3f * PIOS_ADC_PinGet(3) / ((1 << 12) - 1);
|
||||
const float STM32_TEMP_V25 = 0.76f; /* V */
|
||||
const float STM32_TEMP_AVG_SLOPE = 2.5f; /* mV/C */
|
||||
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;
|
||||
#else
|
||||
float temp_voltage = 3.3 * PIOS_ADC_PinGet(0) / ((1 << 12) - 1);
|
||||
const float STM32_TEMP_V25 = 1.43; /* V */
|
||||
const float STM32_TEMP_AVG_SLOPE = 4.3; /* mV/C */
|
||||
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;
|
||||
float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((1 << 12) - 1);
|
||||
const float STM32_TEMP_V25 = 1.43f; /* V */
|
||||
const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */
|
||||
stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;
|
||||
#endif
|
||||
#endif
|
||||
SystemStatsSet(&stats);
|
||||
SystemStatsSet(&stats);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -463,66 +454,64 @@ static void updateStats()
|
||||
*/
|
||||
static void updateSystemAlarms()
|
||||
{
|
||||
SystemStatsData stats;
|
||||
UAVObjStats objStats;
|
||||
EventStats evStats;
|
||||
SystemStatsGet(&stats);
|
||||
SystemStatsData stats;
|
||||
UAVObjStats objStats;
|
||||
EventStats evStats;
|
||||
SystemStatsGet(&stats);
|
||||
|
||||
// Check heap, IRQ stack and malloc failures
|
||||
if ( mallocFailed
|
||||
|| (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
|
||||
// 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)
|
||||
|| (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
|
||||
#endif
|
||||
) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
} else if (
|
||||
(stats.HeapRemaining < HEAP_LIMIT_WARNING)
|
||||
) {
|
||||
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)
|
||||
|| (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
|
||||
#endif
|
||||
) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
|
||||
}
|
||||
) {
|
||||
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 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
|
||||
if (stackOverflow) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
|
||||
}
|
||||
// Check for stack overflow
|
||||
if (stackOverflow) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
@ -530,13 +519,13 @@ static void updateSystemAlarms()
|
||||
*/
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
// Called when the scheduler has no tasks to run
|
||||
if (idleCounterClear == 0) {
|
||||
++idleCounter;
|
||||
} else {
|
||||
idleCounter = 0;
|
||||
idleCounterClear = 0;
|
||||
}
|
||||
// Called when the scheduler has no tasks to run
|
||||
if (idleCounterClear == 0) {
|
||||
++idleCounter;
|
||||
} else {
|
||||
idleCounter = 0;
|
||||
idleCounterClear = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -545,11 +534,11 @@ void vApplicationIdleHook(void)
|
||||
#define DEBUG_STACK_OVERFLOW 0
|
||||
void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
|
||||
{
|
||||
stackOverflow = true;
|
||||
stackOverflow = true;
|
||||
#if DEBUG_STACK_OVERFLOW
|
||||
static volatile bool wait_here = true;
|
||||
while(wait_here);
|
||||
wait_here = true;
|
||||
static volatile bool wait_here = true;
|
||||
while(wait_here);
|
||||
wait_here = true;
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -559,15 +548,15 @@ void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTas
|
||||
#define DEBUG_MALLOC_FAILURES 0
|
||||
void vApplicationMallocFailedHook(void)
|
||||
{
|
||||
mallocFailed = true;
|
||||
mallocFailed = true;
|
||||
#if DEBUG_MALLOC_FAILURES
|
||||
static volatile bool wait_here = true;
|
||||
while(wait_here);
|
||||
wait_here = true;
|
||||
static volatile bool wait_here = true;
|
||||
while(wait_here);
|
||||
wait_here = true;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
Loading…
Reference in New Issue
Block a user