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_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_expo", "%d", currentControlRateProfile->stabilized.rcExpo8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8);
|
BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8);
|
||||||
|
|
|
@ -2474,11 +2474,7 @@ static void cliStatus(char *cmdline)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
cliPrintf("System load: %d", averageSystemLoadPercent);
|
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);
|
const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_GYROPID);
|
||||||
#endif
|
|
||||||
const int pidRate = pidTaskDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)pidTaskDeltaTime));
|
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 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)));
|
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
|
.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,
|
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
|
||||||
.current_profile_index = 0,
|
.current_profile_index = 0,
|
||||||
|
@ -114,9 +114,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
|
||||||
.debug_mode = DEBUG_NONE,
|
.debug_mode = DEBUG_NONE,
|
||||||
.i2c_speed = I2C_SPEED_400KHZ,
|
.i2c_speed = I2C_SPEED_400KHZ,
|
||||||
.cpuUnderclock = 0,
|
.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
|
.throttle_tilt_compensation_strength = 0, // 0-100, 0 - disabled
|
||||||
.pwmRxInputFilteringMode = INPUT_FILTERING_DISABLED,
|
.pwmRxInputFilteringMode = INPUT_FILTERING_DISABLED,
|
||||||
.name = { 0 }
|
.name = { 0 }
|
||||||
|
@ -152,52 +149,9 @@ void validateNavConfig(void)
|
||||||
#define SECOND_PORT_INDEX 1
|
#define SECOND_PORT_INDEX 1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
uint32_t getPidUpdateRate(void)
|
uint32_t getLooptime(void) {
|
||||||
{
|
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
|
||||||
if (systemConfig()->asyncMode == ASYNC_MODE_NONE) {
|
|
||||||
return getGyroUpdateRate();
|
|
||||||
} else {
|
|
||||||
return gyroConfig()->looptime;
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
return gyro.targetLooptime;
|
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)
|
void validateAndFixConfig(void)
|
||||||
{
|
{
|
||||||
|
@ -249,15 +203,6 @@ void validateAndFixConfig(void)
|
||||||
featureClear(FEATURE_SOFTSERIAL);
|
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 defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
|
||||||
if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) {
|
if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) {
|
||||||
const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
|
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)
|
#if !defined(USE_MPU_DATA_READY_SIGNAL)
|
||||||
gyroConfigMutable()->gyroSync = false;
|
gyroConfigMutable()->gyroSync = false;
|
||||||
systemConfigMutable()->asyncMode = ASYNC_MODE_NONE;
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (settingsValidate(NULL)) {
|
if (settingsValidate(NULL)) {
|
||||||
|
|
|
@ -29,13 +29,6 @@
|
||||||
#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
|
#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
|
||||||
#define MAX_NAME_LENGTH 16
|
#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 {
|
typedef enum {
|
||||||
ASYNC_MODE_NONE,
|
ASYNC_MODE_NONE,
|
||||||
ASYNC_MODE_GYRO,
|
ASYNC_MODE_GYRO,
|
||||||
|
@ -78,11 +71,8 @@ typedef enum {
|
||||||
} features_e;
|
} features_e;
|
||||||
|
|
||||||
typedef struct systemConfig_s {
|
typedef struct systemConfig_s {
|
||||||
uint16_t accTaskFrequency;
|
|
||||||
uint16_t attitudeTaskFrequency;
|
|
||||||
uint8_t current_profile_index;
|
uint8_t current_profile_index;
|
||||||
uint8_t current_battery_profile_index;
|
uint8_t current_battery_profile_index;
|
||||||
uint8_t asyncMode;
|
|
||||||
uint8_t debug_mode;
|
uint8_t debug_mode;
|
||||||
uint8_t i2c_speed;
|
uint8_t i2c_speed;
|
||||||
uint8_t cpuUnderclock;
|
uint8_t cpuUnderclock;
|
||||||
|
@ -146,10 +136,4 @@ void createDefaultConfig(void);
|
||||||
void resetConfigs(void);
|
void resetConfigs(void);
|
||||||
void targetConfiguration(void);
|
void targetConfiguration(void);
|
||||||
|
|
||||||
uint32_t getPidUpdateRate(void);
|
uint32_t getLooptime(void);
|
||||||
timeDelta_t getGyroUpdateRate(void);
|
|
||||||
uint16_t getAccUpdateRate(void);
|
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
|
||||||
uint16_t getAttitudeUpdateRate(void);
|
|
||||||
uint8_t getAsyncMode(void);
|
|
||||||
#endif
|
|
||||||
|
|
|
@ -638,7 +638,7 @@ void filterRc(bool isRXDataNew)
|
||||||
|
|
||||||
// Calculate average cycle time (1Hz LPF on cycle time)
|
// Calculate average cycle time (1Hz LPF on cycle time)
|
||||||
if (!filterInitialised) {
|
if (!filterInitialised) {
|
||||||
biquadFilterInitLPF(&filteredCycleTimeState, 1, getPidUpdateRate());
|
biquadFilterInitLPF(&filteredCycleTimeState, 1, getLooptime());
|
||||||
filterInitialised = true;
|
filterInitialised = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -676,7 +676,7 @@ void taskGyro(timeUs_t currentTimeUs) {
|
||||||
if (gyroConfig()->gyroSync) {
|
if (gyroConfig()->gyroSync) {
|
||||||
while (true) {
|
while (true) {
|
||||||
gyroUpdateUs = micros();
|
gyroUpdateUs = micros();
|
||||||
if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
|
if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (getLooptime() + GYRO_WATCHDOG_DELAY))) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -711,22 +711,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
||||||
flightTime += cycleTime;
|
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);
|
taskGyro(currentTimeUs);
|
||||||
imuUpdateAccelerometer();
|
imuUpdateAccelerometer();
|
||||||
imuUpdateAttitude(currentTimeUs);
|
imuUpdateAttitude(currentTimeUs);
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
annexCode();
|
annexCode();
|
||||||
|
|
||||||
|
|
|
@ -1080,15 +1080,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_INAV_PID:
|
case MSP_INAV_PID:
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
sbufWriteU8(dst, 0); //Legacy, no longer in use async processing value
|
||||||
sbufWriteU8(dst, systemConfig()->asyncMode);
|
sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value
|
||||||
sbufWriteU16(dst, systemConfig()->accTaskFrequency);
|
sbufWriteU16(dst, 0); //Legacy, no longer in use async processing value
|
||||||
sbufWriteU16(dst, systemConfig()->attitudeTaskFrequency);
|
|
||||||
#else
|
|
||||||
sbufWriteU8(dst, 0);
|
|
||||||
sbufWriteU16(dst, 0);
|
|
||||||
sbufWriteU16(dst, 0);
|
|
||||||
#endif
|
|
||||||
sbufWriteU8(dst, pidProfile()->heading_hold_rate_limit);
|
sbufWriteU8(dst, pidProfile()->heading_hold_rate_limit);
|
||||||
sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ);
|
sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ);
|
||||||
sbufWriteU16(dst, mixerConfig()->yaw_jump_prevention_limit);
|
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:
|
case MSP_SET_INAV_PID:
|
||||||
if (dataSize >= 15) {
|
if (dataSize >= 15) {
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
sbufReadU8(src); //Legacy, no longer in use async processing value
|
||||||
systemConfigMutable()->asyncMode = sbufReadU8(src);
|
sbufReadU16(src); //Legacy, no longer in use async processing value
|
||||||
systemConfigMutable()->accTaskFrequency = sbufReadU16(src);
|
sbufReadU16(src); //Legacy, no longer in use async processing value
|
||||||
systemConfigMutable()->attitudeTaskFrequency = sbufReadU16(src);
|
|
||||||
#else
|
|
||||||
sbufReadU8(src);
|
|
||||||
sbufReadU16(src);
|
|
||||||
sbufReadU16(src);
|
|
||||||
#endif
|
|
||||||
pidProfileMutable()->heading_hold_rate_limit = sbufReadU8(src);
|
pidProfileMutable()->heading_hold_rate_limit = sbufReadU8(src);
|
||||||
sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ
|
sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ
|
||||||
mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src);
|
mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src);
|
||||||
|
|
|
@ -250,20 +250,6 @@ void taskSyncPwmDriver(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
#endif
|
#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
|
#ifdef USE_OSD
|
||||||
void taskUpdateOsd(timeUs_t currentTimeUs)
|
void taskUpdateOsd(timeUs_t currentTimeUs)
|
||||||
{
|
{
|
||||||
|
@ -277,27 +263,8 @@ void fcTasksInit(void)
|
||||||
{
|
{
|
||||||
schedulerInit();
|
schedulerInit();
|
||||||
|
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
rescheduleTask(TASK_GYROPID, getLooptime());
|
||||||
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());
|
|
||||||
setTaskEnabled(TASK_GYROPID, true);
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
#endif
|
|
||||||
|
|
||||||
setTaskEnabled(TASK_SERIAL, true);
|
setTaskEnabled(TASK_SERIAL, true);
|
||||||
#ifdef BEEPER
|
#ifdef BEEPER
|
||||||
|
@ -376,51 +343,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
||||||
.desiredPeriod = TASK_PERIOD_HZ(10), // run every 100 ms, 10Hz
|
.desiredPeriod = TASK_PERIOD_HZ(10), // run every 100 ms, 10Hz
|
||||||
.staticPriority = TASK_PRIORITY_HIGH,
|
.staticPriority = TASK_PRIORITY_HIGH,
|
||||||
},
|
},
|
||||||
|
[TASK_GYROPID] = {
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
.taskName = "GYRO/PID",
|
||||||
[TASK_PID] = {
|
.taskFunc = taskMainPidLoop,
|
||||||
.taskName = "PID",
|
.desiredPeriod = TASK_PERIOD_US(1000),
|
||||||
.taskFunc = taskMainPidLoop,
|
.staticPriority = TASK_PRIORITY_REALTIME,
|
||||||
.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_SERIAL] = {
|
[TASK_SERIAL] = {
|
||||||
.taskName = "SERIAL",
|
.taskName = "SERIAL",
|
||||||
.taskFunc = taskHandleSerial,
|
.taskFunc = taskHandleSerial,
|
||||||
|
|
|
@ -1626,20 +1626,6 @@ groups:
|
||||||
type: bool
|
type: bool
|
||||||
- name: debug_mode
|
- name: debug_mode
|
||||||
table: debug_modes
|
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
|
- name: throttle_tilt_comp_str
|
||||||
field: throttle_tilt_compensation_strength
|
field: throttle_tilt_compensation_strength
|
||||||
min: 0
|
min: 0
|
||||||
|
|
|
@ -214,7 +214,7 @@ void pidInit(void)
|
||||||
|
|
||||||
bool pidInitFilters(void)
|
bool pidInitFilters(void)
|
||||||
{
|
{
|
||||||
const uint32_t refreshRate = getPidUpdateRate();
|
const uint32_t refreshRate = getLooptime();
|
||||||
notchFilterApplyFn = nullFilterApply;
|
notchFilterApplyFn = nullFilterApply;
|
||||||
|
|
||||||
if (refreshRate == 0) {
|
if (refreshRate == 0) {
|
||||||
|
@ -241,7 +241,7 @@ bool pidInitFilters(void)
|
||||||
void pidResetTPAFilter(void)
|
void pidResetTPAFilter(void)
|
||||||
{
|
{
|
||||||
if (STATE(FIXED_WING) && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
|
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);
|
pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -51,14 +51,7 @@ typedef struct {
|
||||||
typedef enum {
|
typedef enum {
|
||||||
/* Actual tasks */
|
/* Actual tasks */
|
||||||
TASK_SYSTEM = 0,
|
TASK_SYSTEM = 0,
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
|
||||||
TASK_PID,
|
|
||||||
TASK_GYRO,
|
|
||||||
TASK_ACC,
|
|
||||||
TASK_ATTI,
|
|
||||||
#else
|
|
||||||
TASK_GYROPID,
|
TASK_GYROPID,
|
||||||
#endif
|
|
||||||
TASK_RX,
|
TASK_RX,
|
||||||
TASK_SERIAL,
|
TASK_SERIAL,
|
||||||
TASK_BATTERY,
|
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;
|
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
|
* Calculate measured acceleration in body frame in g
|
||||||
*/
|
*/
|
||||||
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
|
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++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
|
measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void accUpdate(void)
|
void accUpdate(void)
|
||||||
|
@ -588,9 +560,6 @@ void accUpdate(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
|
||||||
accUpdateAccumulatedMeasurements();
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void accGetVibrationLevels(fpVector3_t *accVibeLevels)
|
void accGetVibrationLevels(fpVector3_t *accVibeLevels)
|
||||||
|
|
|
@ -310,7 +310,7 @@ void gyroInitFilters(void)
|
||||||
gyroFilterStage2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
gyroFilterStage2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
stage2Filter[axis] = &gyroFilterStage2[axis];
|
stage2Filter[axis] = &gyroFilterStage2[axis];
|
||||||
biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getGyroUpdateRate());
|
biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -319,7 +319,7 @@ void gyroInitFilters(void)
|
||||||
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
softLpfFilter[axis] = &gyroFilterLPF[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;
|
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
notchFilter1[axis] = &gyroFilterNotch_1[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
|
#endif
|
||||||
|
@ -338,7 +338,7 @@ void gyroInitFilters(void)
|
||||||
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
notchFilter2[axis] = &gyroFilterNotch_2[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
|
#endif
|
||||||
|
@ -404,37 +404,14 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t
|
||||||
gyroCalibration->calibratingG--;
|
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
|
* Calculate rotation rate in rad/s in body frame
|
||||||
*/
|
*/
|
||||||
void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
|
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++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]);
|
measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]);
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void gyroUpdate(timeDelta_t gyroUpdateDeltaUs)
|
void gyroUpdate(timeDelta_t gyroUpdateDeltaUs)
|
||||||
|
@ -491,11 +468,6 @@ void gyroUpdate(timeDelta_t gyroUpdateDeltaUs)
|
||||||
#endif
|
#endif
|
||||||
gyro.gyroADCf[axis] = gyroADCf;
|
gyro.gyroADCf[axis] = gyroADCf;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_ASYNC_GYRO_PROCESSING
|
|
||||||
// Accumulate gyro readings for better IMU accuracy
|
|
||||||
gyroUpdateAccumulatedRates(gyroUpdateDeltaUs);
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool gyroReadTemperature(void)
|
bool gyroReadTemperature(void)
|
||||||
|
|
|
@ -51,7 +51,7 @@ bool sensorsAutodetect(void)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
accInit(getAccUpdateRate());
|
accInit(getLooptime());
|
||||||
|
|
||||||
#ifdef USE_BARO
|
#ifdef USE_BARO
|
||||||
baroInit();
|
baroInit();
|
||||||
|
|
|
@ -42,7 +42,6 @@
|
||||||
|
|
||||||
void targetConfiguration(void)
|
void targetConfiguration(void)
|
||||||
{
|
{
|
||||||
systemConfigMutable()->asyncMode = ASYNC_MODE_NONE;
|
|
||||||
mixerConfigMutable()->platformType = PLATFORM_MULTIROTOR;
|
mixerConfigMutable()->platformType = PLATFORM_MULTIROTOR;
|
||||||
|
|
||||||
featureSet(FEATURE_VBAT);
|
featureSet(FEATURE_VBAT);
|
||||||
|
|
|
@ -73,7 +73,6 @@
|
||||||
#if (FLASH_SIZE > 128)
|
#if (FLASH_SIZE > 128)
|
||||||
#define NAV_FIXED_WING_LANDING
|
#define NAV_FIXED_WING_LANDING
|
||||||
#define USE_AUTOTUNE_FIXED_WING
|
#define USE_AUTOTUNE_FIXED_WING
|
||||||
#define USE_ASYNC_GYRO_PROCESSING
|
|
||||||
#define USE_DEBUG_TRACE
|
#define USE_DEBUG_TRACE
|
||||||
#define USE_BOOTLOG
|
#define USE_BOOTLOG
|
||||||
#define BOOTLOG_DESCRIPTIONS
|
#define BOOTLOG_DESCRIPTIONS
|
||||||
|
|
|
@ -146,7 +146,7 @@ void beeper(beeperMode_e) {}
|
||||||
uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };
|
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)
|
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);}
|
{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 sensorsSet(uint32_t) {}
|
||||||
void schedulerResetTaskStatistics(cfTaskId_e) {}
|
void schedulerResetTaskStatistics(cfTaskId_e) {}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue