Commit f85a2115 authored by Ian McInerney's avatar Ian McInerney

Added logging of time for computations

parent 38743b64
......@@ -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
......@@ -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)
......
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