Merge pull request #11114 from limonspb/msp_request_sliders_calculations
MSP commands for calculating/requesting but not setting simplified tuning/values
This commit is contained in:
commit
eb881efba1
|
@ -3099,7 +3099,7 @@ static void cliVtxInfo(const char *cmdName, char *cmdline)
|
|||
static void applySimplifiedTuningAllProfiles(void)
|
||||
{
|
||||
for (unsigned pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {
|
||||
applySimplifiedTuning(pidProfilesMutable(pidProfileIndex));
|
||||
applySimplifiedTuning(pidProfilesMutable(pidProfileIndex), gyroConfigMutable());
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -3111,7 +3111,7 @@ static void cliSimplifiedTuning(const char *cmdName, char *cmdline)
|
|||
cliPrintLine("Applied simplified tuning.");
|
||||
} else if (strcasecmp(cmdline, "disable") == 0) {
|
||||
for (unsigned pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {
|
||||
disableSimplifiedTuning(pidProfilesMutable(pidProfileIndex));
|
||||
disableSimplifiedTuning(pidProfilesMutable(pidProfileIndex), gyroConfigMutable());
|
||||
}
|
||||
|
||||
cliPrintLine("Disabled simplified tuning.");
|
||||
|
|
|
@ -314,7 +314,7 @@ static const void *cmsx_simplifiedTuningOnExit(displayPort_t *pDisp, const OSD_E
|
|||
gyroConfigMutable()->simplified_gyro_filter = cmsx_simplified_gyro_filter;
|
||||
gyroConfigMutable()->simplified_gyro_filter_multiplier = cmsx_simplified_gyro_filter_multiplier;
|
||||
|
||||
applySimplifiedTuning(currentPidProfile);
|
||||
applySimplifiedTuning(currentPidProfile, gyroConfigMutable());
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
|
@ -28,8 +28,6 @@
|
|||
|
||||
#include "config/simplified_tuning.h"
|
||||
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
static void calculateNewPidValues(pidProfile_t *pidProfile)
|
||||
{
|
||||
const pidf_t pidDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = {
|
||||
|
@ -77,61 +75,59 @@ static void calculateNewDTermFilterValues(pidProfile_t *pidProfile)
|
|||
if (pidProfile->dterm_lpf2_static_hz) {
|
||||
pidProfile->dterm_lpf2_static_hz = constrain(DTERM_LPF2_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, LPF_MAX_HZ);
|
||||
}
|
||||
|
||||
if (!pidProfile->dterm_lpf1_type) {
|
||||
pidProfile->dterm_lpf1_type = FILTER_PT1;
|
||||
}
|
||||
|
||||
if (!pidProfile->dterm_lpf2_type) {
|
||||
pidProfile->dterm_lpf2_type = FILTER_PT1;
|
||||
}
|
||||
}
|
||||
|
||||
static void calculateNewGyroFilterValues()
|
||||
static void calculateNewGyroFilterValues(gyroConfig_t *gyroConfig)
|
||||
{
|
||||
if (gyroConfigMutable()->gyro_lpf1_dyn_min_hz) {
|
||||
gyroConfigMutable()->gyro_lpf1_dyn_min_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
|
||||
gyroConfigMutable()->gyro_lpf1_dyn_max_hz = constrain(GYRO_LPF1_DYN_MAX_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
|
||||
if (gyroConfig->gyro_lpf1_dyn_min_hz) {
|
||||
gyroConfig->gyro_lpf1_dyn_min_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
|
||||
gyroConfig->gyro_lpf1_dyn_max_hz = constrain(GYRO_LPF1_DYN_MAX_HZ_DEFAULT * gyroConfig->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
|
||||
}
|
||||
|
||||
if (gyroConfigMutable()->gyro_lpf1_static_hz) {
|
||||
gyroConfigMutable()->gyro_lpf1_static_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
|
||||
if (gyroConfig->gyro_lpf1_static_hz) {
|
||||
gyroConfig->gyro_lpf1_static_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
|
||||
}
|
||||
|
||||
if (gyroConfigMutable()->gyro_lpf2_static_hz) {
|
||||
gyroConfigMutable()->gyro_lpf2_static_hz = constrain(GYRO_LPF2_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, LPF_MAX_HZ);
|
||||
}
|
||||
|
||||
if (!gyroConfigMutable()->gyro_lpf1_type) {
|
||||
gyroConfigMutable()->gyro_lpf1_type = FILTER_PT1;
|
||||
}
|
||||
|
||||
if (!gyroConfigMutable()->gyro_lpf2_type) {
|
||||
gyroConfigMutable()->gyro_lpf2_type = FILTER_PT1;
|
||||
if (gyroConfig->gyro_lpf2_static_hz) {
|
||||
gyroConfig->gyro_lpf2_static_hz = constrain(GYRO_LPF2_HZ_DEFAULT * gyroConfig->simplified_gyro_filter_multiplier / 100, 0, LPF_MAX_HZ);
|
||||
}
|
||||
}
|
||||
|
||||
void applySimplifiedTuning(pidProfile_t *pidProfile)
|
||||
void applySimplifiedTuningPids(pidProfile_t *pidProfile)
|
||||
{
|
||||
if (pidProfile->simplified_pids_mode != PID_SIMPLIFIED_TUNING_OFF) {
|
||||
calculateNewPidValues(pidProfile);
|
||||
}
|
||||
}
|
||||
|
||||
void applySimplifiedTuningDtermFilters(pidProfile_t *pidProfile)
|
||||
{
|
||||
if (pidProfile->simplified_dterm_filter) {
|
||||
calculateNewDTermFilterValues(pidProfile);
|
||||
}
|
||||
}
|
||||
|
||||
if (gyroConfig()->simplified_gyro_filter) {
|
||||
calculateNewGyroFilterValues();
|
||||
void applySimplifiedTuningGyroFilters(gyroConfig_t *gyroConfig)
|
||||
{
|
||||
if (gyroConfig->simplified_gyro_filter) {
|
||||
calculateNewGyroFilterValues(gyroConfig);
|
||||
}
|
||||
}
|
||||
|
||||
void disableSimplifiedTuning(pidProfile_t *pidProfile)
|
||||
|
||||
void applySimplifiedTuning(pidProfile_t *pidProfile, gyroConfig_t *gyroConfig)
|
||||
{
|
||||
applySimplifiedTuningPids(pidProfile);
|
||||
applySimplifiedTuningDtermFilters(pidProfile);
|
||||
applySimplifiedTuningGyroFilters(gyroConfig);
|
||||
}
|
||||
|
||||
void disableSimplifiedTuning(pidProfile_t *pidProfile, gyroConfig_t *gyroConfig)
|
||||
{
|
||||
pidProfile->simplified_pids_mode = PID_SIMPLIFIED_TUNING_OFF;
|
||||
|
||||
pidProfile->simplified_dterm_filter = false;
|
||||
|
||||
gyroConfigMutable()->simplified_gyro_filter = false;
|
||||
gyroConfig->simplified_gyro_filter = false;
|
||||
}
|
||||
#endif // USE_SIMPLIFIED_TUNING
|
||||
|
|
|
@ -21,12 +21,11 @@
|
|||
#pragma once
|
||||
|
||||
#include "flight/pid.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#define SIMPLIFIED_TUNING_MIN 0
|
||||
#define SIMPLIFIED_TUNING_MAX 200
|
||||
#define SIMPLIFIED_TUNING_DEFAULT 100
|
||||
#define SIMPLIFIED_TUNING_PITCH_P_DEFAULT 105
|
||||
#define SIMPLIFIED_TUNING_PITCH_D_DEFAULT 115
|
||||
#define SIMPLIFIED_TUNING_D_DEFAULT 100
|
||||
|
||||
typedef enum {
|
||||
|
@ -36,5 +35,10 @@ typedef enum {
|
|||
PID_SIMPLIFIED_TUNING_MODE_COUNT,
|
||||
} pidSimplifiedTuningMode_e;
|
||||
|
||||
void applySimplifiedTuning(pidProfile_t *pidProfile);
|
||||
void disableSimplifiedTuning(pidProfile_t *pidProfile);
|
||||
void applySimplifiedTuning(pidProfile_t *pidProfile, gyroConfig_t *gyroConfig);
|
||||
|
||||
void applySimplifiedTuningPids(pidProfile_t *pidProfile);
|
||||
void applySimplifiedTuningDtermFilters(pidProfile_t *pidProfile);
|
||||
void applySimplifiedTuningGyroFilters(gyroConfig_t *gyroConfig);
|
||||
|
||||
void disableSimplifiedTuning(pidProfile_t *pidProfile, gyroConfig_t *gyroConfig);
|
||||
|
|
|
@ -213,13 +213,13 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.vbat_sag_compensation = 0,
|
||||
.simplified_pids_mode = PID_SIMPLIFIED_TUNING_RPY,
|
||||
.simplified_master_multiplier = SIMPLIFIED_TUNING_DEFAULT,
|
||||
.simplified_roll_pitch_ratio = SIMPLIFIED_TUNING_PITCH_D_DEFAULT,
|
||||
.simplified_roll_pitch_ratio = SIMPLIFIED_TUNING_DEFAULT,
|
||||
.simplified_i_gain = SIMPLIFIED_TUNING_DEFAULT,
|
||||
.simplified_d_gain = SIMPLIFIED_TUNING_D_DEFAULT,
|
||||
.simplified_pi_gain = SIMPLIFIED_TUNING_DEFAULT,
|
||||
.simplified_dmin_ratio = SIMPLIFIED_TUNING_D_DEFAULT,
|
||||
.simplified_feedforward_gain = SIMPLIFIED_TUNING_DEFAULT,
|
||||
.simplified_pitch_pi_gain = SIMPLIFIED_TUNING_PITCH_P_DEFAULT,
|
||||
.simplified_pitch_pi_gain = SIMPLIFIED_TUNING_DEFAULT,
|
||||
.simplified_dterm_filter = true,
|
||||
.simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT,
|
||||
);
|
||||
|
@ -228,10 +228,6 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->pid[PID_ROLL].D = 30;
|
||||
pidProfile->pid[PID_PITCH].D = 32;
|
||||
#endif
|
||||
|
||||
#ifdef USE_SIMPLIFIED_TUNING
|
||||
applySimplifiedTuning(pidProfile);
|
||||
#endif
|
||||
}
|
||||
|
||||
void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
|
||||
|
|
|
@ -59,9 +59,9 @@
|
|||
#define ITERM_ACCELERATOR_GAIN_OFF 0
|
||||
#define ITERM_ACCELERATOR_GAIN_MAX 30000
|
||||
#define PID_ROLL_DEFAULT { 45, 80, 40, 120 }
|
||||
#define PID_PITCH_DEFAULT { 45, 80, 40, 120 }
|
||||
#define PID_PITCH_DEFAULT { 47, 84, 46, 125 }
|
||||
#define PID_YAW_DEFAULT { 45, 80, 0, 120 }
|
||||
#define D_MIN_DEFAULT { 30, 30, 0 }
|
||||
#define D_MIN_DEFAULT { 30, 34, 0 }
|
||||
|
||||
#define DTERM_LPF1_DYN_MIN_HZ_DEFAULT 75
|
||||
#define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
|
||||
|
|
|
@ -2021,6 +2021,133 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
return !unsupportedCommand;
|
||||
}
|
||||
|
||||
|
||||
#ifdef USE_SIMPLIFIED_TUNING
|
||||
// Reads simplified PID tuning values from MSP buffer
|
||||
static void readSimplifiedPids(pidProfile_t* pidProfile, sbuf_t *src)
|
||||
{
|
||||
pidProfile->simplified_pids_mode = sbufReadU8(src);
|
||||
pidProfile->simplified_master_multiplier = sbufReadU8(src);
|
||||
pidProfile->simplified_roll_pitch_ratio = sbufReadU8(src);
|
||||
pidProfile->simplified_i_gain = sbufReadU8(src);
|
||||
pidProfile->simplified_d_gain = sbufReadU8(src);
|
||||
pidProfile->simplified_pi_gain = sbufReadU8(src);
|
||||
#ifdef USE_D_MIN
|
||||
pidProfile->simplified_dmin_ratio = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
pidProfile->simplified_feedforward_gain = sbufReadU8(src);
|
||||
pidProfile->simplified_pitch_pi_gain = sbufReadU8(src);
|
||||
sbufReadU32(src); // reserved for future use
|
||||
sbufReadU32(src); // reserved for future use
|
||||
}
|
||||
|
||||
// Writes simplified PID tuning values to MSP buffer
|
||||
static void writeSimplifiedPids(const pidProfile_t *pidProfile, sbuf_t *dst)
|
||||
{
|
||||
sbufWriteU8(dst, pidProfile->simplified_pids_mode);
|
||||
sbufWriteU8(dst, pidProfile->simplified_master_multiplier);
|
||||
sbufWriteU8(dst, pidProfile->simplified_roll_pitch_ratio);
|
||||
sbufWriteU8(dst, pidProfile->simplified_i_gain);
|
||||
sbufWriteU8(dst, pidProfile->simplified_d_gain);
|
||||
sbufWriteU8(dst, pidProfile->simplified_pi_gain);
|
||||
#ifdef USE_D_MIN
|
||||
sbufWriteU8(dst, pidProfile->simplified_dmin_ratio);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
sbufWriteU8(dst, pidProfile->simplified_feedforward_gain);
|
||||
sbufWriteU8(dst, pidProfile->simplified_pitch_pi_gain);
|
||||
sbufWriteU32(dst, 0); // reserved for future use
|
||||
sbufWriteU32(dst, 0); // reserved for future use
|
||||
}
|
||||
|
||||
// Reads simplified Dterm Filter values from MSP buffer
|
||||
static void readSimplifiedDtermFilters(pidProfile_t* pidProfile, sbuf_t *src)
|
||||
{
|
||||
pidProfile->simplified_dterm_filter = sbufReadU8(src);
|
||||
pidProfile->simplified_dterm_filter_multiplier = sbufReadU8(src);
|
||||
pidProfile->dterm_lpf1_static_hz = sbufReadU16(src);
|
||||
pidProfile->dterm_lpf2_static_hz = sbufReadU16(src);
|
||||
#if defined(USE_DYN_LPF)
|
||||
pidProfile->dterm_lpf1_dyn_min_hz = sbufReadU16(src);
|
||||
pidProfile->dterm_lpf1_dyn_max_hz = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU16(src);
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
sbufReadU32(src); // reserved for future use
|
||||
sbufReadU32(src); // reserved for future use
|
||||
}
|
||||
|
||||
// Writes simplified Dterm Filter values into MSP buffer
|
||||
static void writeSimplifiedDtermFilters(const pidProfile_t* pidProfile, sbuf_t *dst)
|
||||
{
|
||||
sbufWriteU8(dst, pidProfile->simplified_dterm_filter);
|
||||
sbufWriteU8(dst, pidProfile->simplified_dterm_filter_multiplier);
|
||||
sbufWriteU16(dst, pidProfile->dterm_lpf1_static_hz);
|
||||
sbufWriteU16(dst, pidProfile->dterm_lpf2_static_hz);
|
||||
#if defined(USE_DYN_LPF)
|
||||
sbufWriteU16(dst, pidProfile->dterm_lpf1_dyn_min_hz);
|
||||
sbufWriteU16(dst, pidProfile->dterm_lpf1_dyn_max_hz);
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
sbufWriteU32(dst, 0); // reserved for future use
|
||||
sbufWriteU32(dst, 0); // reserved for future use
|
||||
}
|
||||
|
||||
// Writes simplified Gyro Filter values from MSP buffer
|
||||
static void readSimplifiedGyroFilters(gyroConfig_t *gyroConfig, sbuf_t *src)
|
||||
{
|
||||
gyroConfig->simplified_gyro_filter = sbufReadU8(src);
|
||||
gyroConfig->simplified_gyro_filter_multiplier = sbufReadU8(src);
|
||||
gyroConfig->gyro_lpf1_static_hz = sbufReadU16(src);
|
||||
gyroConfig->gyro_lpf2_static_hz = sbufReadU16(src);
|
||||
#if defined(USE_DYN_LPF)
|
||||
gyroConfig->gyro_lpf1_dyn_min_hz = sbufReadU16(src);
|
||||
gyroConfig->gyro_lpf1_dyn_max_hz = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU16(src);
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
sbufReadU32(src); // reserved for future use
|
||||
sbufReadU32(src); // reserved for future use
|
||||
}
|
||||
|
||||
// Writes simplified Gyro Filter values into MSP buffer
|
||||
static void writeSimplifiedGyroFilters(const gyroConfig_t *gyroConfig, sbuf_t *dst)
|
||||
{
|
||||
sbufWriteU8(dst, gyroConfig->simplified_gyro_filter);
|
||||
sbufWriteU8(dst, gyroConfig->simplified_gyro_filter_multiplier);
|
||||
sbufWriteU16(dst, gyroConfig->gyro_lpf1_static_hz);
|
||||
sbufWriteU16(dst, gyroConfig->gyro_lpf2_static_hz);
|
||||
#if defined(USE_DYN_LPF)
|
||||
sbufWriteU16(dst, gyroConfig->gyro_lpf1_dyn_min_hz);
|
||||
sbufWriteU16(dst, gyroConfig->gyro_lpf1_dyn_max_hz);
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
sbufWriteU32(dst, 0); // reserved for future use
|
||||
sbufWriteU32(dst, 0); // reserved for future use
|
||||
}
|
||||
|
||||
// writes results of simplified PID tuning values to MSP buffer
|
||||
static void writePidfs(pidProfile_t* pidProfile, sbuf_t *dst)
|
||||
{
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
sbufWriteU8(dst, pidProfile->pid[i].P);
|
||||
sbufWriteU8(dst, pidProfile->pid[i].I);
|
||||
sbufWriteU8(dst, pidProfile->pid[i].D);
|
||||
sbufWriteU8(dst, pidProfile->d_min[i]);
|
||||
sbufWriteU16(dst, pidProfile->pid[i].F);
|
||||
}
|
||||
}
|
||||
#endif // USE_SIMPLIFIED_TUNING
|
||||
|
||||
static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
|
||||
{
|
||||
|
||||
|
@ -2149,25 +2276,82 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
|
|||
// Added in MSP API 1.44
|
||||
case MSP_SIMPLIFIED_TUNING:
|
||||
{
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_pids_mode);
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_master_multiplier);
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_roll_pitch_ratio);
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_i_gain);
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_d_gain);
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_pi_gain);
|
||||
#ifdef USE_D_MIN
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_dmin_ratio);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
writeSimplifiedPids(currentPidProfile, dst);
|
||||
writeSimplifiedDtermFilters(currentPidProfile, dst);
|
||||
writeSimplifiedGyroFilters(gyroConfig(), dst);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CALCULATE_SIMPLIFIED_PID:
|
||||
{
|
||||
pidProfile_t tempPidProfile = *currentPidProfile;
|
||||
readSimplifiedPids(&tempPidProfile, src);
|
||||
applySimplifiedTuningPids(&tempPidProfile);
|
||||
writePidfs(&tempPidProfile, dst);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CALCULATE_SIMPLIFIED_DTERM:
|
||||
{
|
||||
pidProfile_t tempPidProfile = *currentPidProfile;
|
||||
readSimplifiedDtermFilters(&tempPidProfile, src);
|
||||
applySimplifiedTuningDtermFilters(&tempPidProfile);
|
||||
writeSimplifiedDtermFilters(&tempPidProfile, dst);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_CALCULATE_SIMPLIFIED_GYRO:
|
||||
{
|
||||
gyroConfig_t tempGyroConfig = *gyroConfig();
|
||||
readSimplifiedGyroFilters(&tempGyroConfig, src);
|
||||
applySimplifiedTuningGyroFilters(&tempGyroConfig);
|
||||
writeSimplifiedGyroFilters(&tempGyroConfig, dst);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_VALIDATE_SIMPLIFIED_TUNING:
|
||||
{
|
||||
pidProfile_t tempPidProfile = *currentPidProfile;
|
||||
applySimplifiedTuningPids(&tempPidProfile);
|
||||
bool result = true;
|
||||
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
result = result &&
|
||||
tempPidProfile.pid[i].P == currentPidProfile->pid[i].P &&
|
||||
tempPidProfile.pid[i].I == currentPidProfile->pid[i].I &&
|
||||
tempPidProfile.pid[i].D == currentPidProfile->pid[i].D &&
|
||||
tempPidProfile.d_min[i] == currentPidProfile->d_min[i] &&
|
||||
tempPidProfile.pid[i].F == currentPidProfile->pid[i].F;
|
||||
}
|
||||
|
||||
sbufWriteU8(dst, result);
|
||||
|
||||
gyroConfig_t tempGyroConfig = *gyroConfig();
|
||||
applySimplifiedTuningGyroFilters(&tempGyroConfig);
|
||||
result =
|
||||
tempGyroConfig.gyro_lpf1_static_hz == gyroConfig()->gyro_lpf1_static_hz &&
|
||||
tempGyroConfig.gyro_lpf2_static_hz == gyroConfig()->gyro_lpf2_static_hz;
|
||||
|
||||
#if defined(USE_DYN_LPF)
|
||||
result = result &&
|
||||
tempGyroConfig.gyro_lpf1_dyn_min_hz == gyroConfig()->gyro_lpf1_dyn_min_hz &&
|
||||
tempGyroConfig.gyro_lpf1_dyn_max_hz == gyroConfig()->gyro_lpf1_dyn_max_hz;
|
||||
#endif
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_feedforward_gain);
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_pitch_pi_gain);
|
||||
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter);
|
||||
sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter_multiplier);
|
||||
sbufWriteU8(dst, result);
|
||||
|
||||
sbufWriteU8(dst, gyroConfig()->simplified_gyro_filter);
|
||||
sbufWriteU8(dst, gyroConfig()->simplified_gyro_filter_multiplier);
|
||||
applySimplifiedTuningDtermFilters(&tempPidProfile);
|
||||
result =
|
||||
tempPidProfile.dterm_lpf1_static_hz == currentPidProfile->dterm_lpf1_static_hz &&
|
||||
tempPidProfile.dterm_lpf2_static_hz == currentPidProfile->dterm_lpf2_static_hz;
|
||||
|
||||
#if defined(USE_DYN_LPF)
|
||||
result = result &&
|
||||
tempPidProfile.dterm_lpf1_dyn_min_hz == currentPidProfile->dterm_lpf1_dyn_min_hz &&
|
||||
tempPidProfile.dterm_lpf1_dyn_max_hz == currentPidProfile->dterm_lpf1_dyn_max_hz;
|
||||
#endif
|
||||
|
||||
sbufWriteU8(dst, result);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
@ -3142,28 +3326,12 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
#ifdef USE_SIMPLIFIED_TUNING
|
||||
// Added in MSP API 1.44
|
||||
case MSP_SET_SIMPLIFIED_TUNING:
|
||||
currentPidProfile->simplified_pids_mode = sbufReadU8(src);
|
||||
currentPidProfile->simplified_master_multiplier = sbufReadU8(src);
|
||||
currentPidProfile->simplified_roll_pitch_ratio = sbufReadU8(src);
|
||||
currentPidProfile->simplified_i_gain = sbufReadU8(src);
|
||||
currentPidProfile->simplified_d_gain = sbufReadU8(src);
|
||||
currentPidProfile->simplified_pi_gain = sbufReadU8(src);
|
||||
#ifdef USE_D_MIN
|
||||
currentPidProfile->simplified_dmin_ratio = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
currentPidProfile->simplified_feedforward_gain = sbufReadU8(src);
|
||||
currentPidProfile->simplified_pitch_pi_gain = sbufReadU8(src);
|
||||
|
||||
currentPidProfile->simplified_dterm_filter = sbufReadU8(src);
|
||||
currentPidProfile->simplified_dterm_filter_multiplier = sbufReadU8(src);
|
||||
|
||||
gyroConfigMutable()->simplified_gyro_filter = sbufReadU8(src);
|
||||
gyroConfigMutable()->simplified_gyro_filter_multiplier = sbufReadU8(src);
|
||||
|
||||
applySimplifiedTuning(currentPidProfile);
|
||||
|
||||
{
|
||||
readSimplifiedPids(currentPidProfile, src);
|
||||
readSimplifiedDtermFilters(currentPidProfile, src);
|
||||
readSimplifiedGyroFilters(gyroConfigMutable(), src);
|
||||
applySimplifiedTuning(currentPidProfile, gyroConfigMutable());
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
|
|
@ -285,8 +285,12 @@
|
|||
#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data
|
||||
#define MSP_MOTOR_TELEMETRY 139 //out message Per-motor telemetry data (RPM, packet stats, ESC temp, etc.)
|
||||
|
||||
#define MSP_SIMPLIFIED_TUNING 140 //out message Simplified tuning values and enabled state
|
||||
#define MSP_SET_SIMPLIFIED_TUNING 141 //in message Set simplified tuning positions and apply the calculated tuning
|
||||
#define MSP_SIMPLIFIED_TUNING 140 //out message Simplified tuning values and enabled state
|
||||
#define MSP_SET_SIMPLIFIED_TUNING 141 //in message Set simplified tuning positions and apply the calculated tuning
|
||||
#define MSP_CALCULATE_SIMPLIFIED_PID 142 //out message Requests calculations of PID values based on sliders. Sends the calculated values back. But don't save anything to the firmware
|
||||
#define MSP_CALCULATE_SIMPLIFIED_GYRO 143 //out message Requests calculations of gyro filter values based on sliders. Sends the calculated values back. But don't save anything to the firmware
|
||||
#define MSP_CALCULATE_SIMPLIFIED_DTERM 144 //out message Requests calculations of gyro filter values based on sliders. Sends the calculated values back. But don't save anything to the firmware
|
||||
#define MSP_VALIDATE_SIMPLIFIED_TUNING 145 //out message Returns an array of true/false showing which simpligfied tuning groups are matching with value and which are not
|
||||
|
||||
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
|
||||
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
|
||||
|
|
|
@ -23,6 +23,7 @@
|
|||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/time.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/bus.h"
|
||||
|
|
Loading…
Reference in New Issue