Commit f85a2115 authored by Ian McInerney's avatar Ian McInerney

Added logging of time for computations

parent 38743b64
...@@ -4,6 +4,7 @@ ...@@ -4,6 +4,7 @@
#include "crtp.h" #include "crtp.h"
#include "debug.h" #include "debug.h"
#include "config.h" #include "config.h"
#include "log.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "task.h" #include "task.h"
...@@ -24,6 +25,8 @@ static bool computationEnabled = false; ...@@ -24,6 +25,8 @@ static bool computationEnabled = false;
// Rate at which the computation should run (in Hz) // Rate at which the computation should run (in Hz)
static uint16_t computationRate; static uint16_t computationRate;
static uint16_t computationDuration;
/** /**
* Callback for the computation CRTP packet * Callback for the computation CRTP packet
*/ */
...@@ -95,8 +98,13 @@ void Computation_Task() { ...@@ -95,8 +98,13 @@ void Computation_Task() {
while(1) { while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(computationRate)); vTaskDelayUntil(&lastWakeTime, F2T(computationRate));
if (computationEnabled) { if (computationEnabled) {
uint32_t computationStart = xTaskGetTickCount();
// Perform the computations // Perform the computations
computation_processing(); 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() { ...@@ -108,6 +116,7 @@ void Computation_Init() {
// Default the computation to using a 100Hz rate // Default the computation to using a 100Hz rate
computationRate = RATE_100_HZ; computationRate = RATE_100_HZ;
computationDuration = 0;
computation_algorithm_init(); computation_algorithm_init();
...@@ -122,3 +131,7 @@ bool Computation_Test() { ...@@ -122,3 +131,7 @@ bool Computation_Test() {
crtpTest(); crtpTest();
return isInit; return isInit;
} }
LOG_GROUP_START(compStats)
LOG_ADD(LOG_UINT16, dur, &computationDuration)
LOG_GROUP_STOP(compStats)
\ No newline at end of file
...@@ -61,6 +61,10 @@ static control_t control; ...@@ -61,6 +61,10 @@ static control_t control;
static uint8_t resetControllers; 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); static void stabilizerTask(void* param);
void stabilizerInit(void) void stabilizerInit(void)
...@@ -123,6 +127,8 @@ static void stabilizerTask(void* param) ...@@ -123,6 +127,8 @@ static void stabilizerTask(void* param)
while(1) { while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP)); vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
uint32_t estimatorStart = xTaskGetTickCount();
#ifdef ESTIMATOR_TYPE_kalman #ifdef ESTIMATOR_TYPE_kalman
stateEstimatorUpdate(&state, &sensorData, &control); stateEstimatorUpdate(&state, &sensorData, &control);
#else #else
...@@ -130,11 +136,15 @@ static void stabilizerTask(void* param) ...@@ -130,11 +136,15 @@ static void stabilizerTask(void* param)
stateEstimator(&state, &sensorData, tick); stateEstimator(&state, &sensorData, tick);
commanderXYZ_GetPosition(&state, tick); commanderXYZ_GetPosition(&state, tick);
#endif #endif
uint32_t estimatorEnd = xTaskGetTickCount();
commanderXYZ_GetSetpoint(&setpoint, &state, &resetControllers); commanderXYZ_GetSetpoint(&setpoint, &state, &resetControllers);
computation_update_state(&state, &sensorData); computation_update_state(&state, &sensorData);
#ifndef BYPASS_CONTROLLERS #ifndef BYPASS_CONTROLLERS
uint32_t controllerStart = xTaskGetTickCount();
stateController(&control, &sensorData, &state, &setpoint, tick, resetControllers); stateController(&control, &sensorData, &state, &setpoint, tick, resetControllers);
// Perform a check to see if the quadcopter is upside-down // Perform a check to see if the quadcopter is upside-down
...@@ -155,10 +165,22 @@ static void stabilizerTask(void* param) ...@@ -155,10 +165,22 @@ static void stabilizerTask(void* param)
powerDistribution(&control); 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++; 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_GROUP_START(ctrltarget)
LOG_ADD(LOG_FLOAT, roll, &setpoint.attitude.roll) LOG_ADD(LOG_FLOAT, roll, &setpoint.attitude.roll)
LOG_ADD(LOG_FLOAT, pitch, &setpoint.attitude.pitch) LOG_ADD(LOG_FLOAT, pitch, &setpoint.attitude.pitch)
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment