rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Further 2khz Stability Enhancements

This commit is contained in:
borisbstyle 2015-12-24 02:35:01 +01:00
parent a3fe45d774
commit 6d679b72e1
5 changed files with 38 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/drivers/gyro_sync.h
View File

@ -5,6 +5,8 @@
* Author: borisb
*/
#define INTERRUPT_WAIT_TIME 10
extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);

26
src/main/flight/pid.c
View File

@ -94,7 +94,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3];
static float delta1[3], delta2[3];
static float delta1[3], delta2[3], delta3[3], delta4[3], delta5[3];
float delta, deltaSum;
int axis;
float horizonLevelStrength = 1;
@ -194,8 +194,17 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta *= (1.0f / dT);
if (!pidProfile->dterm_cut_hz) {
// add moving average here to reduce noise
deltaSum = (delta1[axis] + delta2[axis] + delta) / 3;
// add moving average here to reduce noise. More averaging needed for 2khz mode
deltaSum = delta1[axis] + delta2[axis] + delta;
if (targetLooptime < 1000) {
deltaSum += delta3[axis] + delta4[axis] + delta5[axis];
delta5[axis] = delta4[axis];
delta4[axis] = delta3[axis];
delta3[axis] = delta2[axis];
deltaSum /= 6;
} else {
deltaSum /= 3;
}
delta2[axis] = delta1[axis];
delta1[axis] = delta;
} else {
@ -234,7 +243,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t errorAngle;
int axis;
int32_t delta, deltaSum;
static int32_t delta1[3], delta2[3];
static int32_t delta1[3], delta2[3], delta3[3], delta4[3], delta5[3];;
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
@ -337,8 +346,15 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
if (!pidProfile->dterm_cut_hz) {
// add moving average here to reduce noise
// add moving average here to reduce noise (More moving average required for 2khz mode)
deltaSum = delta1[axis] + delta2[axis] + delta;
if (targetLooptime < 1000) {
deltaSum += delta3[axis] + delta4[axis] + delta5[axis];
delta5[axis] = delta4[axis];
delta4[axis] = delta3[axis];
delta3[axis] = delta2[axis];
deltaSum /= 2; // Get same scaling
}
delta2[axis] = delta1[axis];
delta1[axis] = delta;
} else {

2
src/main/main.c
View File

@ -541,7 +541,7 @@ int main(void) {
init();
/* Setup scheduler */
rescheduleTask(TASK_GYROPID, targetLooptime);
rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME);
setTaskEnabled(TASK_GYROPID, true);
setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC));

23
src/main/mw.c
View File

@ -91,7 +91,7 @@ enum {
ALIGN_MAG = 2
};
#define JITTER_DEBUG 0 // Specify debug value for jitter debug
//#define JITTER_DEBUG 0 // Specify debug value for jitter debug
/* VBAT monitoring interval (in microseconds) - 1s*/
#define VBATINTERVAL (6 * 3500)
@ -644,17 +644,6 @@ void processRx(void)
static bool haveProcessedAnnexCodeOnce = false;
#endif
// Function for loop trigger
bool taskMainPidLoopCheck(uint32_t currentDeltaTime) {
bool loopTrigger = false;
if (gyroSyncCheckUpdate() || (currentDeltaTime >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
loopTrigger = true;
}
return loopTrigger;
}
void taskMainPidLoop(void)
{
cycleTime = getTaskDeltaTime(TASK_SELF);
@ -748,6 +737,16 @@ void taskMainPidLoop(void)
#endif
}
// Function for loop trigger
void taskMainPidLoopCheck(void) {
while (1) {
if (gyroSyncCheckUpdate() || (getTaskDeltaTime(TASK_SELF) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
taskMainPidLoop();
break;
}
}
}
void taskUpdateAccelerometer(void)
{
imuUpdateAccelerometer(&currentProfile->accelerometerTrims);

8
src/main/scheduler.c
View File

@ -27,7 +27,7 @@
#include "drivers/system.h"
#define SCHEDULER_DEBUG
//#define SCHEDULER_DEBUG
cfTaskId_e currentTaskId = TASK_NONE;
@ -62,8 +62,7 @@ typedef struct {
#endif
} cfTask_t;
bool taskMainPidLoopCheck(uint32_t currentDeltaTime);
void taskMainPidLoop(void);
void taskMainPidLoopCheck(void);
void taskUpdateAccelerometer(void);
void taskHandleSerial(void);
void taskUpdateBeeper(void);
@ -91,8 +90,7 @@ static cfTask_t cfTasks[TASK_COUNT] = {
[TASK_GYROPID] = {
.taskName = "GYRO/PID",
.checkFunc = taskMainPidLoopCheck,
.taskFunc = taskMainPidLoop,
.taskFunc = taskMainPidLoopCheck,
.desiredPeriod = 1000,
.staticPriority = TASK_PRIORITY_REALTIME,
},