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

Tidy of main pid loop.

Browse source
This commit is contained in:
Martin Budden 2016-05-04 10:12:11 +01:00
parent e9963f454b
commit 8c7c72c5dc
3 changed files with 19 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/flight/imu.c
View file

@ -419,11 +419,6 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
} }
} }
void imuUpdateGyro(void)
{
gyroUpdate();
}
void imuUpdateAttitude(void) void imuUpdateAttitude(void)
{ {
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) { if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {

2
src/main/flight/imu.h
View file

@ -79,7 +79,6 @@ void imuConfigure(
float getCosTiltAngle(void); float getCosTiltAngle(void);
void calculateEstimatedAltitude(uint32_t currentTime); void calculateEstimatedAltitude(uint32_t currentTime);
void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims); void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
void imuUpdateGyro(void);
void imuUpdateAttitude(void); void imuUpdateAttitude(void);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle); float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); 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); int16_t imuCalculateHeading(t_fp_vector *vec);
void imuResetAccelerationSum(void); void imuResetAccelerationSum(void);
void imuUpdateGyro(void);
void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims); void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims);

50
src/main/mw.c
View file

@ -653,13 +653,8 @@ void processRx(void)
} }
#if defined(BARO) || defined(SONAR) void subTaskPidController(void)
static bool haveProcessedAnnexCodeOnce = false;
#endif
void taskMainPidLoop(void)
{ {
// PID - note this is function pointer set by setPIDController() // PID - note this is function pointer set by setPIDController()
pid_controller( pid_controller(
&currentProfile->pidProfile, &currentProfile->pidProfile,
@ -668,20 +663,14 @@ void taskMainPidLoop(void)
&masterConfig.accelerometerTrims, &masterConfig.accelerometerTrims,
&masterConfig.rxConfig &masterConfig.rxConfig
); );
mixTable();
} }
void subTasksMainPidLoop(void) { void subTaskMainSubprocesses(void) {
if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) { if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
filterRc(); filterRc();
} }
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;
#endif
#ifdef MAG #ifdef MAG
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
updateMagHold(); updateMagHold();
@ -743,7 +732,8 @@ void subTasksMainPidLoop(void) {
#endif #endif
} }
void taskMotorUpdate(void) { void subTaskMotorUpdate(void)
{
if (debugMode == DEBUG_CYCLETIME) { if (debugMode == DEBUG_CYCLETIME) {
static uint32_t previousMotorUpdateTime; static uint32_t previousMotorUpdateTime;
uint32_t currentDeltaTime = micros() - previousMotorUpdateTime; uint32_t currentDeltaTime = micros() - previousMotorUpdateTime;
@ -752,6 +742,8 @@ void taskMotorUpdate(void) {
previousMotorUpdateTime = micros(); previousMotorUpdateTime = micros();
} }
mixTable();
#ifdef USE_SERVOS #ifdef USE_SERVOS
filterServos(); filterServos();
writeServos(); writeServos();
@ -771,11 +763,12 @@ uint8_t setPidUpdateCountDown(void) {
} }
// Function for loop trigger // Function for loop trigger
void taskMainPidLoopCheck(void) { void taskMainPidLoopCheck(void)
{
static uint32_t previousTime; static uint32_t previousTime;
static bool runTaskMainSubprocesses; static bool runTaskMainSubprocesses;
uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
cycleTime = micros() - previousTime; cycleTime = micros() - previousTime;
previousTime = micros(); previousTime = micros();
@ -790,18 +783,18 @@ void taskMainPidLoopCheck(void) {
static uint8_t pidUpdateCountdown; static uint8_t pidUpdateCountdown;
if (runTaskMainSubprocesses) { if (runTaskMainSubprocesses) {
subTasksMainPidLoop(); subTaskMainSubprocesses();
runTaskMainSubprocesses = false; runTaskMainSubprocesses = false;
} }
imuUpdateGyro(); gyroUpdate();
if (pidUpdateCountdown) { if (pidUpdateCountdown) {
pidUpdateCountdown--; pidUpdateCountdown--;
} else { } else {
pidUpdateCountdown = setPidUpdateCountDown(); pidUpdateCountdown = setPidUpdateCountDown();
taskMainPidLoop(); subTaskPidController();
taskMotorUpdate(); subTaskMotorUpdate();
runTaskMainSubprocesses = true; runTaskMainSubprocesses = true;
} }
@ -862,24 +855,19 @@ void taskUpdateRxMain(void)
processRx(); processRx();
isRXDataNew = true; isRXDataNew = true;
// the 'annexCode' initialses rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
annexCode();
#ifdef BARO #ifdef BARO
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. if (sensors(SENSOR_BARO)) {
if (haveProcessedAnnexCodeOnce) { updateAltHoldState();
if (sensors(SENSOR_BARO)) {
updateAltHoldState();
}
} }
#endif #endif
#ifdef SONAR #ifdef SONAR
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. if (sensors(SENSOR_SONAR)) {
if (haveProcessedAnnexCodeOnce) { updateSonarAltHoldState();
if (sensors(SENSOR_SONAR)) {
updateSonarAltHoldState();
}
} }
#endif #endif
annexCode();
} }
#ifdef GPS #ifdef GPS