Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
C
Custom_CrazyFlie_Software
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Analytics
Analytics
Repository
Value Stream
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Commits
Open sidebar
Distributed Autonomous Networked Control Lab
Custom_CrazyFlie_Software
Commits
f85a2115
Commit
f85a2115
authored
May 09, 2017
by
Ian McInerney
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Added logging of time for computations
parent
38743b64
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
35 additions
and
0 deletions
+35
-0
crazyflie-firmware-LateralPosition_2016.09/src/modules/src/computation/computation.c
...osition_2016.09/src/modules/src/computation/computation.c
+13
-0
crazyflie-firmware-LateralPosition_2016.09/src/modules/src/stabilizer.c
...ware-LateralPosition_2016.09/src/modules/src/stabilizer.c
+22
-0
No files found.
crazyflie-firmware-LateralPosition_2016.09/src/modules/src/computation/computation.c
View file @
f85a2115
...
@@ -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
crazyflie-firmware-LateralPosition_2016.09/src/modules/src/stabilizer.c
View file @
f85a2115
...
@@ -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
)
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment