Added logging of time for computations

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

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)