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

Taskmain rework part II 

INT wait

Fix auto settings F3
This commit is contained in:
borisbstyle 2016-02-25 23:36:51 +01:00
parent f7091f48d0
commit 2e8fa5eab1
7 changed files with 137 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/config/config.c
View File

@ -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);

2
src/main/config/config_master.h
View File

@ -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

2
src/main/drivers/gyro_sync.h
View File

@ -5,7 +5,7 @@
* Author: borisb
*/
#define INTERRUPT_WAIT_TIME 5
#define INTERRUPT_WAIT_TIME 4
extern uint32_t targetLooptime;

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

@ -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)

28
src/main/io/serial_cli.c
View File

@ -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;
if (taskId == TASK_GYROPID) {
subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f));
taskFrequency = subTaskFrequency / masterConfig.pid_process_denom;
} else {
taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f));
}
uint32_t taskTotalTime = taskInfo.totalExecutionTime / 1000;
cliPrintf("%d - (%s) max: %dus, avg: %dus, rate: %dhz, total: ", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency);
if (taskId == TASK_GYROPID) {
subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f));
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);
}
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);
}
}

14
src/main/io/serial_msp.c
View File

@ -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;

189
src/main/mw.c
View File

@ -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