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

CMS: Refactor IMU menu to use pid_t instead of ad-hoc arrays #4801

Merged
merged 1 commit into from
Jun 7, 2019
Merged

CMS: Refactor IMU menu to use pid_t instead of ad-hoc arrays #4801

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
123 changes: 62 additions & 61 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 @@ -49,31 +49,37 @@
//
// PIDFF
//
#define PIDFF_MIN 0
#define PIDFF_STEP 1

#define PIDFF_PARAM_COUNT 4
#define RPY_PIDFF_MAX 200
#define OTHER_PIDDF_MAX 255

static uint8_t cmsx_pidRoll[PIDFF_PARAM_COUNT];
static uint8_t cmsx_pidPitch[PIDFF_PARAM_COUNT];
static uint8_t cmsx_pidYaw[PIDFF_PARAM_COUNT];
#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT8_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
#define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX)
#define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX)

static pid8_t cmsx_pidRoll;
static pid8_t cmsx_pidPitch;
static pid8_t cmsx_pidYaw;
static pid8_t cmsx_pidPosZ;
static pid8_t cmsx_pidVelZ;
static pid8_t cmsx_pidHead;
static pid8_t cmsx_pidPosXY;
static pid8_t cmsx_pidVelXY;

static uint8_t tmpProfileIndex;
static uint8_t profileIndex;
static char profileIndexString[] = " p";

static void cmsx_ReadPidToArray(uint8_t *dst, int pidIndex)
static void cmsx_ReadPidToArray(pid8_t *dst, int pidIndex)
{
dst[0] = pidBank()->pid[pidIndex].P;
dst[1] = pidBank()->pid[pidIndex].I;
dst[2] = pidBank()->pid[pidIndex].D;
dst[3] = pidBank()->pid[pidIndex].FF;
memcpy(dst, &pidBank()->pid[pidIndex], sizeof(*dst));
}

static void cmsx_WritebackPidFromArray(uint8_t *src, int pidIndex)
static void cmsx_WritebackPidFromArray(const pid8_t *src, int pidIndex)
{
pidBankMutable()->pid[pidIndex].P = src[0];
pidBankMutable()->pid[pidIndex].I = src[1];
pidBankMutable()->pid[pidIndex].D = src[2];
pidBankMutable()->pid[pidIndex].FF = src[3];
memcpy(&pidBankMutable()->pid[pidIndex], src, sizeof(*src));
}

static long cmsx_menuImu_onEnter(const OSD_Entry *from)
Expand Down Expand Up @@ -101,9 +107,9 @@ static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *pt

