Skip to content

Commit

Permalink
Merge branch 'master' into esc_sensor_hd_pr
Browse files Browse the repository at this point in the history 
* master:
  Fix for CMS feature disabled issue on Spektrum telemetry (#106)
  Update CMS flight profile menus (#104)
  Auto detection fix for Kontronik ESCs (#100)
  • Loading branch information
@bob01
bob01 committed Apr 27, 2024
2 parents fdd8278 + eafd3e3 commit c590e54
Show file tree
Hide file tree
Showing 3 changed files with 214 additions and 20 deletions.
178 changes: 166 additions & 12 deletions src/main/cms/cms_menu_imu.c
View file
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
47 changes: 43 additions & 4 deletions src/main/sensors/esc_sensor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -914,6 +914,12 @@ static void hw5SensorProcess(timeUs_t currentTimeUs)
* 36-39: CRC32
*
*/
#define KON_FRAME_LENGTH 40
#define KON_FRAME_LENGTH_LEGACY 38
#define KON_CRC_LENGTH 4

static uint8_t kontronikPacketLength = 0; // 40 or 38 byte
static uint8_t kontronikCrcExclude = 0; // 0 or 2

static uint32_t calculateCRC32(const uint8_t *ptr, size_t len)
{
Expand All @@ -928,6 +934,11 @@ static uint32_t calculateCRC32(const uint8_t *ptr, size_t len)
return ~crc;
}

static uint32_t kontronikDecodeCRC(uint8_t index)
{
return buffer[index + 3] << 24 | buffer[index + 2] << 16 | buffer[index + 1] << 8 | buffer[index];
}

static bool processKontronikTelemetryStream(uint8_t dataByte)
{
totalByteCount++;
Expand All @@ -952,7 +963,36 @@ static bool processKontronikTelemetryStream(uint8_t dataByte)
else
syncCount++;
}
else if (readBytes == 40) {
else if (readBytes == kontronikPacketLength) {
readBytes = 0;
return true;
}
else if (kontronikPacketLength == 0 && readBytes == KON_FRAME_LENGTH_LEGACY) {
// auto detect legacy 38 byte packet...
uint32_t crc = kontronikDecodeCRC(KON_FRAME_LENGTH_LEGACY - KON_CRC_LENGTH);
if (calculateCRC32(buffer, KON_FRAME_LENGTH_LEGACY - 2 - KON_CRC_LENGTH) == crc) {
// ...w/ 32 byte payload (2 bytes excluded)
kontronikPacketLength = KON_FRAME_LENGTH_LEGACY;
kontronikCrcExclude = 2;
readBytes = 0;
return true;
}
else if (calculateCRC32(buffer, KON_FRAME_LENGTH_LEGACY - KON_CRC_LENGTH) == crc) {
// ...w/ 34 byte payload
kontronikPacketLength = KON_FRAME_LENGTH_LEGACY;
kontronikCrcExclude = 0;
readBytes = 0;
return true;
}
}
else if (kontronikPacketLength == 0 && readBytes == KON_FRAME_LENGTH) {
// auto detect 40 byte packet...
uint32_t crc = kontronikDecodeCRC(KON_FRAME_LENGTH - KON_CRC_LENGTH);
if (calculateCRC32(buffer, KON_FRAME_LENGTH - KON_CRC_LENGTH) == crc) {
// ...w/ 36 byte payload
kontronikPacketLength = KON_FRAME_LENGTH;
kontronikCrcExclude = 0;
}
readBytes = 0;
return true;
}
Expand All @@ -965,9 +1005,8 @@ static void kontronikSensorProcess(timeUs_t currentTimeUs)
// check for any available bytes in the rx buffer
while (serialRxBytesWaiting(escSensorPort)) {
if (processKontronikTelemetryStream(serialRead(escSensorPort))) {
uint32_t crc = buffer[39] << 24 | buffer[38] << 16 | buffer[37] << 8 | buffer[36];

if (calculateCRC32(buffer, 36) == crc) {
uint32_t crc = kontronikDecodeCRC(kontronikPacketLength - KON_CRC_LENGTH);
if (calculateCRC32(buffer, kontronikPacketLength - kontronikCrcExclude - KON_CRC_LENGTH) == crc) {
uint32_t rpm = buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4];
int16_t throttle = (int8_t)buffer[24];
uint16_t pwm = buffer[23] << 8 | buffer[22];
Expand Down
9 changes: 5 additions & 4 deletions src/main/telemetry/srxl.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -502,6 +502,10 @@ bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
static uint8_t lineNo = 0;
int lineCount = 0;

if (!featureIsEnabled(FEATURE_CMS)) {
return false;
}

// Skip already sent lines...
while (lineSent[lineNo] &&
lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) {
Expand All @@ -517,9 +521,6 @@ bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
lineSent[lineNo] = true;
lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;

// Always send something, Always one user frame after the two mandatory frames
// I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true
// too keep the "Waltz" sequence intact.
return true;
}
#endif
Expand Down Expand Up @@ -779,7 +780,7 @@ void initSrxlTelemetry(void)
}

#if defined(USE_SPEKTRUM_CMS_TELEMETRY)
if (srxlTelemetryEnabled) {
if (srxlTelemetryEnabled && featureIsEnabled(FEATURE_CMS)) {
srxlDisplayportRegister();
}
#endif
Expand Down

0 comments on commit c590e54

Please sign in to comment.