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

Minor optimisations to PIDLOOP

This commit is contained in:
Martin Budden 2017-01-08 09:00:56 +00:00
parent 3110f51ccf
commit d8faab6539
7 changed files with 41 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/main/fc/fc_main.c
View File

@ -82,8 +82,6 @@ enum {
#define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
int16_t magHold;
int16_t headFreeModeHold;
@ -94,15 +92,10 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
static float throttlePIDAttenuation;
uint16_t filteredCycleTime;
bool isRXDataNew;
static bool armingCalibrationWasInitialised;
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
float getThrottlePIDAttenuation(void) {
return throttlePIDAttenuation;
}
float getSetpointRate(int axis) {
return setpointRate[axis];
}
@ -689,21 +682,19 @@ void processRx(timeUs_t currentTimeUs)
#endif
}
void subTaskPidController(void)
static void subTaskPidController(void)
{
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(
&currentProfile->pidProfile,
&accelerometerConfig()->accelerometerTrims
);
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;}
pidController(&currentProfile->pidProfile, &accelerometerConfig()->accelerometerTrims, throttlePIDAttenuation);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
}
void subTaskMainSubprocesses(void)
static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
{
const uint32_t startTime = micros();
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// Read out gyro temperature if used for telemmetry
if (feature(FEATURE_TELEMETRY) && gyro.dev.temperature) {
@ -763,25 +754,28 @@ void subTaskMainSubprocesses(void)
#ifdef BLACKBOX
if (!cliMode && feature(FEATURE_BLACKBOX)) {
handleBlackbox(startTime);
handleBlackbox(currentTimeUs);
}
#endif
#ifdef TRANSPONDER
transponderUpdate(startTime);
transponderUpdate(currentTimeUs);
#endif
DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
}
void subTaskMotorUpdate(void)
static void subTaskMotorUpdate(void)
{
const uint32_t startTime = micros();
uint32_t startTime;
if (debugMode == DEBUG_CYCLETIME) {
startTime = micros();
static uint32_t previousMotorUpdateTime;
const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
debug[2] = currentDeltaTime;
debug[3] = currentDeltaTime - targetPidLooptime;
previousMotorUpdateTime = startTime;
} else if (debugMode == DEBUG_PIDLOOP) {
startTime = micros();
}
mixTable(&currentProfile->pidProfile);
@ -813,20 +807,16 @@ uint8_t setPidUpdateCountDown(void)
// Function for loop trigger
void taskMainPidLoop(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
cycleTime = getTaskDeltaTime(TASK_SELF);
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = cycleTime;
debug[0] = getTaskDeltaTime(TASK_SELF);
debug[1] = averageSystemLoadPercent;
}
if (runTaskMainSubprocesses) {
subTaskMainSubprocesses();
subTaskMainSubprocesses(currentTimeUs);
runTaskMainSubprocesses = false;
}
@ -836,9 +826,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
// 2 - subTaskMainSubprocesses()
// 3 - subTaskMotorUpdate()
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
gyroUpdate();
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[0] = micros() - startTime;}
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);
if (pidUpdateCountdown) {
pidUpdateCountdown--;

1
src/main/fc/fc_main.h
View File

@ -34,7 +34,6 @@ void updateLEDs(void);
void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs);
float getThrottlePIDAttenuation(void);
float getSetpointRate(int axis);
float getRcDeflection(int axis);
float getRcDeflectionAbs(int axis);

4
src/main/fc/fc_msp.c
View File

@ -614,7 +614,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_STATUS_EX:
sbufWriteU16(dst, cycleTime);
sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
#ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter());
#else
@ -638,7 +638,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_STATUS:
sbufWriteU16(dst, cycleTime);
sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
#ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter());
#else

6
src/main/fc/serial_cli.c
View File

@ -105,8 +105,6 @@ uint8_t cliMode = 0;
#include "telemetry/frsky.h"
#include "telemetry/telemetry.h"
extern uint16_t cycleTime; // FIXME dependency on mw.c
static serialPort_t *cliPort;
static bufWriter_t *cliWriter;
static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128];
@ -3217,7 +3215,7 @@ static void cliStatus(char *cmdline)
#endif
cliPrintf("Stack size: %d, Stack address: 0x%x\r\n", stackTotalSize(), stackHighMem());
cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", getTaskDeltaTime(TASK_GYROPID), i2cErrorCounter, sizeof(master_t));
#ifdef USE_SDCARD
cliSdInfo(NULL);
@ -3241,7 +3239,7 @@ static void cliTasks(char *cmdline)
int taskFrequency;
int subTaskFrequency;
if (taskId == TASK_GYROPID) {
subTaskFrequency = (int)(1000000.0f / ((float)cycleTime));
subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
if (pidConfig()->pid_process_denom > 1) {
cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);

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

@ -157,7 +157,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
}
float calcHorizonLevelStrength(void) {
static float calcHorizonLevelStrength(void) {
float horizonLevelStrength = 0.0f;
if (horizonTransition > 0.0f) {
const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
@ -167,7 +167,7 @@ float calcHorizonLevelStrength(void) {
return horizonLevelStrength;
}
float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
// calculate error angle and limit the angle to the max inclination
float errorAngle = pidProfile->levelSensitivity * getRcDeflection(axis);
#ifdef GPS
@ -187,7 +187,7 @@ float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims
return currentPidSetpoint;
}
float accelerationLimit(int axis, float currentPidSetpoint) {
static float accelerationLimit(int axis, float currentPidSetpoint) {
static float previousSetpoint[3];
const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
@ -200,14 +200,12 @@ float accelerationLimit(int axis, float currentPidSetpoint) {
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor)
{
static float previousRateError[2];
static float previousSetpoint[3];
// ----------PID controller----------
const float tpaFactor = getThrottlePIDAttenuation();
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float currentPidSetpoint = getSetpointRate(axis);

2
src/main/flight/pid.h
View File

@ -88,7 +88,7 @@ typedef struct pidConfig_s {
} pidConfig_t;
union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, float tpaFactor);
extern float axisPIDf[3];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

9
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -45,6 +45,8 @@
#include "rx/rx.h"
#include "rx/msp.h"
#include "scheduler/scheduler.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
@ -269,7 +271,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
extern volatile uint8_t CRC8;
extern volatile bool coreProReady;
extern uint16_t cycleTime; // FIXME dependency on mw.c
// this is calculated at startup based on enabled features.
static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
@ -562,7 +563,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
break;
case BST_STATUS:
bstWrite16(cycleTime);
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
#ifdef USE_I2C
bstWrite16(i2cGetErrorCounter());
#else
@ -691,7 +692,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
break;
case BST_LOOP_TIME:
//bstWrite16(masterConfig.looptime);
bstWrite16(cycleTime);
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
break;
case BST_RC_TUNING:
bstWrite8(currentControlRateProfile->rcRate8);
@ -1043,7 +1044,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
break;
case BST_SET_LOOP_TIME:
//masterConfig.looptime = bstRead16();
cycleTime = bstRead16();
bstRead16();
break;
case BST_SET_PID_CONTROLLER:
break;