Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

PaPaYoU - Introducing LUA and OSD Updates to utilize the new Filter parameters #51

Closed
wants to merge 8 commits into from
Closed

PaPaYoU - Introducing LUA and OSD Updates to utilize the new Filter parameters #51

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
b9b0f7e
imuf LUA
Mar 10, 2018
35aa27a
Version patch 3.4.3
Mar 11, 2018
c0199ab
ADD Imuf OSD settings
Mar 17, 2018
6db08c1
Add ifdef USE_GYRO_IMUF9001
Mar 22, 2018
3d4e9c5
Merge branch 'master' of https://github.com/ButterFlight/butterflight…
Mar 22, 2018
4be2466
ADD #if defined(USE_GYRO_IMUF9001)
Mar 23, 2018
37b4efc
push master in LUA
Mar 23, 2018
085e8e2
ADD KD_FILTER typo
Mar 23, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/build/version.h
View file
Open in desktop
Expand Up @@ -22,7 +22,7 @@
#define FC_FIRMWARE_NAME "ButterFlight"
#define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 4 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed
#define FC_VERSION_PATCH_LEVEL 3 // increment when a bug is fixed

#define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL)

Expand Down
96 changes: 96 additions & 0 deletions src/main/cms/cms_menu_imu.c
View file
Open in desktop
Expand Up @@ -358,6 +358,99 @@ static CMS_Menu cmsx_menuFilterGlobal = {
.entries = cmsx_menuFilterGlobalEntries,
};

//
// SPRING Imuf
//

#if defined(USE_GYRO_IMUF9001)
static uint16_t gyroConfig_imuf_mode;
static uint16_t gyroConfig_imuf_pitch_q;
static uint16_t gyroConfig_imuf_pitch_r;
static uint16_t gyroConfig_imuf_roll_q;
static uint16_t gyroConfig_imuf_roll_r;
static uint16_t gyroConfig_imuf_yaw_q;
static uint16_t gyroConfig_imuf_yaw_r;
static uint16_t gyroConfig_imuf_pitch_lpf_cutoff_hz;
static uint16_t gyroConfig_imuf_roll_lpf_cutoff_hz;
static uint16_t gyroConfig_imuf_yaw_lpf_cutoff_hz;
static uint16_t gyroConfig_imuf_dyn_gain;
#endif

#if defined(USE_GYRO_IMUF9001)
static long cmsx_menuImuf_onEnter(void)
{
gyroConfig_imuf_mode = gyroConfig()->imuf_mode;
gyroConfig_imuf_pitch_q = gyroConfig()->imuf_pitch_q;
gyroConfig_imuf_pitch_r = gyroConfig()->imuf_pitch_r;
gyroConfig_imuf_roll_q = gyroConfig()->imuf_roll_q;
gyroConfig_imuf_roll_r = gyroConfig()->imuf_roll_r;
gyroConfig_imuf_yaw_q = gyroConfig()->imuf_yaw_q;
gyroConfig_imuf_yaw_r = gyroConfig()->imuf_yaw_r;
gyroConfig_imuf_pitch_lpf_cutoff_hz = gyroConfig()->imuf_pitch_lpf_cutoff_hz;
gyroConfig_imuf_roll_lpf_cutoff_hz = gyroConfig()->imuf_roll_lpf_cutoff_hz;
gyroConfig_imuf_yaw_lpf_cutoff_hz = gyroConfig()->imuf_yaw_lpf_cutoff_hz;
gyroConfig_imuf_dyn_gain = gyroConfig()->imuf_dyn_gain;

return 0;
}
#endif

#if defined(USE_GYRO_IMUF9001)
static long cmsx_menuImuf_onExit(const OSD_Entry *self)
{
UNUSED(self);

gyroConfigMutable()->imuf_mode = gyroConfig_imuf_mode;
gyroConfigMutable()->imuf_pitch_q = gyroConfig_imuf_pitch_q;
gyroConfigMutable()->imuf_pitch_r = gyroConfig_imuf_pitch_r;
gyroConfigMutable()->imuf_roll_q = gyroConfig_imuf_roll_q;
gyroConfigMutable()->imuf_roll_r = gyroConfig_imuf_roll_r;
gyroConfigMutable()->imuf_yaw_q = gyroConfig_imuf_yaw_q;
gyroConfigMutable()->imuf_yaw_r = gyroConfig_imuf_yaw_r;
gyroConfigMutable()->imuf_pitch_lpf_cutoff_hz = gyroConfig_imuf_pitch_lpf_cutoff_hz;
gyroConfigMutable()->imuf_roll_lpf_cutoff_hz = gyroConfig_imuf_roll_lpf_cutoff_hz;
gyroConfigMutable()->imuf_yaw_lpf_cutoff_hz = gyroConfig_imuf_yaw_lpf_cutoff_hz;
gyroConfigMutable()->imuf_dyn_gain = gyroConfig_imuf_dyn_gain;


return 0;
}
#endif

