rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'amorale/OP-1365_pios_instrumentation' into next

Browse Source 
Conflicts:
	flight/modules/Attitude/attitude.c
This commit is contained in:
Alessio Morale 2014-06-22 19:13:46 +02:00
commit a03ad41bab
6 changed files with 207 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -61,6 +61,7 @@ ground/openpilotgcs/.settings
/.project
/.metadata
/.settings
/.pydevproject
# Ignore Eclipse temp folder, git plugin based?
RemoteSystemsTempFiles

65
flight/modules/Attitude/attitude.c
View File

@ -65,17 +65,19 @@
#include "CoordinateConversions.h"
#include <pios_notify.h>
#undef PIOS_INCLUDE_INSTRUMENTATION
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
int8_t counterUpd;
int8_t counterAccelSamples = -1;
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterUpd);
PERF_DEFINE_COUNTER(counterAccelSamples);
PERF_DEFINE_COUNTER(counterPeriod);
PERF_DEFINE_COUNTER(counterAtt);
// Counters:
// - 0xA7710001 sensor fetch duration
// - 0xA7710002 updateAttitude execution time
// - 0xA7710003 Attitude loop rate(period)
// - 0xA7710004 number of accel samples read for each loop (cc only).
#endif
// Private constants
#define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
@ -245,16 +247,15 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PIOS_ADC_Config(46);
#endif
}
PERF_INIT_COUNTER(counterUpd, 0xA7710001);
PERF_INIT_COUNTER(counterAtt, 0xA7710002);
PERF_INIT_COUNTER(counterPeriod, 0xA7710003);
PERF_INIT_COUNTER(counterAccelSamples, 0xA7710004);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
#ifdef PIOS_INCLUDE_INSTRUMENTATION
counterUpd = PIOS_Instrumentation_CreateCounter(0xA7710001);
int8_t counterAtt = PIOS_Instrumentation_CreateCounter(0xA7710002);
int8_t counterPeriod = PIOS_Instrumentation_CreateCounter(0xA7710003);
if (!cc3d) {
counterAccelSamples = PIOS_Instrumentation_CreateCounter(0xA7710004);
}
#endif
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
// Main task loop
@ -309,19 +310,13 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
} else {
// Do not update attitude data in simulation mode
if (!AttitudeStateReadOnly()) {
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeStart(counterAtt);
#endif
PERF_TIMED_SECTION_START(counterAtt);
updateAttitude(&accelState, &gyros);
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeEnd(counterAtt);
#endif
PERF_TIMED_SECTION_END(counterAtt);
}
PERF_MEASURE_PERIOD(counterPeriod);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TrackPeriod(counterPeriod);
#endif
}
}
@ -351,9 +346,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
if (fifoSamples == 0) {
return -1;
}
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeStart(counterUpd);
#endif
PERF_TIMED_SECTION_START(counterUpd);
// First sample is temperature
gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.X;
gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Y;
@ -378,9 +371,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
z += -accel_data.z;
} while ((i < 32) && (samples_remaining > 0));
}
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_updateCounter(counterAccelSamples, i);
#endif
PERF_TRACK_VALUE(counterAccelSamples, i);
float accel[3] = { accel_scale.X * (float)x / i,
accel_scale.Y * (float)y / i,
accel_scale.Z * (float)z / i };
@ -401,6 +392,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
accelState->y = accel[1];
accelState->z = accel[2];
}
if (trim_requested) {
if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) {
trim_requested = false;
@ -438,10 +430,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
gyro_correct_int[2] += -gyros->z * yawBiasRate;
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeEnd(counterUpd);
#endif
PERF_TIMED_SECTION_END(counterUpd);
GyroStateSet(gyros);
AccelStateSet(accelState);
@ -470,10 +459,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
if (GyroStateReadOnly() || AccelStateReadOnly()) {
return 0;
}
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeStart(counterUpd);
vPortEnterCritical();
#endif
PERF_TIMED_SECTION_START(counterUpd);
gyros[0] = mpu6000_data.gyro_x * gyro_scale.X;
gyros[1] = mpu6000_data.gyro_y * gyro_scale.Y;
gyros[2] = mpu6000_data.gyro_z * gyro_scale.Z;
@ -537,10 +523,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
gyro_correct_int[2] += -gyrosData->z * yawBiasRate;
#ifdef PIOS_INCLUDE_INSTRUMENTATION
vPortExitCritical();
PIOS_Instrumentation_TimeEnd(counterUpd);
#endif
PERF_TIMED_SECTION_END(counterUpd);
GyroStateSet(gyrosData);
AccelStateSet(accelStateData);

29
flight/pios/common/pios_instrumentation.c
View File

@ -44,25 +44,32 @@ void PIOS_Instrumentation_Init(int8_t maxCounters)
}
}
int8_t PIOS_Instrumentation_CreateCounter(uint32_t id)
pios_counter_t PIOS_Instrumentation_CreateCounter(uint32_t id)
{
PIOS_Assert(pios_instrumentation_perf_counters && (pios_instrumentation_max_counters > pios_instrumentation_last_used_counter));
int8_t idx = ++pios_instrumentation_last_used_counter;
pios_instrumentation_perf_counters[idx].id = id;
pios_instrumentation_perf_counters[idx].max = INT32_MIN;
pios_instrumentation_perf_counters[idx].min = INT32_MAX;
return idx;
pios_counter_t counter_handle = PIOS_Instrumentation_SearchCounter(id);
if (!counter_handle) {
pios_perf_counter_t *newcounter = &pios_instrumentation_perf_counters[++pios_instrumentation_last_used_counter];
newcounter->id = id;
newcounter->max = INT32_MIN;
newcounter->min = INT32_MAX;
counter_handle = (pios_counter_t)newcounter;
}
return counter_handle;
}
int8_t PIOS_Instrumentation_SearchCounter(uint32_t id)
pios_counter_t PIOS_Instrumentation_SearchCounter(uint32_t id)
{
PIOS_Assert(pios_instrumentation_perf_counters);
uint8_t i = 0;
while (i < pios_instrumentation_last_used_counter && pios_instrumentation_perf_counters[i].id != id) {}
if (pios_instrumentation_perf_counters[i].id != id) {
return -1;
while (i < pios_instrumentation_last_used_counter && pios_instrumentation_perf_counters[i].id != id) {
i++;
}
return i;
if (pios_instrumentation_perf_counters[i].id != id) {
return NULL;
}
return (pios_counter_t)&pios_instrumentation_perf_counters[i];
}
void PIOS_Instrumentation_ForEachCounter(InstrumentationCounterCallback callback, void *context)

72
flight/pios/inc/pios_instrumentation.h
View File

@ -36,19 +36,22 @@ typedef struct {
int32_t value;
uint32_t lastUpdateTS;
} pios_perf_counter_t;
typedef void *pios_counter_t;
extern pios_perf_counter_t *pios_instrumentation_perf_counters;
extern int8_t pios_instrumentation_last_used_counter;
/**
* Update a counter with a new value
* @param counterIdx index of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
* @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
* @param newValue the updated value.
*/
inline void PIOS_Instrumentation_updateCounter(int8_t counterIdx, int32_t newValue)
inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, int32_t newValue)
{
PIOS_Assert(pios_instrumentation_perf_counters && (counterIdx <= pios_instrumentation_last_used_counter));
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
vPortEnterCritical();
pios_perf_counter_t *counter = &pios_instrumentation_perf_counters[counterIdx];
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value = newValue;
if (counter->value > counter->max) {
counter->max = counter->value;
@ -62,26 +65,28 @@ inline void PIOS_Instrumentation_updateCounter(int8_t counterIdx, int32_t newVal
/**
* Used to determine the time duration of a code block, mark the begin of the block. @see PIOS_Instrumentation_TimeEnd
* @param counterIdx counterIdx index of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
* @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
*/
inline void PIOS_Instrumentation_TimeStart(int8_t counterIdx)
inline void PIOS_Instrumentation_TimeStart(pios_counter_t counter_handle)
{
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
vPortEnterCritical();
PIOS_Assert(pios_instrumentation_perf_counters && (counterIdx <= pios_instrumentation_last_used_counter));
pios_perf_counter_t *counter = &pios_instrumentation_perf_counters[counterIdx];
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->lastUpdateTS = PIOS_DELAY_GetRaw();
vPortExitCritical();
}
/**
* Used to determine the time duration of a code block, mark the end of the block. @see PIOS_Instrumentation_TimeStart
* @param counterIdx counterIdx index of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
* @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
*/
inline void PIOS_Instrumentation_TimeEnd(int8_t counterIdx)
inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
{
PIOS_Assert(pios_instrumentation_perf_counters && (counterIdx <= pios_instrumentation_last_used_counter));
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
vPortEnterCritical();
pios_perf_counter_t *counter = &pios_instrumentation_perf_counters[counterIdx];
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
if (counter->value > counter->max) {
counter->max = counter->value;
@ -95,23 +100,25 @@ inline void PIOS_Instrumentation_TimeEnd(int8_t counterIdx)
/**
* Used to determine the mean period between each call to the function
* @param counterIdx counterIdx index of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
* @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
*/
inline void PIOS_Instrumentation_TrackPeriod(int8_t counterIdx)
inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
{
vPortEnterCritical();
PIOS_Assert(pios_instrumentation_perf_counters && (counterIdx <= pios_instrumentation_last_used_counter));
pios_perf_counter_t *counter = &pios_instrumentation_perf_counters[counterIdx];
uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
if (counter->lastUpdateTS != 0) {
vPortEnterCritical();
uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
counter->value = (counter->value * 15 + period) / 16;
if ((int32_t)period > counter->max) {
counter->max = period;
}
if ((int32_t)period < counter->min) {
counter->min = period;
}
vPortExitCritical();
}
counter->lastUpdateTS = PIOS_DELAY_GetRaw();
counter->value = (counter->value * 14 + period * 2) / 16;
if (counter->value > counter->max) {
counter->max = counter->value;
}
if (counter->value < counter->min) {
counter->min = counter->value;
}
vPortExitCritical();
}
/**
@ -122,17 +129,18 @@ void PIOS_Instrumentation_Init(int8_t maxCounters);
/**
* Create a new counter.
* @param id the unique id to assig to the counter
* @return the counter index to be used to manage its content
* @param id the unique id to assign to the counter
* @return the counter handle to be used to manage its content
*/
int8_t PIOS_Instrumentation_CreateCounter(uint32_t id);
pios_counter_t PIOS_Instrumentation_CreateCounter(uint32_t id);
/**
* search a counter index by its unique Id
* @param id the unique id to assig to the counter
* @return the counter index to be used to manage its content
* @param id the unique id to assign to the counter.
* If a counter with the same id exists, the previous instance is returned
* @return the counter handle to be used to manage its content
*/
int8_t PIOS_Instrumentation_SearchCounter(uint32_t id);
pios_counter_t PIOS_Instrumentation_SearchCounter(uint32_t id);
typedef void (*InstrumentationCounterCallback)(const pios_perf_counter_t *counter, const int8_t index, void *context);
/**

111
flight/pios/inc/pios_instrumentation_helper.h Normal file
View File

@ -0,0 +1,111 @@
/**
******************************************************************************
*
* @file pios_instrumentation_helper.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Macros to easily add optional performance monitoring to a 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
*/
/**
* @addtogroup PIOS PiOS Instrumentation Support
* @{
*/
/**
* \par
* This is a collections of helper macros that ease adding instrumentation support.
* \par
* Step by step guide:
*
* Define PIOS_INSTRUMENT_MODULE before including this file to enable instrumentation for a module
*
* <pre>#define PIOS_INSTRUMENT_MODULE
* #include <pios_instrumentation_helper.h></pre>
*
* Declare the variables used to hold counter handlers.
* Place the following code along all module variables declaration.
* <pre>PERF_DEFINE_COUNTER(counterUpd);
* PERF_DEFINE_COUNTER(counterAccelSamples);
* PERF_DEFINE_COUNTER(counterPeriod);
* PERF_DEFINE_COUNTER(counterAtt);</pre>
*
* Counters needs to be initialized before they are used.
* The following code needs to be added to a function called at module initialization.
* the second parameter is a unique counter Id.
* A good pracice is to use the upper half word as module id and lower as counter id
* <pre>PERF_INIT_COUNTER(counterUpd, 0xA7710001);
* PERF_INIT_COUNTER(counterAtt, 0xA7710002);
* PERF_INIT_COUNTER(counterPeriod, 0xA7710003);
* PERF_INIT_COUNTER(counterAccelSamples, 0xA7710004);</pre>
*
* At this point you can start using the counters as in the following samples
*
* Track the time spent on a certain function:
* <pre>PERF_TIMED_SECTION_START(counterAtt);
* updateAttitude(&accelState, &gyros);
* PERF_TIMED_SECTION_END(counterAtt);</pre>
* PERF_TIMED_SECTION_[START!STOP] marks the beginning and the end of the code to monitor
*
* Measure the mean of the period a certain point is reached:
* <pre>PERF_MEASURE_PERIOD(counterPeriod);</pre>
* Note that the value stored in the counter is a long running mean while max and min are single point values
*
* Track an user defined int32_t value:
* <pre>PERF_TRACK_VALUE(counterAccelSamples, i);</pre>
* the counter is then updated with the value of i.
*
* \par
*/
#ifndef PIOS_INSTRUMENTATION_HELPER_H
#define PIOS_INSTRUMENTATION_HELPER_H
#if defined(PIOS_INCLUDE_INSTRUMENTATION) && defined(PIOS_INSTRUMENT_MODULE)
#include <pios_instrumentation.h>
/**
* include the following macro together with modules variable declaration
*/
#define PERF_DEFINE_COUNTER(x) pios_counter_t x
/**
* this mast be called at some module init code
*/
#define PERF_INIT_COUNTER(x, id) x = PIOS_Instrumentation_CreateCounter(id)
/**
* those are the monitoring macros
*/
#define PERF_TIMED_SECTION_START(x) PIOS_Instrumentation_TimeStart(x)
#define PERF_TIMED_SECTION_END(x) PIOS_Instrumentation_TimeEnd(x)
#define PERF_MEASURE_PERIOD(x) PIOS_Instrumentation_TrackPeriod(x)
#define PERF_TRACK_VALUE(x, y) PIOS_Instrumentation_updateCounter(x, y)
#else
#define PERF_DEFINE_COUNTER(x)
#define PERF_INIT_COUNTER(x, id)
#define PERF_TIMED_SECTION_START(x)
#define PERF_TIMED_SECTION_END(x)
#define PERF_MEASURE_PERIOD(x)
#define PERF_TRACK_VALUE(x, y)
#endif /* PIOS_INCLUDE_INSTRUMENTATION */
#endif /* PIOS_INSTRUMENTATION_HELPER_H */

22
make/common-defs.mk
View File

@ -21,23 +21,24 @@ ifndef OPENPILOT_IS_COOL
endif
# Set to YES to compile for debugging
DEBUG ?= NO
DEBUG ?= NO
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
ENABLE_DEBUG_PINS ?= NO
ENABLE_DEBUG_PINS ?= NO
# Set to YES to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO
ENABLE_AUX_UART ?= NO
# Include objects that are just nice information to show
DIAG_STACK ?= NO
DIAG_MIXERSTATUS ?= NO
DIAG_RATEDESIRED ?= NO
DIAG_I2C_WDG_STATS ?= NO
DIAG_TASKS ?= NO
DIAG_STACK ?= NO
DIAG_MIXERSTATUS ?= NO
DIAG_RATEDESIRED ?= NO
DIAG_I2C_WDG_STATS ?= NO
DIAG_TASKS ?= NO
DIAG_INSTRUMENTATION ?= NO
# Or just turn on all the above diagnostics. WARNING: this consumes massive amounts of memory.
DIAG_ALL ?= NO
DIAG_ALL ?= NO
# Optimization level, can be [0, 1, 2, 3, s].
# 0 = turn off optimization. s = optimize for size.
@ -91,6 +92,9 @@ ifneq (,$(filter YES,$(DIAG_TASKS) $(DIAG_ALL)))
CFLAGS += -DDIAG_TASKS
endif
ifneq (,$(filter YES,$(DIAG_INSTRUMENTATION) $(DIAG_ALL)))
CFLAGS += -DPIOS_INCLUDE_INSTRUMENTATION
endif
# Place project-specific -D and/or -U options for Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
ADEFS = -D__ASSEMBLY__