mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
async processing removed
This commit is contained in:
parent
fd34e7acf2
commit
aac9e485c0
16 changed files with 28 additions and 283 deletions
|
@ -1548,7 +1548,7 @@ static bool blackboxWriteSysinfo(void)
|
|||
}
|
||||
);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", getPidUpdateRate());
|
||||
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", getLooptime());
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", 100); //For compatibility reasons write rc_rate 100
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->stabilized.rcExpo8);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8);
|
||||
|
|
|
@ -2474,11 +2474,7 @@ static void cliStatus(char *cmdline)
|
|||
#endif
|
||||
|
||||
cliPrintf("System load: %d", averageSystemLoadPercent);
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_PID);
|
||||
#else
|
||||
const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_GYROPID);
|
||||
#endif
|
||||
const int pidRate = pidTaskDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)pidTaskDeltaTime));
|
||||
const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX)));
|
||||
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
|
||||
|
|
|
@ -106,7 +106,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
|
|||
.enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES
|
||||
);
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 1);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
|
||||
.current_profile_index = 0,
|
||||
|
@ -114,9 +114,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
|
|||
.debug_mode = DEBUG_NONE,
|
||||
.i2c_speed = I2C_SPEED_400KHZ,
|
||||
.cpuUnderclock = 0,
|
||||
.accTaskFrequency = ACC_TASK_FREQUENCY_DEFAULT,
|
||||
.attitudeTaskFrequency = ATTITUDE_TASK_FREQUENCY_DEFAULT,
|
||||
.asyncMode = ASYNC_MODE_NONE,
|
||||
.throttle_tilt_compensation_strength = 0, // 0-100, 0 - disabled
|
||||
.pwmRxInputFilteringMode = INPUT_FILTERING_DISABLED,
|
||||
.name = { 0 }
|
||||
|
@ -152,52 +149,9 @@ void validateNavConfig(void)
|
|||
#define SECOND_PORT_INDEX 1
|
||||
#endif
|
||||
|
||||
uint32_t getPidUpdateRate(void)
|
||||
{
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
if (systemConfig()->asyncMode == ASYNC_MODE_NONE) {
|
||||
return getGyroUpdateRate();
|
||||
} else {
|
||||
return gyroConfig()->looptime;
|
||||
}
|
||||
#else
|
||||
uint32_t getLooptime(void) {
|
||||
return gyro.targetLooptime;
|
||||
#endif
|
||||
}
|
||||
|
||||
timeDelta_t getGyroUpdateRate(void)
|
||||
{
|
||||
return gyro.targetLooptime;
|
||||
}
|
||||
|
||||
uint16_t getAccUpdateRate(void)
|
||||
{
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
// ACC will be updated at its own rate
|
||||
if (systemConfig()->asyncMode == ASYNC_MODE_ALL) {
|
||||
return 1000000 / systemConfig()->accTaskFrequency;
|
||||
} else {
|
||||
return getPidUpdateRate();
|
||||
}
|
||||
#else
|
||||
// ACC updated at same frequency in taskMainPidLoop in mw.c
|
||||
return gyro.targetLooptime;
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
uint16_t getAttitudeUpdateRate(void) {
|
||||
if (systemConfig()->asyncMode == ASYNC_MODE_ALL) {
|
||||
return 1000000 / systemConfig()->attitudeTaskFrequency;
|
||||
} else {
|
||||
return getPidUpdateRate();
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t getAsyncMode(void) {
|
||||
return systemConfig()->asyncMode;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void validateAndFixConfig(void)
|
||||
{
|
||||
|
@ -249,15 +203,6 @@ void validateAndFixConfig(void)
|
|||
featureClear(FEATURE_SOFTSERIAL);
|
||||
}
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
/*
|
||||
* When async processing mode is enabled, gyroSync has to be forced to "ON"
|
||||
*/
|
||||
if (getAsyncMode() != ASYNC_MODE_NONE) {
|
||||
gyroConfigMutable()->gyroSync = 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
|
||||
if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) {
|
||||
const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
|
||||
|
@ -322,7 +267,6 @@ void validateAndFixConfig(void)
|
|||
|
||||
#if !defined(USE_MPU_DATA_READY_SIGNAL)
|
||||
gyroConfigMutable()->gyroSync = false;
|
||||
systemConfigMutable()->asyncMode = ASYNC_MODE_NONE;
|
||||
#endif
|
||||
|
||||
if (settingsValidate(NULL)) {
|
||||
|
|
|
@ -29,13 +29,6 @@
|
|||
#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
|
||||
#define MAX_NAME_LENGTH 16
|
||||
|
||||
#define ACC_TASK_FREQUENCY_DEFAULT 500
|
||||
#define ACC_TASK_FREQUENCY_MIN 100
|
||||
#define ACC_TASK_FREQUENCY_MAX 1000
|
||||
#define ATTITUDE_TASK_FREQUENCY_DEFAULT 250
|
||||
#define ATTITUDE_TASK_FREQUENCY_MIN 100
|
||||
#define ATTITUDE_TASK_FREQUENCY_MAX 1000
|
||||
|
||||
typedef enum {
|
||||
ASYNC_MODE_NONE,
|
||||
ASYNC_MODE_GYRO,
|
||||
|
@ -78,11 +71,8 @@ typedef enum {
|
|||
} features_e;
|
||||
|
||||
typedef struct systemConfig_s {
|
||||
uint16_t accTaskFrequency;
|
||||
uint16_t attitudeTaskFrequency;
|
||||
uint8_t current_profile_index;
|
||||
uint8_t current_battery_profile_index;
|
||||
uint8_t asyncMode;
|
||||
uint8_t debug_mode;
|
||||
uint8_t i2c_speed;
|
||||
uint8_t cpuUnderclock;
|
||||
|
@ -146,10 +136,4 @@ void createDefaultConfig(void);
|
|||
void resetConfigs(void);
|
||||
void targetConfiguration(void);
|
||||
|
||||
uint32_t getPidUpdateRate(void);
|
||||
timeDelta_t getGyroUpdateRate(void);
|
||||
uint16_t getAccUpdateRate(void);
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
uint16_t getAttitudeUpdateRate(void);
|
||||
uint8_t getAsyncMode(void);
|
||||
#endif
|
||||
uint32_t getLooptime(void);
|
||||
|
|
|
@ -638,7 +638,7 @@ void filterRc(bool isRXDataNew)
|
|||
|
||||
// Calculate average cycle time (1Hz LPF on cycle time)
|
||||
if (!filterInitialised) {
|
||||
biquadFilterInitLPF(&filteredCycleTimeState, 1, getPidUpdateRate());
|
||||
biquadFilterInitLPF(&filteredCycleTimeState, 1, getLooptime());
|
||||
filterInitialised = true;
|
||||
}
|
||||
|
||||
|
@ -676,7 +676,7 @@ void taskGyro(timeUs_t currentTimeUs) {
|
|||
if (gyroConfig()->gyroSync) {
|
||||
while (true) {
|
||||
gyroUpdateUs = micros();
|
||||
if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
|
||||
if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (getLooptime() + GYRO_WATCHDOG_DELAY))) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -711,22 +711,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
flightTime += cycleTime;
|
||||
}
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
if (getAsyncMode() == ASYNC_MODE_NONE) {
|
||||
taskGyro(currentTimeUs);
|
||||
}
|
||||
|
||||
if (getAsyncMode() != ASYNC_MODE_ALL && sensors(SENSOR_ACC)) {
|
||||
imuUpdateAccelerometer();
|
||||
imuUpdateAttitude(currentTimeUs);
|
||||
}
|
||||
#else
|
||||
/* Update gyroscope */
|
||||
taskGyro(currentTimeUs);
|
||||
imuUpdateAccelerometer();
|
||||
imuUpdateAttitude(currentTimeUs);
|
||||
#endif
|
||||
|
||||
|
||||
annexCode();
|
||||
|
||||
|
|
|
@ -1080,15 +1080,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
break;
|
||||
|
||||
case MSP_INAV_PID:
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
sbufWriteU8(dst, systemConfig()->asyncMode);
|
||||
sbufWriteU16(dst, systemConfig()->accTaskFrequency);
|
||||
sbufWriteU16(dst, systemConfig()->attitudeTaskFrequency);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU16(dst, 0);
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
sbufWriteU8(dst, 0); //Legacy, no longer in use async processing value
|
||||
sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value
|
||||
sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value
|
||||
sbufWriteU8(dst, pidProfile()->heading_hold_rate_limit);
|
||||
sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ);
|
||||
sbufWriteU16(dst, mixerConfig()->yaw_jump_prevention_limit);
|
||||
|
@ -1904,15 +1898,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
|
||||
case MSP_SET_INAV_PID:
|
||||
if (dataSize >= 15) {
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
systemConfigMutable()->asyncMode = sbufReadU8(src);
|
||||
systemConfigMutable()->accTaskFrequency = sbufReadU16(src);
|
||||
systemConfigMutable()->attitudeTaskFrequency = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
sbufReadU16(src);
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
sbufReadU8(src); //Legacy, no longer in use async processing value
|
||||
sbufReadU16(src); //Legacy, no longer in use async processing value
|
||||
sbufReadU16(src); //Legacy, no longer in use async processing value
|
||||
pidProfileMutable()->heading_hold_rate_limit = sbufReadU8(src);
|
||||
sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ
|
||||
mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src);
|
||||
|
|
|
@ -250,20 +250,6 @@ void taskSyncPwmDriver(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
void taskAttitude(timeUs_t currentTimeUs)
|
||||
{
|
||||
imuUpdateAttitude(currentTimeUs);
|
||||
}
|
||||
|
||||
void taskAcc(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
||||
imuUpdateAccelerometer();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_OSD
|
||||
void taskUpdateOsd(timeUs_t currentTimeUs)
|
||||
{
|
||||
|
@ -277,27 +263,8 @@ void fcTasksInit(void)
|
|||
{
|
||||
schedulerInit();
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
rescheduleTask(TASK_PID, getPidUpdateRate());
|
||||
setTaskEnabled(TASK_PID, true);
|
||||
|
||||
if (getAsyncMode() != ASYNC_MODE_NONE) {
|
||||
rescheduleTask(TASK_GYRO, getGyroUpdateRate());
|
||||
setTaskEnabled(TASK_GYRO, true);
|
||||
}
|
||||
|
||||
if (getAsyncMode() == ASYNC_MODE_ALL && sensors(SENSOR_ACC)) {
|
||||
rescheduleTask(TASK_ACC, getAccUpdateRate());
|
||||
setTaskEnabled(TASK_ACC, true);
|
||||
|
||||
rescheduleTask(TASK_ATTI, getAttitudeUpdateRate());
|
||||
setTaskEnabled(TASK_ATTI, true);
|
||||
}
|
||||
|
||||
#else
|
||||
rescheduleTask(TASK_GYROPID, getGyroUpdateRate());
|
||||
rescheduleTask(TASK_GYROPID, getLooptime());
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
#endif
|
||||
|
||||
setTaskEnabled(TASK_SERIAL, true);
|
||||
#ifdef BEEPER
|
||||
|
@ -376,51 +343,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
.desiredPeriod = TASK_PERIOD_HZ(10), // run every 100 ms, 10Hz
|
||||
.staticPriority = TASK_PRIORITY_HIGH,
|
||||
},
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
[TASK_PID] = {
|
||||
.taskName = "PID",
|
||||
.taskFunc = taskMainPidLoop,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(500), // Run at 500Hz
|
||||
.staticPriority = TASK_PRIORITY_HIGH,
|
||||
},
|
||||
|
||||
[TASK_GYRO] = {
|
||||
.taskName = "GYRO",
|
||||
.taskFunc = taskGyro,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(1000), //Run at 1000Hz
|
||||
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||
},
|
||||
|
||||
[TASK_ACC] = {
|
||||
.taskName = "ACC",
|
||||
.taskFunc = taskAcc,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(520), //520Hz is ACC bandwidth (260Hz) * 2
|
||||
.staticPriority = TASK_PRIORITY_HIGH,
|
||||
},
|
||||
|
||||
[TASK_ATTI] = {
|
||||
.taskName = "ATTITUDE",
|
||||
.taskFunc = taskAttitude,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(60), //With acc LPF at 15Hz 60Hz attitude refresh should be enough
|
||||
.staticPriority = TASK_PRIORITY_HIGH,
|
||||
},
|
||||
|
||||
#else
|
||||
|
||||
/*
|
||||
* Legacy synchronous PID/gyro/acc/atti mode
|
||||
* for 64kB targets and other smaller targets
|
||||
*/
|
||||
|
||||
[TASK_GYROPID] = {
|
||||
.taskName = "GYRO/PID",
|
||||
.taskFunc = taskMainPidLoop,
|
||||
.desiredPeriod = TASK_PERIOD_US(1000),
|
||||
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||
},
|
||||
#endif
|
||||
|
||||
[TASK_GYROPID] = {
|
||||
.taskName = "GYRO/PID",
|
||||
.taskFunc = taskMainPidLoop,
|
||||
.desiredPeriod = TASK_PERIOD_US(1000),
|
||||
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||
},
|
||||
[TASK_SERIAL] = {
|
||||
.taskName = "SERIAL",
|
||||
.taskFunc = taskHandleSerial,
|
||||
|
|
|
@ -1626,20 +1626,6 @@ groups:
|
|||
type: bool
|
||||
- name: debug_mode
|
||||
table: debug_modes
|
||||
- name: acc_task_frequency
|
||||
field: accTaskFrequency
|
||||
condition: USE_ASYNC_GYRO_PROCESSING
|
||||
min: ACC_TASK_FREQUENCY_MIN
|
||||
max: ACC_TASK_FREQUENCY_MAX
|
||||
- name: attitude_task_frequency
|
||||
field: attitudeTaskFrequency
|
||||
condition: USE_ASYNC_GYRO_PROCESSING
|
||||
min: ATTITUDE_TASK_FREQUENCY_MIN
|
||||
max: ATTITUDE_TASK_FREQUENCY_MAX
|
||||
- name: async_mode
|
||||
field: asyncMode
|
||||
condition: USE_ASYNC_GYRO_PROCESSING
|
||||
table: async_mode
|
||||
- name: throttle_tilt_comp_str
|
||||
field: throttle_tilt_compensation_strength
|
||||
min: 0
|
||||
|
|
|
@ -214,7 +214,7 @@ void pidInit(void)
|
|||
|
||||
bool pidInitFilters(void)
|
||||
{
|
||||
const uint32_t refreshRate = getPidUpdateRate();
|
||||
const uint32_t refreshRate = getLooptime();
|
||||
notchFilterApplyFn = nullFilterApply;
|
||||
|
||||
if (refreshRate == 0) {
|
||||
|
@ -241,7 +241,7 @@ bool pidInitFilters(void)
|
|||
void pidResetTPAFilter(void)
|
||||
{
|
||||
if (STATE(FIXED_WING) && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
|
||||
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getPidUpdateRate() * 1e-6f);
|
||||
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f);
|
||||
pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -51,14 +51,7 @@ typedef struct {
|
|||
typedef enum {
|
||||
/* Actual tasks */
|
||||
TASK_SYSTEM = 0,
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
TASK_PID,
|
||||
TASK_GYRO,
|
||||
TASK_ACC,
|
||||
TASK_ATTI,
|
||||
#else
|
||||
TASK_GYROPID,
|
||||
#endif
|
||||
TASK_RX,
|
||||
TASK_SERIAL,
|
||||
TASK_BATTERY,
|
||||
|
|
|
@ -495,42 +495,14 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
|
|||
accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
|
||||
}
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
STATIC_FASTRAM float accumulatedMeasurements[XYZ_AXIS_COUNT];
|
||||
STATIC_FASTRAM int accumulatedMeasurementCount;
|
||||
|
||||
static void accUpdateAccumulatedMeasurements(void)
|
||||
{
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
accumulatedMeasurements[axis] += acc.accADCf[axis];
|
||||
}
|
||||
accumulatedMeasurementCount++;
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Calculate measured acceleration in body frame in g
|
||||
*/
|
||||
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
|
||||
{
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
if (accumulatedMeasurementCount) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
measuredAcc->v[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount;
|
||||
accumulatedMeasurements[axis] = 0;
|
||||
}
|
||||
accumulatedMeasurementCount = 0;
|
||||
}
|
||||
else {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
|
||||
}
|
||||
}
|
||||
#else
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void accUpdate(void)
|
||||
|
@ -588,9 +560,6 @@ void accUpdate(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
accUpdateAccumulatedMeasurements();
|
||||
#endif
|
||||
}
|
||||
|
||||
void accGetVibrationLevels(fpVector3_t *accVibeLevels)
|
||||
|
|
|
@ -310,7 +310,7 @@ void gyroInitFilters(void)
|
|||
gyroFilterStage2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
stage2Filter[axis] = &gyroFilterStage2[axis];
|
||||
biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getGyroUpdateRate());
|
||||
biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime());
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -319,7 +319,7 @@ void gyroInitFilters(void)
|
|||
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
softLpfFilter[axis] = &gyroFilterLPF[axis];
|
||||
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getGyroUpdateRate());
|
||||
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getLooptime());
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -328,7 +328,7 @@ void gyroInitFilters(void)
|
|||
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
notchFilter1[axis] = &gyroFilterNotch_1[axis];
|
||||
biquadFilterInitNotch(notchFilter1[axis], getGyroUpdateRate(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||
biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -338,7 +338,7 @@ void gyroInitFilters(void)
|
|||
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
notchFilter2[axis] = &gyroFilterNotch_2[axis];
|
||||
biquadFilterInitNotch(notchFilter2[axis], getGyroUpdateRate(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -404,37 +404,14 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t
|
|||
gyroCalibration->calibratingG--;
|
||||
}
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
STATIC_FASTRAM float accumulatedRates[XYZ_AXIS_COUNT];
|
||||
STATIC_FASTRAM timeUs_t accumulatedRateTimeUs;
|
||||
|
||||
static void gyroUpdateAccumulatedRates(timeDelta_t gyroUpdateDeltaUs)
|
||||
{
|
||||
accumulatedRateTimeUs += gyroUpdateDeltaUs;
|
||||
const float gyroUpdateDelta = gyroUpdateDeltaUs * 1e-6f;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
accumulatedRates[axis] += gyro.gyroADCf[axis] * gyroUpdateDelta;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Calculate rotation rate in rad/s in body frame
|
||||
*/
|
||||
void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
|
||||
{
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
const float accumulatedRateTime = accumulatedRateTimeUs * 1e-6;
|
||||
accumulatedRateTimeUs = 0;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(accumulatedRates[axis] / accumulatedRateTime);
|
||||
accumulatedRates[axis] = 0.0f;
|
||||
}
|
||||
#else
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void gyroUpdate(timeDelta_t gyroUpdateDeltaUs)
|
||||
|
@ -491,11 +468,6 @@ void gyroUpdate(timeDelta_t gyroUpdateDeltaUs)
|
|||
#endif
|
||||
gyro.gyroADCf[axis] = gyroADCf;
|
||||
}
|
||||
|
||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
||||
// Accumulate gyro readings for better IMU accuracy
|
||||
gyroUpdateAccumulatedRates(gyroUpdateDeltaUs);
|
||||
#endif
|
||||
}
|
||||
|
||||
bool gyroReadTemperature(void)
|
||||
|
|
|
@ -51,7 +51,7 @@ bool sensorsAutodetect(void)
|
|||
return false;
|
||||
}
|
||||
|
||||
accInit(getAccUpdateRate());
|
||||
accInit(getLooptime());
|
||||
|
||||
#ifdef USE_BARO
|
||||
baroInit();
|
||||
|
|
|
@ -42,7 +42,6 @@
|
|||
|
||||
void targetConfiguration(void)
|
||||
{
|
||||
systemConfigMutable()->asyncMode = ASYNC_MODE_NONE;
|
||||
mixerConfigMutable()->platformType = PLATFORM_MULTIROTOR;
|
||||
|
||||
featureSet(FEATURE_VBAT);
|
||||
|
|
|
@ -73,7 +73,6 @@
|
|||
#if (FLASH_SIZE > 128)
|
||||
#define NAV_FIXED_WING_LANDING
|
||||
#define USE_AUTOTUNE_FIXED_WING
|
||||
#define USE_ASYNC_GYRO_PROCESSING
|
||||
#define USE_DEBUG_TRACE
|
||||
#define USE_BOOTLOG
|
||||
#define BOOTLOG_DESCRIPTIONS
|
||||
|
|
|
@ -146,7 +146,7 @@ void beeper(beeperMode_e) {}
|
|||
uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };
|
||||
void addBootlogEvent6(bootLogEventCode_e eventCode, uint16_t eventFlags, uint16_t param1, uint16_t param2, uint16_t param3, uint16_t param4)
|
||||
{UNUSED(eventCode);UNUSED(eventFlags);UNUSED(param1);UNUSED(param2);UNUSED(param3);UNUSED(param4);}
|
||||
timeDelta_t getGyroUpdateRate(void) {return gyro.targetLooptime;}
|
||||
timeDelta_t getLooptime(void) {return gyro.targetLooptime;}
|
||||
void sensorsSet(uint32_t) {}
|
||||
void schedulerResetTaskStatistics(cfTaskId_e) {}
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue