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

CMS updates #104

Merged
merged 2 commits into from
Apr 25, 2024
Merged

CMS updates #104

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bc7b516
Added CMS yaw menu. Added O and B to PID menu. Added angle limit. Ren…
AndersHoglund Apr 23, 2024
1dd4abf
Added CMS Governor menu.
AndersHoglund Apr 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
178 changes: 166 additions & 12 deletions src/main/cms/cms_menu_imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,8 @@ static uint8_t pidProfileIndex;
static char pidProfileIndexString[MAX_PROFILE_NAME_LENGTH + 5];
static uint8_t tempPid[3][3];
static uint16_t tempPidF[3];
static uint16_t tempPidO[3];
static uint16_t tempPidB[3];

static uint8_t tmpRateProfileIndex;
static uint8_t rateProfileIndex;
Expand Down Expand Up @@ -157,6 +159,8 @@ static const void *cmsx_PidRead(void)
tempPid[i][1] = pidProfile->pid[i].I;
tempPid[i][2] = pidProfile->pid[i].D;
tempPidF[i] = pidProfile->pid[i].F;
tempPidO[i] = pidProfile->pid[i].O;
tempPidB[i] = pidProfile->pid[i].B;
}

return NULL;
Expand All @@ -183,6 +187,8 @@ static const void *cmsx_PidWriteback(displayPort_t *pDisp, const OSD_Entry *self
pidProfile->pid[i].I = tempPid[i][1];
pidProfile->pid[i].D = tempPid[i][2];
pidProfile->pid[i].F = tempPidF[i];
pidProfile->pid[i].O = tempPidO[i];
pidProfile->pid[i].B = tempPidB[i];
}
pidInitProfile(currentPidProfile);

Expand All @@ -197,16 +203,22 @@ static OSD_Entry cmsx_menuPidEntries[] =
{ "ROLL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1], 0, 200, 1 }},
{ "ROLL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2], 0, 200, 1 }},
{ "ROLL F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_ROLL], 0, 2000, 1 }},
{ "ROLL O", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidO[PID_ROLL], 0, 200, 1 }},
{ "ROLL B", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidB[PID_ROLL], 0, 200, 1 }},

{ "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }},
{ "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }},
{ "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }},
{ "PITCH F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_PITCH], 0, 2000, 1 }},
{ "PITCH O", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidO[PID_PITCH], 0, 200, 1 }},
{ "PITCH B", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidB[PID_PITCH], 0, 200, 1 }},

{ "YAW P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0], 0, 200, 1 }},
{ "YAW I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1], 0, 200, 1 }},
{ "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2], 0, 200, 1 }},
{ "YAW F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW], 0, 2000, 1 }},
{ "YAW O", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidO[PID_YAW], 0, 200, 1 }},
{ "YAW B", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidB[PID_YAW], 0, 200, 1 }},

{ "BACK", OME_Back, NULL, NULL },
{ NULL, OME_END, NULL, NULL}
Expand Down Expand Up @@ -288,11 +300,79 @@ static CMS_Menu cmsx_menuRateProfile = {
.entries = cmsx_menuRateProfileEntries
};

/////////////////// Yaw Profile menu items ///////////////////////

static uint8_t cmsx_yawStopCW;
static uint8_t cmsx_yawStopCCW;
static uint8_t cmsx_yawCollectiveFF;
static uint8_t cmsx_yawCyclicFF;
static uint8_t cmsx_yawTTA;

static const void *cmsx_profileYawOnEnter(displayPort_t *pDisp)
{
UNUSED(pDisp);

setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);

const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);

cmsx_yawStopCW = pidProfile->yaw_cw_stop_gain;
cmsx_yawStopCCW = pidProfile->yaw_ccw_stop_gain;
cmsx_yawCyclicFF = pidProfile->yaw_cyclic_ff_gain;
cmsx_yawCollectiveFF = pidProfile->yaw_collective_ff_gain;
cmsx_yawTTA = pidProfile->governor.tta_gain;

return NULL;
}

static const void *cmsx_profileYawOnExit(displayPort_t *pDisp, const OSD_Entry *self)
{
UNUSED(pDisp);
UNUSED(self);

pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidInitProfile(currentPidProfile);

pidProfile->yaw_cw_stop_gain = cmsx_yawStopCW;
pidProfile->yaw_ccw_stop_gain = cmsx_yawStopCCW;
pidProfile->yaw_cyclic_ff_gain = cmsx_yawCyclicFF;
pidProfile->yaw_collective_ff_gain = cmsx_yawCollectiveFF;
pidProfile->governor.tta_gain = cmsx_yawTTA;

return NULL;
}

static const OSD_Entry cmsx_menuProfileYawEntries[] = {
{ "-- YAW --", OME_Label, NULL, pidProfileIndexString },
{ "STOP CW", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawStopCW, 25, 250, 1 } },
{ "STOP CCW", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawStopCCW, 25, 250, 1 } },
{ "CYCl FF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawCyclicFF, 0, 250, 1 } },
{ "COLL FF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawCollectiveFF, 0, 250, 1 } },
{ "TTA GAIN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawTTA, 0, 250, 1 } },

{ "BACK", OME_Back, NULL, NULL },
{ NULL, OME_END, NULL, NULL}
};

static CMS_Menu cmsx_menuProfileYaw = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XPROFYAW",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_profileYawOnEnter,
.onExit = cmsx_profileYawOnExit,
.onDisplayUpdate = NULL,
.entries = cmsx_menuProfileYawEntries,
};

/////////////////// Level Modes Profile menu items ///////////////////////

static uint8_t cmsx_angleStrength;
static uint8_t cmsx_angleLimit;
static uint8_t cmsx_horizonStrength;
static uint8_t cmsx_horizonTransition;

static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
static const void *cmsx_profileLevelOnEnter(displayPort_t *pDisp)
{
UNUSED(pDisp);

Expand All @@ -301,46 +381,118 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);

cmsx_angleStrength = pidProfile->angle.level_strength;
cmsx_angleLimit = pidProfile->angle.level_limit;
cmsx_horizonStrength = pidProfile->horizon.level_strength;
cmsx_horizonTransition = pidProfile->horizon.transition;

return NULL;
}

static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry *self)
static const void *cmsx_profileLevelOnExit(displayPort_t *pDisp, const OSD_Entry *self)
{
UNUSED(pDisp);
UNUSED(self);

pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidInitProfile(currentPidProfile);

pidProfile->angle.level_strength = cmsx_angleStrength;
pidProfile->angle.level_strength = cmsx_angleStrength;
pidProfile->angle.level_limit = cmsx_angleLimit;
pidProfile->horizon.level_strength = cmsx_horizonStrength;
pidProfile->horizon.transition = cmsx_horizonTransition;
pidProfile->horizon.transition = cmsx_horizonTransition;

return NULL;
}

static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString },
static const OSD_Entry cmsx_menuProfileLevelEntries[] = {
{ "-- LEVEL --", OME_Label, NULL, pidProfileIndexString },
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } },
{ "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleLimit, 10, 90, 1 } },
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } },
{ "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } },

{ "BACK", OME_Back, NULL, NULL },
{ NULL, OME_END, NULL, NULL}
};