#if defined(USE_GYRO_IMUF9001)
static OSD_Entry cmsx_menuImufEntries[] =
{
{ "-- SPRING_IMUF --", OME_Label, NULL, NULL, 0 },

{ "MODE", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_mode, 0, 255, 1 }, 0 },
{ "PITCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_q, 0, 16000, 50 }, 0 },
{ "PITCH R", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_r, 0, 16000, 1 }, 0 },
{ "ROLL Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q, 0, 16000, 50 }, 0 },
{ "ROLL R", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_r, 0, 16000, 1 }, 0 },
{ "YAW Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_q, 0, 16000, 50 }, 0 },
{ "YAW R", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_r, 0, 16000, 1 }, 0 },
{ "PITCH LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_lpf_cutoff_hz, 0, 255, 1 }, 0 },
{ "ROLL LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_lpf_cutoff_hz, 0, 255, 1 }, 0 },
{ "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_lpf_cutoff_hz, 0, 255, 1 }, 0 },
{ "GAIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_dyn_gain, 0, 1000, 1 }, 0 },

{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
};
#endif

#if defined(USE_GYRO_IMUF9001)
static CMS_Menu cmsx_menuImuf = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XIMUF",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_menuImuf_onEnter,
.onExit = cmsx_menuImuf_onExit,
.entries = cmsx_menuImufEntries,
};
#endif

static uint16_t cmsx_dterm_lpf_hz;
static uint16_t cmsx_dterm_notch_hz;
static uint16_t cmsx_dterm_notch_cutoff;
Expand Down Expand Up @@ -495,6 +588,9 @@ static OSD_Entry cmsx_menuImuEntries[] =
{"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0},

{"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0},
#if defined(USE_GYRO_IMUF9001)
{"IMUF", OME_Submenu, cmsMenuChange, &cmsx_menuImuf, 0},
#endif
#ifdef USE_COPY_PROFILE_CMS_MENU
{"COPY PROF", OME_Submenu, cmsMenuChange, &cmsx_menuCopyProfile, 0},
#endif
Expand Down
6 changes: 3 additions & 3 deletions src/main/common/filter.h
View file
Open in desktop
Expand Up @@ -70,9 +70,9 @@ typedef enum {
} filterType_e;

typedef enum {
KD_FILTER_CLASSIC = 0,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hate to do this to ya PaPaYou but I think we should revert this change and keep them as KD_FILTER_XXXX - just so the code clarity is improved.

SP / CLASSIC / NOSP are fine for people familiar with the genesis of these features but for anyone else that comes in on this code it might be confusing. The KD_FILTER add some clarity by making it immeditaly obvious that its dealing with D term filters.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes sure no problem

KD_FILTER_SP,
KD_FILTER_NOSP,
CLASSIC = 0, //kd filter
SP,
NOSP,
} kdFilterStyle_e;

typedef enum {
Expand Down
12 changes: 6 additions & 6 deletions src/main/flight/pid.c
View file
Open in desktop
Expand Up @@ -108,7 +108,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dterm_notch_hz = 260,
.dterm_notch_cutoff = 160,
.dterm_filter_type = FILTER_PT1,
.dterm_filter_style = KD_FILTER_CLASSIC,
.dterm_filter_style = CLASSIC,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Revert to KD_FILTER_CLASSIC

.itermWindupPointPercent = 50,
.vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON,
Expand Down Expand Up @@ -511,28 +511,28 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
if (axis != FD_YAW) {
// apply filters
float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate);
if (pidProfile->dterm_filter_style == KD_FILTER_CLASSIC)
if (pidProfile->dterm_filter_style == CLASSIC)
{
gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered);
}

const float rD = dynCd * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f) * currentPidSetpoint - gyroRateFiltered; // cr - y
const float pureRD = dynCd * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f) * getSetpointRate(axis) - gyroRateFiltered; // cr - y

float delta = 0.0f;
float iDT = 1.0f/deltaT; //divide once

switch (pidProfile->dterm_filter_style) {
case KD_FILTER_CLASSIC:
case CLASSIC:
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Revert cases to KD_FILTER_XXXX variant

delta = (rD - previousRateError[axis]) * iDT;
previousRateError[axis] = rD;
break;
case KD_FILTER_SP:
case SP:
//filter Kd properly along with sp
delta = dtermLpfApplyFn(dtermFilterLpf[axis], (rD - previousRateError[axis]) * iDT );
previousRateError[axis] = rD;
break;
case KD_FILTER_NOSP:
case NOSP:
//filter Kd properly, no sp
delta = dtermLpfApplyFn(dtermFilterLpf[axis], (pureRD - previousRateError[axis]) * iDT );
previousRateError[axis] = pureRD;
Expand Down
49 changes: 40 additions & 9 deletions src/main/interface/msp.c
View file
Open in desktop
Expand Up @@ -1178,16 +1178,32 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
sbufWriteU8(dst, currentPidProfile->dterm_filter_style);
break;

#ifdef USE_GYRO_FAST_KALMAN
case MSP_ADVANCED_FILTER_CONFIG :
sbufWriteU16(dst, gyroConfig()->gyro_soft_lpf_hz_2);
case MSP_FAST_KALMAN :
sbufWriteU16(dst, gyroConfig()->gyro_filter_q);
sbufWriteU16(dst, gyroConfig()->gyro_filter_r);
sbufWriteU16(dst, gyroConfig()->gyro_filter_p);
sbufWriteU8(dst, gyroConfig()->gyro_stage2_filter_type);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we not need gyro_stage2_filter_type anymore?

break;
#endif

#ifdef USE_GYRO_IMUF9001
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Something whack going on with the TABS on this area - could we please get that harmonized to line up with the rest?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

case MSP_IMUF_CONFIG :
sbufWriteU16(dst, gyroConfig()->imuf_mode);
sbufWriteU16(dst, gyroConfig()->imuf_pitch_q);
sbufWriteU16(dst, gyroConfig()->imuf_pitch_r);
sbufWriteU16(dst, gyroConfig()->imuf_roll_q);
sbufWriteU16(dst, gyroConfig()->imuf_roll_r);
sbufWriteU16(dst, gyroConfig()->imuf_yaw_q);
sbufWriteU16(dst, gyroConfig()->imuf_yaw_r);
sbufWriteU16(dst, gyroConfig()->imuf_pitch_lpf_cutoff_hz);
sbufWriteU16(dst, gyroConfig()->imuf_roll_lpf_cutoff_hz);
sbufWriteU16(dst, gyroConfig()->imuf_yaw_lpf_cutoff_hz);
sbufWriteU16(dst, gyroConfig()->imuf_dyn_gain);
break;
#endif

case MSP_PID_ADVANCED:
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
Expand Down Expand Up @@ -1647,22 +1663,37 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
}
if (sbufBytesRemaining(src) >= 1) {
if (sbufBytesRemaining(src) >= 2) {
currentPidProfile->dterm_filter_type = sbufReadU8(src);
currentPidProfile->dterm_filter_style = sbufReadU8(src);
}
// reinitialize the gyro filters with the new values
validateAndFixGyroConfig();
gyroInitFilters();
// reinitialize the PID filters with the new values
pidInitFilters(currentPidProfile);
break;

#ifdef USE_GYRO_FAST_KALMAN
case MSP_SET_ADVANCED_FILTER_CONFIG :
gyroConfigMutable()->gyro_soft_lpf_hz_2 = sbufReadU16(src);
case MSP_SET_FAST_KALMAN :
gyroConfigMutable()->gyro_filter_q = sbufReadU16(src);
gyroConfigMutable()->gyro_filter_r = sbufReadU16(src);
gyroConfigMutable()->gyro_filter_p = sbufReadU16(src);
gyroConfigMutable()->gyro_stage2_filter_type = sbufReadU8(src);
break;
#endif

#ifdef USE_GYRO_IMUF9001
case MSP_SET_IMUF_CONFIG :
gyroConfigMutable()->imuf_mode = sbufReadU16(src);
gyroConfigMutable()->imuf_pitch_q = sbufReadU16(src);
gyroConfigMutable()->imuf_pitch_r = sbufReadU16(src);
gyroConfigMutable()->imuf_roll_q = sbufReadU16(src);
gyroConfigMutable()->imuf_roll_r = sbufReadU16(src);
gyroConfigMutable()->imuf_yaw_q = sbufReadU16(src);
gyroConfigMutable()->imuf_yaw_r = sbufReadU16(src);
gyroConfigMutable()->imuf_pitch_lpf_cutoff_hz = sbufReadU16(src);
gyroConfigMutable()->imuf_roll_lpf_cutoff_hz = sbufReadU16(src);
gyroConfigMutable()->imuf_yaw_lpf_cutoff_hz = sbufReadU16(src);
gyroConfigMutable()->imuf_dyn_gain = sbufReadU16(src);
break;
#endif
case MSP_SET_PID_ADVANCED:
Expand Down
8 changes: 5 additions & 3 deletions src/main/interface/msp_protocol.h
View file
Open in desktop
Expand Up @@ -327,6 +327,8 @@
#define MSP_SET_RTC 246 //in message Sets the RTC clock
#define MSP_RTC 247 //out message Gets the RTC clock

// additionnal advanced filter settings
#define MSP_ADVANCED_FILTER_CONFIG 225 //out message
#define MSP_SET_ADVANCED_FILTER_CONFIG 226 //in message
// ButterFlight
#define MSP_FAST_KALMAN 225 //out message
#define MSP_SET_FAST_KALMAN 226 //in message
#define MSP_IMUF_CONFIG 227 //out message
#define MSP_SET_IMUF_CONFIG 228 //in message
2 changes: 1 addition & 1 deletion src/main/interface/settings.c
View file
Open in desktop
Expand Up @@ -250,7 +250,7 @@ static const char * const lookupTableLowpassType[] = {
};

static const char * const lookupTableKdStyle[] = {
"KD_FILTER_CLASSIC", "KD_FILTER_SP", "KD_FILTER_NOSP"
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please revert for clarity =D

"CLASSIC", "SP", "NOSP"
};

static const char * const lookupTableFailsafe[] = {
Expand Down
35 changes: 17 additions & 18 deletions src/main/target/HELIOSPRING/config.c
View file
Open in desktop
Expand Up @@ -48,29 +48,29 @@ void targetConfiguration(void) {

#if defined(HELIO_RACE)
//optimizng for strech-x
pidProfile->pid[PID_PITCH].P = 30;
pidProfile->pid[PID_PITCH].I = 60;
pidProfile->pid[PID_PITCH].D = 17;
pidProfile->pid[PID_ROLL].P = 28;
pidProfile->pid[PID_ROLL].I = 70;
pidProfile->pid[PID_PITCH].P = 30;
pidProfile->pid[PID_PITCH].I = 60;
pidProfile->pid[PID_PITCH].D = 17;
pidProfile->pid[PID_ROLL].P = 28;
pidProfile->pid[PID_ROLL].I = 70;
pidProfile->pid[PID_ROLL].D = 20;
#elif defined(HELIO_FREESTYLE)
//optimizng for squished-x
pidProfile->pid[PID_PITCH].P = 40;
pidProfile->pid[PID_PITCH].I = 55;
pidProfile->pid[PID_PITCH].D = 27;
pidProfile->pid[PID_ROLL].P = 43;
pidProfile->pid[PID_ROLL].I = 45;
pidProfile->pid[PID_PITCH].P = 40;
pidProfile->pid[PID_PITCH].I = 55;
pidProfile->pid[PID_PITCH].D = 27;
pidProfile->pid[PID_ROLL].P = 43;
pidProfile->pid[PID_ROLL].I = 45;
pidProfile->pid[PID_ROLL].D = 25;
#elif defined(HELIO_BANGOOD_SPECIAL)
//optimizng for IDKWTF set the normal defaults
#else
//optimizng for true-x and most standard tunes.
pidProfile->pid[PID_PITCH].P = 45;
pidProfile->pid[PID_PITCH].I = 50;
pidProfile->pid[PID_PITCH].D = 30;
pidProfile->pid[PID_ROLL].P = 45;
pidProfile->pid[PID_ROLL].I = 50;
pidProfile->pid[PID_PITCH].P = 45;
pidProfile->pid[PID_PITCH].I = 50;
pidProfile->pid[PID_PITCH].D = 30;
pidProfile->pid[PID_ROLL].P = 45;
pidProfile->pid[PID_ROLL].I = 50;
pidProfile->pid[PID_ROLL].D = 30;
#endif

Expand All @@ -80,12 +80,11 @@ void targetConfiguration(void) {
pidProfile->itermAcceleratorGain = 5000;
// should't need to set these since they don't get init in gyro.c with USE_GYRO_IMUF
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If these are really no longer needed we should just remove them entirely instead of leaving comments IMO

// pidProfile->yaw_lpf_hz = 0;
// pidProfile->dterm_lpf_hz = 0;
// pidProfile->dterm_lpf_hz = 0;
// pidProfile->dterm_notch_hz = 0;
// pidProfile->dterm_notch_cutoff = 0;
pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_filter_style = KD_FILTER_NOSP;
pidProfile->dterm_filter_style = NOSP;
pidProfile->dterm_lpf_hz = 60;
}
}