Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Gyro native rate sampling, filtering, and scheduler restructuring

Browse source
This commit is contained in:
Bruce Luckcuck 2020-01-06 07:52:35 -05:00
parent b11565efbe
commit f584780944
42 changed files with 718 additions and 421 deletions
Download patch file Download diff file Expand all files Collapse all files

48
src/main/fc/core.c
View file

@ -58,6 +58,7 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "fc/stats.h"
#include "fc/tasks.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
@ -142,6 +143,8 @@ enum {
int16_t magHold;
#endif
static FAST_RAM_ZERO_INIT uint8_t pidUpdateCounter;
static bool flipOverAfterCrashActive = false;
static timeUs_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
@ -1214,10 +1217,42 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
processRcCommand();
}
FAST_CODE void taskGyroSample(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
gyroUpdate();
if (pidUpdateCounter % activePidLoopDenom == 0) {
pidUpdateCounter = 0;
}
pidUpdateCounter++;
}
FAST_CODE bool gyroFilterReady(void)
{
if (pidUpdateCounter % activePidLoopDenom == 0) {
return true;
} else {
return false;
}
}
FAST_CODE bool pidLoopReady(void)
{
if ((pidUpdateCounter % activePidLoopDenom) == (activePidLoopDenom / 2)) {
return true;
}
return false;
}
FAST_CODE void taskFiltering(timeUs_t currentTimeUs)
{
gyroFiltering(currentTimeUs);
}
// Function for loop trigger
FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
{
static uint32_t pidUpdateCounter = 0;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
if (lockMainPID() != 0) return;
@ -1228,15 +1263,12 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
// 1 - subTaskPidController()
// 2 - subTaskMotorUpdate()
// 3 - subTaskPidSubprocesses()
gyroUpdate(currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
if (pidUpdateCounter++ % pidConfig()->pid_process_denom == 0) {
subTaskRcCommand(currentTimeUs);
subTaskPidController(currentTimeUs);
subTaskMotorUpdate(currentTimeUs);
subTaskPidSubprocesses(currentTimeUs);
}
subTaskRcCommand(currentTimeUs);
subTaskPidController(currentTimeUs);
subTaskMotorUpdate(currentTimeUs);
subTaskPidSubprocesses(currentTimeUs);
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = getTaskDeltaTime(TASK_SELF);