diff --git a/crazyflie-firmware-LateralPosition_2016.09/src/modules/src/computation/computation.c b/crazyflie-firmware-LateralPosition_2016.09/src/modules/src/computation/computation.c index c0e5d4ee94b46713cacab073abab96bc2b6ab50c..9e6fef15c83097da4dcbb4a4552e76d97ddd5183 100644 --- a/crazyflie-firmware-LateralPosition_2016.09/src/modules/src/computation/computation.c +++ b/crazyflie-firmware-LateralPosition_2016.09/src/modules/src/computation/computation.c @@ -4,6 +4,7 @@ #include "crtp.h" #include "debug.h" #include "config.h" +#include "log.h" #include "FreeRTOS.h" #include "task.h" @@ -24,6 +25,8 @@ static bool computationEnabled = false; // Rate at which the computation should run (in Hz) static uint16_t computationRate; +static uint16_t computationDuration; + /** * Callback for the computation CRTP packet */ @@ -95,8 +98,13 @@ void Computation_Task() { while(1) { vTaskDelayUntil(&lastWakeTime, F2T(computationRate)); if (computationEnabled) { + uint32_t computationStart = xTaskGetTickCount(); // Perform the computations computation_processing(); + uint32_t computationEnd = xTaskGetTickCount(); + + // Convert the number of ticks to milliseconds elapsed + computationDuration = (computationEnd - computationStart) * (1000/configTICK_RATE_HZ); } } } @@ -108,6 +116,7 @@ void Computation_Init() { // Default the computation to using a 100Hz rate computationRate = RATE_100_HZ; + computationDuration = 0; computation_algorithm_init(); @@ -122,3 +131,7 @@ bool Computation_Test() { crtpTest(); return isInit; } + +LOG_GROUP_START(compStats) +LOG_ADD(LOG_UINT16, dur, &computationDuration) +LOG_GROUP_STOP(compStats) \ No newline at end of file diff --git a/crazyflie-firmware-LateralPosition_2016.09/src/modules/src/stabilizer.c b/crazyflie-firmware-LateralPosition_2016.09/src/modules/src/stabilizer.c index ea1d53975e0215d09ae0ee7458c410b695d67950..d6cf668d93ebe57a765eaa2c1564c28181c25584 100644 --- a/crazyflie-firmware-LateralPosition_2016.09/src/modules/src/stabilizer.c +++ b/crazyflie-firmware-LateralPosition_2016.09/src/modules/src/stabilizer.c @@ -61,6 +61,10 @@ static control_t control; static uint8_t resetControllers; +// Variables to hold the amount of time things have taken +static uint16_t controllerDuration; +static uint16_t estimatorDuration; + static void stabilizerTask(void* param); void stabilizerInit(void) @@ -123,6 +127,8 @@ static void stabilizerTask(void* param) while(1) { vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP)); + uint32_t estimatorStart = xTaskGetTickCount(); + #ifdef ESTIMATOR_TYPE_kalman stateEstimatorUpdate(&state, &sensorData, &control); #else @@ -130,11 +136,15 @@ static void stabilizerTask(void* param) stateEstimator(&state, &sensorData, tick); commanderXYZ_GetPosition(&state, tick); #endif + + uint32_t estimatorEnd = xTaskGetTickCount(); + commanderXYZ_GetSetpoint(&setpoint, &state, &resetControllers); computation_update_state(&state, &sensorData); #ifndef BYPASS_CONTROLLERS + uint32_t controllerStart = xTaskGetTickCount(); stateController(&control, &sensorData, &state, &setpoint, tick, resetControllers); // Perform a check to see if the quadcopter is upside-down @@ -155,10 +165,22 @@ static void stabilizerTask(void* param) powerDistribution(&control); + uint32_t controllerEnd = xTaskGetTickCount(); + + // Convert the number of ticks to milliseconds elapsed + controllerDuration = (controllerEnd - controllerStart) * (1000/configTICK_RATE_HZ); + estimatorDuration = (estimatorEnd - estimatorStart) * (1000/configTICK_RATE_HZ); + + tick++; } } +LOG_GROUP_START(controlStats) +LOG_ADD(LOG_UINT16, estimDur, &estimatorDuration) +LOG_ADD(LOG_UINT16, contDur, &controllerDuration) +LOG_GROUP_STOP(controlStats) + LOG_GROUP_START(ctrltarget) LOG_ADD(LOG_FLOAT, roll, &setpoint.attitude.roll) LOG_ADD(LOG_FLOAT, pitch, &setpoint.attitude.pitch)