static long cmsx_PidRead(void)
{
cmsx_ReadPidToArray(cmsx_pidRoll, PID_ROLL);
cmsx_ReadPidToArray(cmsx_pidPitch, PID_PITCH);
cmsx_ReadPidToArray(cmsx_pidYaw, PID_YAW);
cmsx_ReadPidToArray(&cmsx_pidRoll, PID_ROLL);
cmsx_ReadPidToArray(&cmsx_pidPitch, PID_PITCH);
cmsx_ReadPidToArray(&cmsx_pidYaw, PID_YAW);

return 0;
}
Expand All @@ -122,9 +128,9 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
{
UNUSED(self);

cmsx_WritebackPidFromArray(cmsx_pidRoll, PID_ROLL);
cmsx_WritebackPidFromArray(cmsx_pidPitch, PID_PITCH);
cmsx_WritebackPidFromArray(cmsx_pidYaw, PID_YAW);
cmsx_WritebackPidFromArray(&cmsx_pidRoll, PID_ROLL);
cmsx_WritebackPidFromArray(&cmsx_pidPitch, PID_PITCH);
cmsx_WritebackPidFromArray(&cmsx_pidYaw, PID_YAW);

schedulePidGainsUpdate();

Expand All @@ -135,20 +141,20 @@ static const OSD_Entry cmsx_menuPidEntries[] =
{
OSD_LABEL_DATA_ENTRY("-- PID --", profileIndexString),

OSD_UINT8_ENTRY("ROLL P", (&(const OSD_UINT8_t){ &cmsx_pidRoll[0], 0, 200, 1 })),
OSD_UINT8_ENTRY("ROLL I", (&(const OSD_UINT8_t){ &cmsx_pidRoll[1], 0, 200, 1 })),
OSD_UINT8_ENTRY("ROLL D", (&(const OSD_UINT8_t){ &cmsx_pidRoll[2], 0, 200, 1 })),
OSD_UINT8_ENTRY("ROLL FF", (&(const OSD_UINT8_t){ &cmsx_pidRoll[3], 0, 200, 1 })),
RPY_PIDFF_ENTRY("ROLL P", &cmsx_pidRoll.P),
RPY_PIDFF_ENTRY("ROLL I", &cmsx_pidRoll.I),
RPY_PIDFF_ENTRY("ROLL D", &cmsx_pidRoll.D),
RPY_PIDFF_ENTRY("ROLL FF", &cmsx_pidRoll.FF),

OSD_UINT8_ENTRY("PITCH P", (&(const OSD_UINT8_t){ &cmsx_pidPitch[0], 0, 200, 1 })),
OSD_UINT8_ENTRY("PITCH I", (&(const OSD_UINT8_t){ &cmsx_pidPitch[1], 0, 200, 1 })),
OSD_UINT8_ENTRY("PITCH D", (&(const OSD_UINT8_t){ &cmsx_pidPitch[2], 0, 200, 1 })),
OSD_UINT8_ENTRY("PITCH FF", (&(const OSD_UINT8_t){ &cmsx_pidPitch[3], 0, 200, 1 })),
RPY_PIDFF_ENTRY("PITCH P", &cmsx_pidPitch.P),
RPY_PIDFF_ENTRY("PITCH I", &cmsx_pidPitch.I),
RPY_PIDFF_ENTRY("PITCH D", &cmsx_pidPitch.D),
RPY_PIDFF_ENTRY("PITCH FF", &cmsx_pidPitch.FF),

OSD_UINT8_ENTRY("YAW P", (&(const OSD_UINT8_t){ &cmsx_pidYaw[0], 0, 200, 1 })),
OSD_UINT8_ENTRY("YAW I", (&(const OSD_UINT8_t){ &cmsx_pidYaw[1], 0, 200, 1 })),
OSD_UINT8_ENTRY("YAW D", (&(const OSD_UINT8_t){ &cmsx_pidYaw[2], 0, 200, 1 })),
OSD_UINT8_ENTRY("YAW FF", (&(const OSD_UINT8_t){ &cmsx_pidYaw[3], 0, 200, 1 })),
RPY_PIDFF_ENTRY("YAW P", &cmsx_pidYaw.P),
RPY_PIDFF_ENTRY("YAW I", &cmsx_pidYaw.I),
RPY_PIDFF_ENTRY("YAW D", &cmsx_pidYaw.D),
RPY_PIDFF_ENTRY("YAW FF", &cmsx_pidYaw.FF),

OSD_BACK_AND_END_ENTRY,
};
Expand All @@ -164,17 +170,13 @@ static const CMS_Menu cmsx_menuPid = {
.entries = cmsx_menuPidEntries
};

static uint8_t cmsx_pidPosZ[3];
static uint8_t cmsx_pidVelZ[3];
static uint8_t cmsx_pidHead[3];

static long cmsx_menuPidAltMag_onEnter(const OSD_Entry *from)
{
UNUSED(from);

cmsx_ReadPidToArray(cmsx_pidPosZ, PID_POS_Z);
cmsx_ReadPidToArray(cmsx_pidVelZ, PID_VEL_Z);
cmsx_pidHead[0] = pidBank()->pid[PID_HEADING].P;
cmsx_ReadPidToArray(&cmsx_pidPosZ, PID_POS_Z);
cmsx_ReadPidToArray(&cmsx_pidVelZ, PID_VEL_Z);
cmsx_ReadPidToArray(&cmsx_pidHead, PID_HEADING);

return 0;
}
Expand All @@ -183,9 +185,9 @@ static long cmsx_menuPidAltMag_onExit(const OSD_Entry *self)
{
UNUSED(self);

cmsx_WritebackPidFromArray(cmsx_pidPosZ, PID_POS_Z);
cmsx_WritebackPidFromArray(cmsx_pidVelZ, PID_VEL_Z);
pidBankMutable()->pid[PID_HEADING].P = cmsx_pidHead[0];
cmsx_WritebackPidFromArray(&cmsx_pidPosZ, PID_POS_Z);
cmsx_WritebackPidFromArray(&cmsx_pidVelZ, PID_VEL_Z);
cmsx_WritebackPidFromArray(&cmsx_pidHead, PID_HEADING);

navigationUsePIDs();

Expand All @@ -196,14 +198,15 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
{
OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString),

OSD_UINT8_ENTRY("ALT P", (&(const OSD_UINT8_t){ &cmsx_pidPosZ[0], 0, 255, 1 })),
OSD_UINT8_ENTRY("ALT I", (&(const OSD_UINT8_t){ &cmsx_pidPosZ[1], 0, 255, 1 })),
OSD_UINT8_ENTRY("ALT D", (&(const OSD_UINT8_t){ &cmsx_pidPosZ[2], 0, 255, 1 })),
OSD_UINT8_ENTRY("VEL P", (&(const OSD_UINT8_t){ &cmsx_pidVelZ[0], 0, 255, 1 })),
OSD_UINT8_ENTRY("VEL I", (&(const OSD_UINT8_t){ &cmsx_pidVelZ[1], 0, 255, 1 })),
OSD_UINT8_ENTRY("VEL D", (&(const OSD_UINT8_t){ &cmsx_pidVelZ[2], 0, 255, 1 })),
OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),

OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P),
OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I),
OTHER_PIDFF_ENTRY("VEL D", &cmsx_pidVelZ.D),

