Merge pull request #377 from martinbudden/bf_tasks
Minor improvements to pid loop in mw.c
This commit is contained in:
commit
56acda8814
|
@ -419,11 +419,6 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
|
|||
}
|
||||
}
|
||||
|
||||
void imuUpdateGyro(void)
|
||||
{
|
||||
gyroUpdate();
|
||||
}
|
||||
|
||||
void imuUpdateAttitude(void)
|
||||
{
|
||||
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
|
||||
|
|
|
@ -79,7 +79,6 @@ void imuConfigure(
|
|||
float getCosTiltAngle(void);
|
||||
void calculateEstimatedAltitude(uint32_t currentTime);
|
||||
void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
|
||||
void imuUpdateGyro(void);
|
||||
void imuUpdateAttitude(void);
|
||||
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
|
||||
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||
|
@ -88,5 +87,4 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
|
|||
int16_t imuCalculateHeading(t_fp_vector *vec);
|
||||
|
||||
void imuResetAccelerationSum(void);
|
||||
void imuUpdateGyro(void);
|
||||
void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims);
|
||||
|
|
|
@ -653,13 +653,8 @@ void processRx(void)
|
|||
|
||||
}
|
||||
|
||||
#if defined(BARO) || defined(SONAR)
|
||||
static bool haveProcessedAnnexCodeOnce = false;
|
||||
#endif
|
||||
|
||||
void taskMainPidLoop(void)
|
||||
void subTaskPidController(void)
|
||||
{
|
||||
|
||||
// PID - note this is function pointer set by setPIDController()
|
||||
pid_controller(
|
||||
¤tProfile->pidProfile,
|
||||
|
@ -668,20 +663,14 @@ void taskMainPidLoop(void)
|
|||
&masterConfig.accelerometerTrims,
|
||||
&masterConfig.rxConfig
|
||||
);
|
||||
|
||||
mixTable();
|
||||
}
|
||||
|
||||
void subTasksMainPidLoop(void) {
|
||||
void subTaskMainSubprocesses(void) {
|
||||
|
||||
if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
|
||||
filterRc();
|
||||
}
|
||||
|
||||
#if defined(BARO) || defined(SONAR)
|
||||
haveProcessedAnnexCodeOnce = true;
|
||||
#endif
|
||||
|
||||
#ifdef MAG
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
updateMagHold();
|
||||
|
@ -743,7 +732,8 @@ void subTasksMainPidLoop(void) {
|
|||
#endif
|
||||
}
|
||||
|
||||
void taskMotorUpdate(void) {
|
||||
void subTaskMotorUpdate(void)
|
||||
{
|
||||
if (debugMode == DEBUG_CYCLETIME) {
|
||||
static uint32_t previousMotorUpdateTime;
|
||||
uint32_t currentDeltaTime = micros() - previousMotorUpdateTime;
|
||||
|
@ -752,6 +742,8 @@ void taskMotorUpdate(void) {
|
|||
previousMotorUpdateTime = micros();
|
||||
}
|
||||
|
||||
mixTable();
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
filterServos();
|
||||
writeServos();
|
||||
|
@ -771,11 +763,12 @@ uint8_t setPidUpdateCountDown(void) {
|
|||
}
|
||||
|
||||
// Function for loop trigger
|
||||
void taskMainPidLoopCheck(void) {
|
||||
void taskMainPidLoopCheck(void)
|
||||
{
|
||||
static uint32_t previousTime;
|
||||
static bool runTaskMainSubprocesses;
|
||||
|
||||
uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
|
||||
const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
|
||||
|
||||
cycleTime = micros() - previousTime;
|
||||
previousTime = micros();
|
||||
|
@ -790,18 +783,18 @@ void taskMainPidLoopCheck(void) {
|
|||
static uint8_t pidUpdateCountdown;
|
||||
|
||||
if (runTaskMainSubprocesses) {
|
||||
subTasksMainPidLoop();
|
||||
subTaskMainSubprocesses();
|
||||
runTaskMainSubprocesses = false;
|
||||
}
|
||||
|
||||
imuUpdateGyro();
|
||||
gyroUpdate();
|
||||
|
||||
if (pidUpdateCountdown) {
|
||||
pidUpdateCountdown--;
|
||||
} else {
|
||||
pidUpdateCountdown = setPidUpdateCountDown();
|
||||
taskMainPidLoop();
|
||||
taskMotorUpdate();
|
||||
subTaskPidController();
|
||||
subTaskMotorUpdate();
|
||||
runTaskMainSubprocesses = true;
|
||||
}
|
||||
|
||||
|
@ -862,24 +855,19 @@ void taskUpdateRxMain(void)
|
|||
processRx();
|
||||
isRXDataNew = true;
|
||||
|
||||
// the 'annexCode' initialses rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
|
||||
annexCode();
|
||||
#ifdef BARO
|
||||
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
|
||||
if (haveProcessedAnnexCodeOnce) {
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
updateAltHoldState();
|
||||
}
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
updateAltHoldState();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
|
||||
if (haveProcessedAnnexCodeOnce) {
|
||||
if (sensors(SENSOR_SONAR)) {
|
||||
updateSonarAltHoldState();
|
||||
}
|
||||
if (sensors(SENSOR_SONAR)) {
|
||||
updateSonarAltHoldState();
|
||||
}
|
||||
#endif
|
||||
annexCode();
|
||||
}
|
||||
|
||||
#ifdef GPS
|
||||
|
|
Loading…
Reference in New Issue