From d5c31accdf37a5396303e3790464054d6f949bc5 Mon Sep 17 00:00:00 2001 From: Ivan Efimov Date: Sat, 18 Dec 2021 15:28:19 -0600 Subject: [PATCH] FW sliders: separate MSP commands for requesting calculations but not applying them into the FW --- src/main/cli/cli.c | 4 +- src/main/cms/cms_menu_imu.c | 2 +- src/main/config/simplified_tuning.c | 58 +++---- src/main/config/simplified_tuning.h | 12 +- src/main/flight/pid.c | 8 +- src/main/flight/pid.h | 4 +- src/main/msp/msp.c | 244 +++++++++++++++++++++++----- src/main/msp/msp_protocol.h | 8 +- src/main/sensors/gyro.h | 1 + 9 files changed, 255 insertions(+), 86 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index f81d7e1a6..5d2440097 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -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."); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index da4152aa3..67bcb9e97 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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; diff --git a/src/main/config/simplified_tuning.c b/src/main/config/simplified_tuning.c index 167f0120d..33e77dc6b 100644 --- a/src/main/config/simplified_tuning.c +++ b/src/main/config/simplified_tuning.c @@ -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 diff --git a/src/main/config/simplified_tuning.h b/src/main/config/simplified_tuning.h index 3d2ee3b6b..4905ed1f7 100644 --- a/src/main/config/simplified_tuning.h +++ b/src/main/config/simplified_tuning.h @@ -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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1a332a79c..9624a1ab5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 98f2de81c..7b49f5603 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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 diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 744259893..9ceae2bfd 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -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 diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 7f4bc7f6b..7cd59b94a 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -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 diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 241ea1267..b3577ba16 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -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"