rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-1365 Flight code instrumentation API: Sample use of instrumentation in Atittude module.

Browse Source 
When instrumentation is enabled at board level (pios_board.h, enable PIOS_INCLUDE_INSTRUMENTATION) PIOS_INSTRUMENT_MODULE setups the module to be instrumented.
This commit is contained in:
Alessio Morale 2014-06-10 21:38:12 +02:00
parent 8ba5741f99
commit 16cb081f5c
1 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -65,6 +65,19 @@
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include <pios_notify.h> #include <pios_notify.h>
//#define PIOS_INSTRUMENT_MODULE
#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).
// Private constants // Private constants
#define STACK_SIZE_BYTES 540 #define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
@ -234,6 +247,12 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
#endif #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 // Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle()); settingsUpdatedCb(AttitudeSettingsHandle());
@ -291,9 +310,11 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
} else { } else {
// Do not update attitude data in simulation mode // Do not update attitude data in simulation mode
if (!AttitudeStateReadOnly()) { if (!AttitudeStateReadOnly()) {
PERF_TIMED_SECTION_START(counterAtt);
updateAttitude(&accelState, &gyros); updateAttitude(&accelState, &gyros);
PERF_TIMED_SECTION_END(counterAtt);
} }
PERF_MEASURE_PERIOD(counterPeriod);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
} }
} }
@ -324,7 +345,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
if (PIOS_ADXL345_FifoElements() == 0) { if (PIOS_ADXL345_FifoElements() == 0) {
return -1; return -1;
} }
PERF_TIMED_SECTION_START(counterUpd);
// First sample is temperature // First sample is temperature
gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.X; gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.X;
gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Y; gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Y;
@ -342,7 +363,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
y += -accel_data.y; y += -accel_data.y;
z += -accel_data.z; z += -accel_data.z;
} while ((i < 32) && (samples_remaining > 0)); } while ((i < 32) && (samples_remaining > 0));
PERF_TRACK_VALUE(counterAccelSamples, i);
float accel[3] = { accel_scale.X * (float)x / i, float accel[3] = { accel_scale.X * (float)x / i,
accel_scale.Y * (float)y / i, accel_scale.Y * (float)y / i,
accel_scale.Z * (float)z / i }; accel_scale.Z * (float)z / i };
@ -401,6 +422,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try // Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly) // and make it average zero (weakly)
gyro_correct_int[2] += -gyros->z * yawBiasRate; gyro_correct_int[2] += -gyros->z * yawBiasRate;
PERF_TIMED_SECTION_END(counterUpd);
GyroStateSet(gyros); GyroStateSet(gyros);
AccelStateSet(accelState); AccelStateSet(accelState);
@ -429,6 +451,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
if (GyroStateReadOnly() || AccelStateReadOnly()) { if (GyroStateReadOnly() || AccelStateReadOnly()) {
return 0; return 0;
} }
PERF_TIMED_SECTION_START(counterUpd);
gyros[0] = mpu6000_data.gyro_x * gyro_scale.X; gyros[0] = mpu6000_data.gyro_x * gyro_scale.X;
gyros[1] = mpu6000_data.gyro_y * gyro_scale.Y; gyros[1] = mpu6000_data.gyro_y * gyro_scale.Y;
gyros[2] = mpu6000_data.gyro_z * gyro_scale.Z; gyros[2] = mpu6000_data.gyro_z * gyro_scale.Z;
@ -492,7 +515,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try // Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly) // and make it average zero (weakly)
gyro_correct_int[2] += -gyrosData->z * yawBiasRate; gyro_correct_int[2] += -gyrosData->z * yawBiasRate;
PERF_TIMED_SECTION_END(counterUpd);
GyroStateSet(gyrosData); GyroStateSet(gyrosData);
AccelStateSet(accelStateData); AccelStateSet(accelStateData);