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)
|
static void applySimplifiedTuningAllProfiles(void)
|
||||||
{
|
{
|
||||||
for (unsigned pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {
|
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.");
|
cliPrintLine("Applied simplified tuning.");
|
||||||
} else if (strcasecmp(cmdline, "disable") == 0) {
|
} else if (strcasecmp(cmdline, "disable") == 0) {
|
||||||
for (unsigned pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {
|
for (unsigned pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {
|
||||||
disableSimplifiedTuning(pidProfilesMutable(pidProfileIndex));
|
disableSimplifiedTuning(pidProfilesMutable(pidProfileIndex), gyroConfigMutable());
|
||||||
}
|
}
|
||||||
|
|
||||||
cliPrintLine("Disabled simplified tuning.");
|
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 = cmsx_simplified_gyro_filter;
|
||||||
gyroConfigMutable()->simplified_gyro_filter_multiplier = cmsx_simplified_gyro_filter_multiplier;
|
gyroConfigMutable()->simplified_gyro_filter_multiplier = cmsx_simplified_gyro_filter_multiplier;
|
||||||
|
|
||||||
applySimplifiedTuning(currentPidProfile);
|
applySimplifiedTuning(currentPidProfile, gyroConfigMutable());
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
|
|
@ -28,8 +28,6 @@
|
||||||
|
|
||||||
#include "config/simplified_tuning.h"
|
#include "config/simplified_tuning.h"
|
||||||
|
|
||||||
#include "sensors/gyro.h"
|
|
||||||
|
|
||||||
static void calculateNewPidValues(pidProfile_t *pidProfile)
|
static void calculateNewPidValues(pidProfile_t *pidProfile)
|
||||||
{
|
{
|
||||||
const pidf_t pidDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = {
|
const pidf_t pidDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = {
|
||||||
|
@ -77,61 +75,59 @@ static void calculateNewDTermFilterValues(pidProfile_t *pidProfile)
|
||||||
if (pidProfile->dterm_lpf2_static_hz) {
|
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);
|
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) {
|
static void calculateNewGyroFilterValues(gyroConfig_t *gyroConfig)
|
||||||
pidProfile->dterm_lpf2_type = FILTER_PT1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static void calculateNewGyroFilterValues()
|
|
||||||
{
|
{
|
||||||
if (gyroConfigMutable()->gyro_lpf1_dyn_min_hz) {
|
if (gyroConfig->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);
|
gyroConfig->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);
|
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) {
|
if (gyroConfig->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);
|
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) {
|
if (gyroConfig->gyro_lpf2_static_hz) {
|
||||||
gyroConfigMutable()->gyro_lpf2_static_hz = constrain(GYRO_LPF2_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, LPF_MAX_HZ);
|
gyroConfig->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;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void applySimplifiedTuning(pidProfile_t *pidProfile)
|
void applySimplifiedTuningPids(pidProfile_t *pidProfile)
|
||||||
{
|
{
|
||||||
if (pidProfile->simplified_pids_mode != PID_SIMPLIFIED_TUNING_OFF) {
|
if (pidProfile->simplified_pids_mode != PID_SIMPLIFIED_TUNING_OFF) {
|
||||||
calculateNewPidValues(pidProfile);
|
calculateNewPidValues(pidProfile);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void applySimplifiedTuningDtermFilters(pidProfile_t *pidProfile)
|
||||||
|
{
|
||||||
if (pidProfile->simplified_dterm_filter) {
|
if (pidProfile->simplified_dterm_filter) {
|
||||||
calculateNewDTermFilterValues(pidProfile);
|
calculateNewDTermFilterValues(pidProfile);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (gyroConfig()->simplified_gyro_filter) {
|
void applySimplifiedTuningGyroFilters(gyroConfig_t *gyroConfig)
|
||||||
calculateNewGyroFilterValues();
|
{
|
||||||
|
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_pids_mode = PID_SIMPLIFIED_TUNING_OFF;
|
||||||
|
|
||||||
pidProfile->simplified_dterm_filter = false;
|
pidProfile->simplified_dterm_filter = false;
|
||||||
|
|
||||||
gyroConfigMutable()->simplified_gyro_filter = false;
|
gyroConfig->simplified_gyro_filter = false;
|
||||||
}
|
}
|
||||||
#endif // USE_SIMPLIFIED_TUNING
|
#endif // USE_SIMPLIFIED_TUNING
|
||||||
|
|
|
@ -21,12 +21,11 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
|
#include "sensors/gyro.h"
|
||||||
|
|
||||||
#define SIMPLIFIED_TUNING_MIN 0
|
#define SIMPLIFIED_TUNING_MIN 0
|
||||||
#define SIMPLIFIED_TUNING_MAX 200
|
#define SIMPLIFIED_TUNING_MAX 200
|
||||||
#define SIMPLIFIED_TUNING_DEFAULT 100
|
#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
|
#define SIMPLIFIED_TUNING_D_DEFAULT 100
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -36,5 +35,10 @@ typedef enum {
|
||||||
PID_SIMPLIFIED_TUNING_MODE_COUNT,
|
PID_SIMPLIFIED_TUNING_MODE_COUNT,
|
||||||
} pidSimplifiedTuningMode_e;
|
} pidSimplifiedTuningMode_e;
|
||||||
|
|
||||||
void applySimplifiedTuning(pidProfile_t *pidProfile);
|
void applySimplifiedTuning(pidProfile_t *pidProfile, gyroConfig_t *gyroConfig);
|
||||||
void disableSimplifiedTuning(pidProfile_t *pidProfile);
|
|
||||||
|
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,
|
.vbat_sag_compensation = 0,
|
||||||
.simplified_pids_mode = PID_SIMPLIFIED_TUNING_RPY,
|
.simplified_pids_mode = PID_SIMPLIFIED_TUNING_RPY,
|
||||||
.simplified_master_multiplier = SIMPLIFIED_TUNING_DEFAULT,
|
.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_i_gain = SIMPLIFIED_TUNING_DEFAULT,
|
||||||
.simplified_d_gain = SIMPLIFIED_TUNING_D_DEFAULT,
|
.simplified_d_gain = SIMPLIFIED_TUNING_D_DEFAULT,
|
||||||
.simplified_pi_gain = SIMPLIFIED_TUNING_DEFAULT,
|
.simplified_pi_gain = SIMPLIFIED_TUNING_DEFAULT,
|
||||||
.simplified_dmin_ratio = SIMPLIFIED_TUNING_D_DEFAULT,
|
.simplified_dmin_ratio = SIMPLIFIED_TUNING_D_DEFAULT,
|
||||||
.simplified_feedforward_gain = SIMPLIFIED_TUNING_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 = true,
|
||||||
.simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT,
|
.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_ROLL].D = 30;
|
||||||
pidProfile->pid[PID_PITCH].D = 32;
|
pidProfile->pid[PID_PITCH].D = 32;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_SIMPLIFIED_TUNING
|
|
||||||
applySimplifiedTuning(pidProfile);
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
|
void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
|
||||||
|
|
|
@ -59,9 +59,9 @@
|
||||||
#define ITERM_ACCELERATOR_GAIN_OFF 0
|
#define ITERM_ACCELERATOR_GAIN_OFF 0
|
||||||
#define ITERM_ACCELERATOR_GAIN_MAX 30000
|
#define ITERM_ACCELERATOR_GAIN_MAX 30000
|
||||||
#define PID_ROLL_DEFAULT { 45, 80, 40, 120 }
|
#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 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_MIN_HZ_DEFAULT 75
|
||||||
#define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
|
#define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
|
||||||
|
|
|
@ -2021,6 +2021,133 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
||||||
return !unsupportedCommand;
|
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)
|
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
|
// Added in MSP API 1.44
|
||||||
case MSP_SIMPLIFIED_TUNING:
|
case MSP_SIMPLIFIED_TUNING:
|
||||||
{
|
{
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_pids_mode);
|
writeSimplifiedPids(currentPidProfile, dst);
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_master_multiplier);
|
writeSimplifiedDtermFilters(currentPidProfile, dst);
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_roll_pitch_ratio);
|
writeSimplifiedGyroFilters(gyroConfig(), dst);
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_i_gain);
|
}
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_d_gain);
|
break;
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_pi_gain);
|
|
||||||
#ifdef USE_D_MIN
|
case MSP_CALCULATE_SIMPLIFIED_PID:
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_dmin_ratio);
|
{
|
||||||
#else
|
pidProfile_t tempPidProfile = *currentPidProfile;
|
||||||
sbufWriteU8(dst, 0);
|
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
|
#endif
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_feedforward_gain);
|
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_pitch_pi_gain);
|
|
||||||
|
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter);
|
sbufWriteU8(dst, result);
|
||||||
sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter_multiplier);
|
|
||||||
|
|
||||||
sbufWriteU8(dst, gyroConfig()->simplified_gyro_filter);
|
applySimplifiedTuningDtermFilters(&tempPidProfile);
|
||||||
sbufWriteU8(dst, gyroConfig()->simplified_gyro_filter_multiplier);
|
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;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
@ -3142,28 +3326,12 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
||||||
#ifdef USE_SIMPLIFIED_TUNING
|
#ifdef USE_SIMPLIFIED_TUNING
|
||||||
// Added in MSP API 1.44
|
// Added in MSP API 1.44
|
||||||
case MSP_SET_SIMPLIFIED_TUNING:
|
case MSP_SET_SIMPLIFIED_TUNING:
|
||||||
currentPidProfile->simplified_pids_mode = sbufReadU8(src);
|
{
|
||||||
currentPidProfile->simplified_master_multiplier = sbufReadU8(src);
|
readSimplifiedPids(currentPidProfile, src);
|
||||||
currentPidProfile->simplified_roll_pitch_ratio = sbufReadU8(src);
|
readSimplifiedDtermFilters(currentPidProfile, src);
|
||||||
currentPidProfile->simplified_i_gain = sbufReadU8(src);
|
readSimplifiedGyroFilters(gyroConfigMutable(), src);
|
||||||
currentPidProfile->simplified_d_gain = sbufReadU8(src);
|
applySimplifiedTuning(currentPidProfile, gyroConfigMutable());
|
||||||
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);
|
|
||||||
|
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -287,6 +287,10 @@
|
||||||
|
|
||||||
#define MSP_SIMPLIFIED_TUNING 140 //out message Simplified tuning values and enabled state
|
#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_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_RC 200 //in message 8 rc chan
|
||||||
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
|
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
|
||||||
|
|
|
@ -23,6 +23,7 @@
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/filter.h"
|
#include "common/filter.h"
|
||||||
#include "common/time.h"
|
#include "common/time.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
#include "drivers/accgyro/accgyro.h"
|
#include "drivers/accgyro/accgyro.h"
|
||||||
#include "drivers/bus.h"
|
#include "drivers/bus.h"
|
||||||
|
|
Loading…
Reference in New Issue