OSD_UINT8_ENTRY("MAG P", (&(const OSD_UINT8_t){ &cmsx_pidHead[0], 0, 255, 1 })),
OTHER_PIDFF_ENTRY("MAG P", &cmsx_pidHead.P),

OSD_BACK_AND_END_ENTRY,
};
Expand All @@ -219,15 +222,12 @@ static const CMS_Menu cmsx_menuPidAltMag = {
.entries = cmsx_menuPidAltMagEntries,
};

static uint8_t cmsx_pidPosXY[3];
static uint8_t cmsx_pidVelXY[3];

static long cmsx_menuPidGpsnav_onEnter(const OSD_Entry *from)
{
UNUSED(from);

cmsx_ReadPidToArray(cmsx_pidPosXY, PID_POS_XY);
cmsx_ReadPidToArray(cmsx_pidVelXY, PID_VEL_XY);
cmsx_ReadPidToArray(&cmsx_pidPosXY, PID_POS_XY);
cmsx_ReadPidToArray(&cmsx_pidVelXY, PID_VEL_XY);

return 0;
}
Expand All @@ -236,8 +236,8 @@ static long cmsx_menuPidGpsnav_onExit(const OSD_Entry *self)
{
UNUSED(self);

cmsx_WritebackPidFromArray(cmsx_pidPosXY, PID_POS_XY);
cmsx_WritebackPidFromArray(cmsx_pidVelXY, PID_VEL_XY);
cmsx_WritebackPidFromArray(&cmsx_pidPosXY, PID_POS_XY);
cmsx_WritebackPidFromArray(&cmsx_pidVelXY, PID_VEL_XY);

navigationUsePIDs();

Expand All @@ -248,11 +248,12 @@ static const OSD_Entry cmsx_menuPidGpsnavEntries[] =
{
OSD_LABEL_DATA_ENTRY("-- GPSNAV --", profileIndexString),

OSD_UINT8_ENTRY("POS P", (&(const OSD_UINT8_t){ &cmsx_pidPosXY[0], 0, 255, 1 })),
OSD_UINT8_ENTRY("POS I", (&(const OSD_UINT8_t){ &cmsx_pidPosXY[1], 0, 255, 1 })),
OSD_UINT8_ENTRY("POSR P", (&(const OSD_UINT8_t){ &cmsx_pidVelXY[0], 0, 255, 1 })),
OSD_UINT8_ENTRY("POSR I", (&(const OSD_UINT8_t){ &cmsx_pidVelXY[1], 0, 255, 1 })),
OSD_UINT8_ENTRY("POSR D", (&(const OSD_UINT8_t){ &cmsx_pidVelXY[2], 0, 255, 1 })),
OTHER_PIDFF_ENTRY("POS P", &cmsx_pidPosXY.P),
OTHER_PIDFF_ENTRY("POS I", &cmsx_pidPosXY.I),

OTHER_PIDFF_ENTRY("POSR P", &cmsx_pidVelXY.P),
OTHER_PIDFF_ENTRY("POSR I", &cmsx_pidVelXY.I),
OTHER_PIDFF_ENTRY("POSR D", &cmsx_pidVelXY.D),

OSD_BACK_AND_END_ENTRY,
};
Expand Down