From 2e8fa5eab10c3873c0b73f45074bff78af822432 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 25 Feb 2016 23:36:51 +0100 Subject: [PATCH] Taskmain rework part II INT wait Fix auto settings F3 --- src/main/config/config.c | 4 +- src/main/config/config_master.h | 2 + src/main/drivers/gyro_sync.h | 2 +- src/main/flight/pid.c | 2 +- src/main/io/serial_cli.c | 20 +++-- src/main/io/serial_msp.c | 14 ++- src/main/mw.c | 189 +++++++++++++++++++++------------------- 7 files changed, 133 insertions(+), 100 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 87d4d765c..3fee52865 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 126; +static const uint8_t EEPROM_CONF_VERSION = 127; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -415,6 +415,8 @@ static void resetConf(void) masterConfig.pid_process_denom = 1; + masterConfig.debug_mode = 0; + resetAccelerometerTrims(&masterConfig.accZero); resetSensorAlignment(&masterConfig.sensorAlignmentConfig); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 111308336..e55cb8f6e 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -67,6 +67,8 @@ typedef struct master_t { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate + uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate + gyroConfig_t gyroConfig; uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index 8e159c684..553cd1c05 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -5,7 +5,7 @@ * Author: borisb */ -#define INTERRUPT_WAIT_TIME 5 +#define INTERRUPT_WAIT_TIME 4 extern uint32_t targetLooptime; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1dc859056..b08469ab0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -78,7 +78,7 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii void setTargetPidLooptime(uint8_t pidProcessDenom) { - targetPidLooptime = targetLooptime / pidProcessDenom; + targetPidLooptime = targetLooptime * pidProcessDenom; } void pidResetErrorAngle(void) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 19322dcd5..0cbd7dc0b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -526,6 +526,7 @@ const clivalue_t valueTable[] = { { "rc_smoothing", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON } }, { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 13 } }, + { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_OFF_ON } }, { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, @@ -2581,24 +2582,31 @@ static void cliTasks(char *cmdline) uint16_t taskFrequency; uint16_t subTaskFrequency; + uint32_t taskTotalTime = taskInfo.totalExecutionTime / 1000; + if (taskId == TASK_GYROPID) { subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f)); - taskFrequency = subTaskFrequency / masterConfig.pid_process_denom; + if (masterConfig.pid_process_denom > 1) { + taskFrequency = subTaskFrequency / masterConfig.pid_process_denom; + cliPrintf("%d - (%s) ", taskId, taskInfo.taskName); + } else { + taskFrequency = subTaskFrequency; + cliPrintf("%d - (%s/%s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); + } } else { taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f)); + cliPrintf("%d - (%s) ", taskId, taskInfo.taskName); } - uint32_t taskTotalTime = taskInfo.totalExecutionTime / 1000; - - cliPrintf("%d - (%s) max: %dus, avg: %dus, rate: %dhz, total: ", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency); + cliPrintf("max: %dus, avg: %dus, rate: %dhz, total: ", taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency); if (taskTotalTime >= 1000) { - cliPrintf("%ds", taskTotalTime / 1000); + cliPrintf("%dsec", taskTotalTime / 1000); } else { cliPrintf("%dms", taskTotalTime); } - if (taskId == TASK_GYROPID) cliPrintf(" (%s rate: %dhz)", taskInfo.subTaskName, subTaskFrequency); + if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) cliPrintf("\r\n- - (%s) rate: %dhz", taskInfo.subTaskName, subTaskFrequency); cliPrintf("\r\n", taskTotalTime); } } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index e79efaf57..24110327e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -114,7 +114,10 @@ void setGyroSamplingSpeed(uint16_t looptime) { gyroSampleRate = 125; maxDivider = 8; masterConfig.pid_process_denom = 1; - if (looptime < 250) { + masterConfig.acc_hardware = 0; + masterConfig.baro_hardware = 0; + masterConfig.mag_hardware = 0; + if (looptime < 375) { masterConfig.acc_hardware = 1; masterConfig.baro_hardware = 1; masterConfig.mag_hardware = 1; @@ -123,6 +126,9 @@ void setGyroSamplingSpeed(uint16_t looptime) { } else { masterConfig.gyro_lpf = 1; masterConfig.pid_process_denom = 1; + masterConfig.acc_hardware = 0; + masterConfig.baro_hardware = 0; + masterConfig.mag_hardware = 0; } #else if (looptime < 1000) { @@ -133,14 +139,18 @@ void setGyroSamplingSpeed(uint16_t looptime) { gyroSampleRate = 125; maxDivider = 8; masterConfig.pid_process_denom = 1; + masterConfig.emf_avoidance = 0; + if (currentProfile->pidProfile.pidController == 2) masterConfig.pid_process_denom = 2; if (looptime < 250) { masterConfig.pid_process_denom = 3; - } else if (looptime < 500) { + masterConfig.emf_avoidance = 1; + } else if (looptime < 375) { if (currentProfile->pidProfile.pidController == 2) { masterConfig.pid_process_denom = 3; } else { masterConfig.pid_process_denom = 2; } + masterConfig.emf_avoidance = 1; } } else { masterConfig.gyro_lpf = 1; diff --git a/src/main/mw.c b/src/main/mw.c index 8308f9a9d..84934d768 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -286,10 +286,6 @@ void annexCode(void) rcCommand[PITCH] = rcCommand_PITCH; } - if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) { - filterRc(); - } - // experimental scaling of RC command to FPV cam angle if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) { scaleRcCommandToFpvCamAngle(); @@ -644,59 +640,6 @@ static bool haveProcessedAnnexCodeOnce = false; void taskMainPidLoop(void) { - imuUpdateAttitude(); - - annexCode(); - -#if defined(BARO) || defined(SONAR) - haveProcessedAnnexCodeOnce = true; -#endif - -#ifdef MAG - if (sensors(SENSOR_MAG)) { - updateMagHold(); - } -#endif - -#ifdef GTUNE - updateGtuneState(); -#endif - -#if defined(BARO) || defined(SONAR) - 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 - // sticks, do not process yaw input from the rx. We do this so the - // motors do not spin up while we are trying to arm or disarm. - // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. - if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck -#ifndef USE_QUAD_MIXER_ONLY -#ifdef USE_SERVOS - && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) -#endif - && masterConfig.mixerMode != MIXER_AIRPLANE - && masterConfig.mixerMode != MIXER_FLYING_WING -#endif - ) { - rcCommand[YAW] = 0; - } - - if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { - rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value); - } - -#ifdef GPS - if (sensors(SENSOR_GPS)) { - if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { - updateGpsStateForHomeAndHoldMode(); - } - } -#endif // PID - note this is function pointer set by setPIDController() pid_controller( @@ -714,30 +657,98 @@ void taskMainPidLoop(void) writeServos(); #endif + if (masterConfig.debug_mode) { + static uint32_t previousMotorUpdateTime, motorCycleTime; + + motorCycleTime = micros() - previousMotorUpdateTime; + previousMotorUpdateTime = micros(); + debug[2] = motorCycleTime; + debug[3] = motorCycleTime - targetPidLooptime; + } + if (motorControlEnable) { writeMotors(); } +} -#ifdef USE_SDCARD - afatfs_poll(); -#endif +void subTasksPreMainPidLoop(void) { + imuUpdateAttitude(); -#ifdef BLACKBOX - if (!cliMode && feature(FEATURE_BLACKBOX)) { - handleBlackbox(); + if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) { + filterRc(); } -#endif -#ifdef TRANSPONDER - updateTransponder(); -#endif + #if defined(BARO) || defined(SONAR) + haveProcessedAnnexCodeOnce = true; + #endif + + #ifdef MAG + if (sensors(SENSOR_MAG)) { + updateMagHold(); + } + #endif + + #ifdef GTUNE + updateGtuneState(); + #endif + + #if defined(BARO) || defined(SONAR) + 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 + // sticks, do not process yaw input from the rx. We do this so the + // motors do not spin up while we are trying to arm or disarm. + // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. + if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck + #ifndef USE_QUAD_MIXER_ONLY + #ifdef USE_SERVOS + && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) + #endif + && masterConfig.mixerMode != MIXER_AIRPLANE + && masterConfig.mixerMode != MIXER_FLYING_WING + #endif + ) { + rcCommand[YAW] = 0; + } + + if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value); + } + + #ifdef GPS + if (sensors(SENSOR_GPS)) { + if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { + updateGpsStateForHomeAndHoldMode(); + } + } + #endif +} + +void subTasksPostMainPidLoop(void) { + #ifdef USE_SDCARD + afatfs_poll(); + #endif + + #ifdef BLACKBOX + if (!cliMode && feature(FEATURE_BLACKBOX)) { + handleBlackbox(); + } + #endif + + #ifdef TRANSPONDER + updateTransponder(); + #endif } // Function for loop trigger void taskMainPidLoopCheck(void) { static uint32_t previousTime; static uint8_t pidUpdateCountdown; - static uint32_t previousPidUpdateTime, pidCycleTime; if (!pidUpdateCountdown) pidUpdateCountdown = masterConfig.pid_process_denom; @@ -746,33 +757,32 @@ void taskMainPidLoopCheck(void) { cycleTime = micros() - previousTime; previousTime = micros(); - // Debugging parameters - debug[0] = cycleTime; - debug[1] = cycleTime - targetLooptime; - debug[2] = averageSystemLoadPercent; - debug[3] = pidCycleTime; + if (masterConfig.debug_mode) { + // Debugging parameters + debug[0] = cycleTime; + debug[1] = averageSystemLoadPercent; + } - while (1) { + while (true) { if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) { imuUpdateGyro(); - switch (pidUpdateCountdown) { - case(1): - pidCycleTime = micros() - previousPidUpdateTime; - previousPidUpdateTime = micros(); - - pidUpdateCountdown = masterConfig.pid_process_denom; - taskMainPidLoop(); - if (masterConfig.pid_process_denom > 1) realTimeCycle = false; - break; - case(2): - realTimeCycle = true; - pidUpdateCountdown--; - break; - default: - pidUpdateCountdown--; + if (pidUpdateCountdown == 2 || masterConfig.pid_process_denom == 1) { + subTasksPreMainPidLoop(); + realTimeCycle = true; // Set realtime guard interval } + + if (pidUpdateCountdown == 1) { + pidUpdateCountdown = masterConfig.pid_process_denom; + taskMainPidLoop(); + if (masterConfig.pid_process_denom > 1) realTimeCycle = false; + } else { + pidUpdateCountdown--; + } + + if (realTimeCycle) subTasksPostMainPidLoop(); + break; } } @@ -843,6 +853,7 @@ void taskUpdateRxMain(void) } } #endif + annexCode(); } #ifdef GPS -- 2.11.4.GIT