rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tidied cms imu code

This commit is contained in:
Martin Budden 2016-12-21 15:27:31 +00:00 committed by borisbstyle
parent 02b778438f
commit 8f3ef2676f
1 changed files with 32 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/main/cms/cms_menu_imu.c
View File

@ -104,10 +104,11 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void
static long cmsx_PidRead(void)
{
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
for (uint8_t i = 0; i < 3; i++) {
tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i];
tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i];
tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i];
tempPid[i][0] = pidProfile->P8[i];
tempPid[i][1] = pidProfile->I8[i];
tempPid[i][2] = pidProfile->D8[i];
}
return 0;
@ -125,10 +126,11 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
{
UNUSED(self);
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
for (uint8_t i = 0; i < 3; i++) {
masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0];
masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
pidProfile->P8[i] = tempPid[i][0];
pidProfile->I8[i] = tempPid[i][1];
pidProfile->D8[i] = tempPid[i][2];
}
pidInitConfig(&currentProfile->pidProfile);
@ -233,12 +235,13 @@ static long cmsx_profileOtherOnEnter(void)
{
profileIndexString[1] = '0' + tmpProfileIndex;
cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight;
cmsx_setpointRelaxRatio = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio;
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight;
cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio;
cmsx_angleStrength = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL];
cmsx_horizonStrength = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL];
cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL];
cmsx_angleStrength = pidProfile->P8[PIDLEVEL];
cmsx_horizonStrength = pidProfile->I8[PIDLEVEL];
cmsx_horizonTransition = pidProfile->D8[PIDLEVEL];
return 0;
}
@ -247,13 +250,14 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
{
UNUSED(self);
masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight;
pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidInitConfig(&currentProfile->pidProfile);
masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;
masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition;
pidProfile->P8[PIDLEVEL] = cmsx_angleStrength;
pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength;
pidProfile->D8[PIDLEVEL] = cmsx_horizonTransition;
return 0;
}
@ -311,11 +315,12 @@ static uint16_t cmsx_yaw_p_limit;
static long cmsx_FilterPerProfileRead(void)
{
cmsx_dterm_lpf_hz = masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz;
cmsx_dterm_notch_hz = masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz;
cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff;
cmsx_yaw_lpf_hz = masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz;
cmsx_yaw_p_limit = masterConfig.profile[profileIndex].pidProfile.yaw_p_limit;
const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
cmsx_dterm_lpf_hz = pidProfile->dterm_lpf_hz;
cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz;
cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
cmsx_yaw_lpf_hz = pidProfile->yaw_lpf_hz;
cmsx_yaw_p_limit = pidProfile->yaw_p_limit;
return 0;
}
@ -324,11 +329,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
{
UNUSED(self);
masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz;
masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz = cmsx_dterm_notch_hz;
masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz;
masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit;
pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile;
pidProfile->dterm_lpf_hz = cmsx_dterm_lpf_hz;
pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz;
pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
pidProfile->yaw_lpf_hz = cmsx_yaw_lpf_hz;
pidProfile->yaw_p_limit = cmsx_yaw_p_limit;
return 0;
}