static CMS_Menu cmsx_menuProfileOther = {
static CMS_Menu cmsx_menuProfileLevel = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XPROFLEVEL",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_profileLevelOnEnter,
.onExit = cmsx_profileLevelOnExit,
.onDisplayUpdate = NULL,
.entries = cmsx_menuProfileLevelEntries,
};

/////////////////// Governor Profile menu items ///////////////////////

static uint16_t cmsx_govHeadspeed;
static uint8_t cmsx_govMasterGain;
static uint8_t cmsx_govP;
static uint8_t cmsx_govI;
static uint8_t cmsx_govD;
static uint8_t cmsx_govF;

static const void *cmsx_profileGovernorOnEnter(displayPort_t *pDisp)
{
UNUSED(pDisp);

setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);

const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);

cmsx_govHeadspeed = pidProfile->governor.headspeed;
cmsx_govMasterGain = pidProfile->governor.gain;
cmsx_govP = pidProfile->governor.p_gain;
cmsx_govI = pidProfile->governor.i_gain;
cmsx_govD = pidProfile->governor.d_gain;
cmsx_govF = pidProfile->governor.f_gain;

return NULL;
}

static const void *cmsx_profileGovernorOnExit(displayPort_t *pDisp, const OSD_Entry *self)
{
UNUSED(pDisp);
UNUSED(self);

pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidInitProfile(currentPidProfile);

pidProfile->governor.headspeed = cmsx_govHeadspeed;
pidProfile->governor.gain = cmsx_govMasterGain;
pidProfile->governor.p_gain = cmsx_govP;
pidProfile->governor.i_gain = cmsx_govI;
pidProfile->governor.d_gain = cmsx_govD;
pidProfile->governor.f_gain = cmsx_govF;

return NULL;
}

static const OSD_Entry cmsx_menuProfileGovernorEntries[] = {
{ "-- GOV --", OME_Label, NULL, pidProfileIndexString },
{ "HEAD SPD", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_govHeadspeed, 0, 50000, 1 } },
{ "GAIN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_govMasterGain, 0, 250, 1 } },
{ "P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_govP, 0, 250, 1 } },
{ "I", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_govI, 0, 250, 1 } },
{ "D", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_govD, 0, 250, 1 } },
{ "FF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_govF, 0, 250, 1 } },

{ "BACK", OME_Back, NULL, NULL },
{ NULL, OME_END, NULL, NULL}
};

static CMS_Menu cmsx_menuProfileGovernor = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XPROFOTHER",
.GUARD_text = "XPROFGOV",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_profileOtherOnEnter,
.onExit = cmsx_profileOtherOnExit,
.onEnter = cmsx_profileGovernorOnEnter,
.onExit = cmsx_profileGovernorOnExit,
.onDisplayUpdate = NULL,
.entries = cmsx_menuProfileOtherEntries,
.entries = cmsx_menuProfileGovernorEntries,
};


Expand Down Expand Up @@ -614,7 +766,9 @@ static const OSD_Entry cmsx_menuImuEntries[] =

{"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpPidProfileIndex, 1, PID_PROFILE_COUNT, 1}},
{"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid},
{"MISC PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther},
{"YAW", OME_Submenu, cmsMenuChange, &cmsx_menuProfileYaw},
{"LEVEL", OME_Submenu, cmsMenuChange, &cmsx_menuProfileLevel},
{"GOV", OME_Submenu, cmsMenuChange, &cmsx_menuProfileGovernor},
//{"FILT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile},

{"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, CONTROL_RATE_PROFILE_COUNT, 1}},
Expand Down