diff --git a/src/main/fc/fc_main.c b/src/main/fc/fc_main.c index a5367b2eba..0d354f8d54 100644 --- a/src/main/fc/fc_main.c +++ b/src/main/fc/fc_main.c @@ -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( - ¤tProfile->pidProfile, - &accelerometerConfig()->accelerometerTrims - ); - if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;} + pidController(¤tProfile->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) { @@ -711,19 +702,19 @@ void subTaskMainSubprocesses(void) } #ifdef MAG - if (sensors(SENSOR_MAG)) { - updateMagHold(); - } + if (sensors(SENSOR_MAG)) { + updateMagHold(); + } #endif #if defined(BARO) || defined(SONAR) - // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState - updateRcCommands(); - if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { - if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { - applyAltHold(&masterConfig.airplaneConfig); - } + // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + updateRcCommands(); + if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { + if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { + applyAltHold(&masterConfig.airplaneConfig); } + } #endif // If we're armed, at minimum throttle, and we do arming via the @@ -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(¤tProfile->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--; diff --git a/src/main/fc/fc_main.h b/src/main/fc/fc_main.h index 1471e15a8d..000e9844a5 100644 --- a/src/main/fc/fc_main.h +++ b/src/main/fc/fc_main.h @@ -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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8d2aa09e22..cfb8685321 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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 diff --git a/src/main/fc/serial_cli.c b/src/main/fc/serial_cli.c index 1f12836354..9126c7ccb3 100755 --- a/src/main/fc/serial_cli.c +++ b/src/main/fc/serial_cli.c @@ -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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index deb9d8d4c9..7071610d91 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 613c1d186d..fde3441471 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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]; diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 1362c84859..7cc6297cad 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -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;