From 26b874564ca424976e274f12ba65853abbbcd8d2 Mon Sep 17 00:00:00 2001 From: Rob G Date: Thu, 25 Apr 2024 08:38:41 -0400 Subject: [PATCH 01/11] Auto detection fix for Kontronik ESCs (#100) * Auto detection fix for legacy Kontronik ESCs - verified Kolibri v3.0/v3.5, Kosmik 200 HV v4.17, JivePro 120 HV v1.13 * Auto detection fix for legacy Kontronik ESCs - minor code cleanup - verified w/ Kolibri v3.5 --- src/main/sensors/esc_sensor.c | 47 ++++++++++++++++++++++++++++++++--- 1 file changed, 43 insertions(+), 4 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 4d6319e41b..06235816fa 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -1012,6 +1012,12 @@ static void uncSensorProcess(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) { @@ -1026,6 +1032,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++; @@ -1050,7 +1061,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; } @@ -1063,9 +1103,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]; uint16_t pwm = buffer[23] << 8 | buffer[22]; uint16_t voltage = buffer[9] << 8 | buffer[8]; From 480cd07c4d98ebc01de0210bf521a7556588e10d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B6glund?= Date: Thu, 25 Apr 2024 14:50:06 +0200 Subject: [PATCH 02/11] Update CMS flight profile menus (#104) * Added CMS yaw menu. Added O and B to PID menu. Added angle limit. Renamed a few level items. * Added CMS Governor menu. --- src/main/cms/cms_menu_imu.c | 178 +++++++++++++++++++++++++++++++++--- 1 file changed, 166 insertions(+), 12 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 195d7c3dd6..1d1d126e79 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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; @@ -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; @@ -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); @@ -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} @@ -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); @@ -301,13 +381,14 @@ 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); @@ -315,16 +396,18 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry 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 } }, @@ -332,15 +415,84 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = { { 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, }; @@ -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}}, From eafd3e302e596956b734e0855496c477588edcfb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B6glund?= Date: Fri, 26 Apr 2024 11:47:12 +0200 Subject: [PATCH 03/11] Fix for CMS feature disabled issue on Spektrum telemetry (#106) --- src/main/telemetry/srxl.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index b06b8e2584..fc1ca11615 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -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) { @@ -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 @@ -779,7 +780,7 @@ void initSrxlTelemetry(void) } #if defined(USE_SPEKTRUM_CMS_TELEMETRY) - if (srxlTelemetryEnabled) { + if (srxlTelemetryEnabled && featureIsEnabled(FEATURE_CMS)) { srxlDisplayportRegister(); } #endif From b7e37837ea0230cd7724a157b4225b9af04399d2 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Sun, 28 Apr 2024 14:12:26 +0100 Subject: [PATCH 04/11] Fix hover drift in PID mode 3 Use error_decay_time_cyclic and error_decay_limit_cyclic as scalers to the decay curves in PID mode 3. The default for decay_time = 250, for preventing drift in hover. The original behaviour (curve values) can be set with decay_time = 20. --- src/main/flight/pid.c | 90 ++++++++++++++++++++++++++----------------- src/main/pg/pid.c | 4 +- 2 files changed, 57 insertions(+), 37 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index acdea0c98f..0320e4e95c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -179,13 +179,13 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile) pid.offsetLimit[i] = pidProfile->offset_limit[i]; // Exponential error decay rates - pid.errorDecayRateGround = (pidProfile->error_decay_time_ground) ? (10 * pid.dT / pidProfile->error_decay_time_ground) : 0; - pid.errorDecayRateCyclic = (pidProfile->error_decay_time_cyclic) ? (10 * pid.dT / pidProfile->error_decay_time_cyclic) : 0; - pid.errorDecayRateYaw = (pidProfile->error_decay_time_yaw) ? (10 * pid.dT / pidProfile->error_decay_time_yaw) : 0; + pid.errorDecayRateGround = (pidProfile->error_decay_time_ground) ? (10.0f / pidProfile->error_decay_time_ground) : 0; + pid.errorDecayRateCyclic = (pidProfile->error_decay_time_cyclic) ? (10.0f / pidProfile->error_decay_time_cyclic) : 0; + pid.errorDecayRateYaw = (pidProfile->error_decay_time_yaw) ? (10.0f / pidProfile->error_decay_time_yaw) : 0; // Max decay speeds in degs/s (linear decay) - pid.errorDecayLimitCyclic = (pidProfile->error_decay_limit_cyclic) ? (pid.dT * pidProfile->error_decay_limit_cyclic) : 1e6; - pid.errorDecayLimitYaw = (pidProfile->error_decay_limit_yaw) ? (pid.dT * pidProfile->error_decay_limit_yaw) : 1e6; + pid.errorDecayLimitCyclic = (pidProfile->error_decay_limit_cyclic) ? pidProfile->error_decay_limit_cyclic : 3600; + pid.errorDecayLimitYaw = (pidProfile->error_decay_limit_yaw) ? pidProfile->error_decay_limit_yaw : 3600; // Error Rotation enable pid.errorRotation = pidProfile->error_rotation; @@ -769,9 +769,9 @@ static void pidApplyCyclicMode2(uint8_t axis) pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError; // Apply I-term error decay - pid.data[axis].axisError -= isAirborne() ? - limitf(pid.data[axis].axisError * pid.errorDecayRateCyclic, pid.errorDecayLimitCyclic): - pid.data[axis].axisError * pid.errorDecayRateGround; + pid.data[axis].axisError -= pid.dT * (isAirborne() ? + limitf(pid.data[axis].axisError * pid.errorDecayRateCyclic, pid.errorDecayLimitCyclic) : + pid.data[axis].axisError * pid.errorDecayRateGround); //// Feedforward @@ -847,9 +847,9 @@ static void pidApplyYawMode2(void) pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError; // Apply I-term error decay - pid.data[axis].axisError -= isSpooledUp() ? - limitf(pid.data[axis].axisError * pid.errorDecayRateYaw, pid.errorDecayLimitYaw): - pid.data[axis].axisError * pid.errorDecayRateGround; + pid.data[axis].axisError -= pid.dT * (isSpooledUp() ? + limitf(pid.data[axis].axisError * pid.errorDecayRateYaw, pid.errorDecayLimitYaw) : + pid.data[axis].axisError * pid.errorDecayRateGround); //// Feedforward @@ -942,23 +942,23 @@ static void pidApplyCyclicMode3(uint8_t axis, const pidProfile_t * pidProfile) const float curve = fabsf(collective) * 0.8f; // Apply error decay - float decayRate, decayLimit, errorDecay; + float errorDecayRate, errorDecayLimit; - if (isAirborne()) { - decayRate = pidTableLookup(curve, pidProfile->error_decay_rate_curve, LOOKUP_CURVE_POINTS) * 0.04f; - decayLimit = pidTableLookup(curve, pidProfile->error_decay_limit_curve, LOOKUP_CURVE_POINTS); - errorDecay = limitf(pid.data[axis].axisError * decayRate, decayLimit); + if (isAirborne() || pid.errorDecayRateGround == 0) { + errorDecayRate = pid.errorDecayRateCyclic * pidTableLookup(curve, pidProfile->error_decay_rate_curve, LOOKUP_CURVE_POINTS) * 0.08f; + errorDecayLimit = pid.errorDecayLimitCyclic * pidTableLookup(curve, pidProfile->error_decay_limit_curve, LOOKUP_CURVE_POINTS) * 0.08f; } else { - decayRate = pid.errorDecayRateGround / pid.dT; - decayLimit = 0; - errorDecay = pid.data[axis].axisError * decayRate; + errorDecayRate = pid.errorDecayRateGround; + errorDecayLimit = 3600; } + const float errorDecay = limitf(pid.data[axis].axisError * errorDecayRate, errorDecayLimit); + pid.data[axis].axisError -= errorDecay * pid.dT; - DEBUG_AXIS(ERROR_DECAY, axis, 0, decayRate * 100); - DEBUG_AXIS(ERROR_DECAY, axis, 1, decayLimit); + DEBUG_AXIS(ERROR_DECAY, axis, 0, errorDecayRate * 100); + DEBUG_AXIS(ERROR_DECAY, axis, 1, errorDecayLimit); DEBUG_AXIS(ERROR_DECAY, axis, 2, errorDecay * 100); DEBUG_AXIS(ERROR_DECAY, axis, 3, pid.data[axis].axisError * 10); @@ -986,22 +986,24 @@ static void pidApplyCyclicMode3(uint8_t axis, const pidProfile_t * pidProfile) DEBUG_AXIS(HS_OFFSET, axis, 7, pid.data[axis].I * 1000); // Apply offset decay - if (isSpooledUp()) { - decayRate = pidTableLookup(curve, pidProfile->offset_decay_rate_curve, LOOKUP_CURVE_POINTS) * 0.04f; - decayLimit = pidTableLookup(curve, pidProfile->offset_decay_limit_curve, LOOKUP_CURVE_POINTS); - errorDecay = limitf(pid.data[axis].axisOffset * decayRate, decayLimit); + float offsetDecayRate, offsetDecayLimit; + + if (isAirborne() || pid.errorDecayRateGround == 0) { + offsetDecayRate = pidTableLookup(curve, pidProfile->offset_decay_rate_curve, LOOKUP_CURVE_POINTS) * 0.04f; + offsetDecayLimit = pidTableLookup(curve, pidProfile->offset_decay_limit_curve, LOOKUP_CURVE_POINTS); } else { - decayRate = pid.errorDecayRateGround / pid.dT; - decayLimit = 0; - errorDecay = pid.data[axis].axisOffset * decayRate; + offsetDecayRate = pid.errorDecayRateGround; + offsetDecayLimit = 3600; } - pid.data[axis].axisOffset -= errorDecay * pid.dT; + const float offsetDecay = limitf(pid.data[axis].axisOffset * offsetDecayRate, offsetDecayLimit); - DEBUG_AXIS(ERROR_DECAY, axis, 4, decayRate * 100); - DEBUG_AXIS(ERROR_DECAY, axis, 5, decayLimit); - DEBUG_AXIS(ERROR_DECAY, axis, 6, errorDecay * 100); + pid.data[axis].axisOffset -= offsetDecay * pid.dT; + + DEBUG_AXIS(ERROR_DECAY, axis, 4, offsetDecayRate * 100); + DEBUG_AXIS(ERROR_DECAY, axis, 5, offsetDecayLimit); + DEBUG_AXIS(ERROR_DECAY, axis, 6, offsetDecay * 100); DEBUG_AXIS(ERROR_DECAY, axis, 7, pid.data[axis].axisOffset * 10); @@ -1079,9 +1081,27 @@ static void pidApplyYawMode3(void) pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError; // Apply error decay - pid.data[axis].axisError -= isSpooledUp() ? - limitf(pid.data[axis].axisError * pid.errorDecayRateYaw, pid.errorDecayLimitYaw): - pid.data[axis].axisError * pid.errorDecayRateGround; + float decayRate, decayLimit, errorDecay; + + if (isSpooledUp() || !pid.errorDecayRateGround) { + decayRate = pid.errorDecayRateYaw; + decayLimit = pid.errorDecayLimitYaw; + } + else { + decayRate = pid.errorDecayRateGround; + decayLimit = 3600; + } + + errorDecay = limitf(pid.data[axis].axisError * decayRate, decayLimit); + + pid.data[axis].axisError -= pid.dT * limitf(pid.data[axis].axisError * + (isSpooledUp() ? pid.errorDecayRateYaw : pid.errorDecayRateGround), + pid.errorDecayLimitYaw); + + DEBUG_AXIS(ERROR_DECAY, axis, 0, decayRate * 100); + DEBUG_AXIS(ERROR_DECAY, axis, 1, decayLimit); + DEBUG_AXIS(ERROR_DECAY, axis, 2, errorDecay * 100); + DEBUG_AXIS(ERROR_DECAY, axis, 3, pid.data[axis].axisError * 10); //// Feedforward diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c index 9d75589f34..a2da44160b 100644 --- a/src/main/pg/pid.c +++ b/src/main/pg/pid.c @@ -53,9 +53,9 @@ void resetPidProfile(pidProfile_t *pidProfile) .dterm_mode = 0, .dterm_mode_yaw = 0, .error_decay_time_ground = 25, - .error_decay_time_cyclic = 18, + .error_decay_time_cyclic = 250, .error_decay_time_yaw = 0, - .error_decay_limit_cyclic = 20, + .error_decay_limit_cyclic = 12, .error_decay_limit_yaw = 0, .error_decay_rate_curve = { 12,13,14,15,17,20,23,28,36,49,78,187,250,250,250,250 }, .error_decay_limit_curve = { 12,12,12,12,12,12,12,12,12,12,12,12,13,14,15,16 }, From f9bf901e68d06dd0e470e84572ebfd7133584b3c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B6glund?= Date: Tue, 30 Apr 2024 14:17:40 +0200 Subject: [PATCH 05/11] Add CMS Rescue, PID controller and bandwidth menus (#107) --- src/main/cms/cms_menu_imu.c | 226 +++++++++++++++++++++++++++++++++++- 1 file changed, 225 insertions(+), 1 deletion(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 1d1d126e79..95d7064f23 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -307,6 +307,7 @@ static uint8_t cmsx_yawStopCCW; static uint8_t cmsx_yawCollectiveFF; static uint8_t cmsx_yawCyclicFF; static uint8_t cmsx_yawTTA; +static uint8_t cmsx_yawPrecompCutoff; static const void *cmsx_profileYawOnEnter(displayPort_t *pDisp) { @@ -321,7 +322,7 @@ static const void *cmsx_profileYawOnEnter(displayPort_t *pDisp) cmsx_yawCyclicFF = pidProfile->yaw_cyclic_ff_gain; cmsx_yawCollectiveFF = pidProfile->yaw_collective_ff_gain; cmsx_yawTTA = pidProfile->governor.tta_gain; - + cmsx_yawPrecompCutoff = pidProfile->yaw_precomp_cutoff; return NULL; } @@ -338,6 +339,7 @@ static const void *cmsx_profileYawOnExit(displayPort_t *pDisp, const OSD_Entry * pidProfile->yaw_cyclic_ff_gain = cmsx_yawCyclicFF; pidProfile->yaw_collective_ff_gain = cmsx_yawCollectiveFF; pidProfile->governor.tta_gain = cmsx_yawTTA; + pidProfile->yaw_precomp_cutoff = cmsx_yawPrecompCutoff; return NULL; } @@ -346,6 +348,7 @@ 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 } }, + { "PRECOMP CUT",OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawPrecompCutoff,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 } }, @@ -365,6 +368,151 @@ static CMS_Menu cmsx_menuProfileYaw = { .entries = cmsx_menuProfileYawEntries, }; +/////////////////// PID controller Profile menu items /////////////////////// + +static uint8_t cmsx_pidCyclicCrossCouplingGain; +static uint8_t cmsx_pidCyclicCrossCouplingRatio; +static uint8_t cmsx_pidCyclicCrossCouplingCutOff; +static uint8_t cmsx_pidItermRelaxCutoff[3]; + +static const void *cmsx_profileCtrlOnEnter(displayPort_t *pDisp) +{ + UNUSED(pDisp); + + const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); + + setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName); + + cmsx_pidCyclicCrossCouplingGain = pidProfile->cyclic_cross_coupling_gain; + cmsx_pidCyclicCrossCouplingRatio = pidProfile->cyclic_cross_coupling_ratio; + cmsx_pidCyclicCrossCouplingCutOff = pidProfile->cyclic_cross_coupling_cutoff; + + for (uint8_t i = 0; i < 3; i++) { + cmsx_pidItermRelaxCutoff[i] = pidProfile->iterm_relax_cutoff[i]; + } + + return NULL; +} + +static const void *cmsx_profileCtrlOnExit(displayPort_t *pDisp, const OSD_Entry *self) +{ + UNUSED(pDisp); + UNUSED(self); + + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + pidInitProfile(currentPidProfile); + + pidProfile->cyclic_cross_coupling_gain = cmsx_pidCyclicCrossCouplingGain; + pidProfile->cyclic_cross_coupling_ratio = cmsx_pidCyclicCrossCouplingRatio; + pidProfile->cyclic_cross_coupling_cutoff= cmsx_pidCyclicCrossCouplingCutOff; + + for (uint8_t i = 0; i < 3; i++) { + pidProfile->iterm_relax_cutoff[i] = cmsx_pidItermRelaxCutoff[i]; + } + + return NULL; +} + +static const OSD_Entry cmsx_menuProfileCtrlEntries[] = { + { "- PID CTRL -", OME_Label, NULL, pidProfileIndexString }, + { "CYCL GAIN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidCyclicCrossCouplingGain, 0, 250, 1 } }, + { "CYCL RATIO", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidCyclicCrossCouplingRatio, 0, 200, 1 } }, + { "CYCL CUT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidCyclicCrossCouplingCutOff, 0, 250, 1 } }, + { "I RLX CUT R", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidItermRelaxCutoff[0], 1, 100, 1 } }, + { "I RLX CUT P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidItermRelaxCutoff[1], 1, 100, 1 } }, + { "I RLX CUT Y", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidItermRelaxCutoff[2], 1, 100, 1 } }, + + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL} +}; + +static CMS_Menu cmsx_menuProfileCtrl = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XPROFCTRL", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_profileCtrlOnEnter, + .onExit = cmsx_profileCtrlOnExit, + .onDisplayUpdate = NULL, + .entries = cmsx_menuProfileCtrlEntries, +}; + +/* +*/ + +/////////////////// PID controller Bandwieth Profile menu items /////////////////////// + +static uint8_t cmsx_pidBandwidth[3]; +static uint8_t cmsx_pidDCutoff[3]; +static uint8_t cmsx_pidBCutoff[3]; + +static const void *cmsx_profileCtrlBwOnEnter(displayPort_t *pDisp) +{ + UNUSED(pDisp); + + const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); + + setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName); + + for (uint8_t i = 0; i < 3; i++) { + cmsx_pidBandwidth[i] = pidProfile->gyro_cutoff[i]; + cmsx_pidDCutoff[i] = pidProfile->dterm_cutoff[i]; + cmsx_pidBCutoff[i] = pidProfile->bterm_cutoff[i]; + } + + return NULL; +} + +static const void *cmsx_profileCtrlBwOnExit(displayPort_t *pDisp, const OSD_Entry *self) +{ + UNUSED(pDisp); + UNUSED(self); + + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + pidInitProfile(currentPidProfile); + + pidProfile->cyclic_cross_coupling_gain = cmsx_pidCyclicCrossCouplingGain; + pidProfile->cyclic_cross_coupling_ratio = cmsx_pidCyclicCrossCouplingRatio; + pidProfile->cyclic_cross_coupling_cutoff= cmsx_pidCyclicCrossCouplingCutOff; + + for (uint8_t i = 0; i < 3; i++) { + pidProfile->gyro_cutoff[i] = cmsx_pidBandwidth[i]; + pidProfile->dterm_cutoff[i] = cmsx_pidDCutoff[i]; + pidProfile->bterm_cutoff[i] = cmsx_pidBCutoff[i]; + } + + return NULL; +} + +static const OSD_Entry cmsx_menuProfileCtrlBWEntries[] = { + { "- PID BW -", OME_Label, NULL, pidProfileIndexString }, + { "BANDWIDTH R", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidBandwidth[0], 0, 250, 1 } }, + { "BANDWIDTH P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidBandwidth[1], 0, 250, 1 } }, + { "BANDWIDTH Y", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidBandwidth[2], 0, 250, 1 } }, + { "D CUTOFF R", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidDCutoff[0], 0, 250, 1 } }, + { "D CUTOFf P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidDCutoff[1], 0, 250, 1 } }, + { "D CUTOFF Y", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidDCutoff[2], 0, 250, 1 } }, + { "B CUTOFF R", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidBCutoff[0], 0, 250, 1 } }, + { "B CUTOFf P", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidBCutoff[1], 0, 250, 1 } }, + { "B CUTOFF Y", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pidBCutoff[2], 0, 250, 1 } }, + + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL} +}; + +static CMS_Menu cmsx_menuProfileCtrlBw = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XPROFCBW", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_profileCtrlBwOnEnter, + .onExit = cmsx_profileCtrlBwOnExit, + .onDisplayUpdate = NULL, + .entries = cmsx_menuProfileCtrlBWEntries, +}; + + + /////////////////// Level Modes Profile menu items /////////////////////// static uint8_t cmsx_angleStrength; @@ -426,6 +574,79 @@ static CMS_Menu cmsx_menuProfileLevel = { .entries = cmsx_menuProfileLevelEntries, }; +/////////////////// Rescue Profile menu items /////////////////////// + +static uint16_t cmsx_rescuePullupCollective; +static uint8_t cmsx_rescuePullupTime; +static uint16_t cmsx_rescueClimbCollective; +static uint8_t cmsx_rescueClimbTime; +static uint16_t cmsx_rescueHoverCollective; +static uint8_t cmsx_rescueLevelGain; +static uint8_t cmsx_rescueFlipGain; + +static const void *cmsx_profileRescueOnEnter(displayPort_t *pDisp) +{ + UNUSED(pDisp); + + setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName); + + const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); + + cmsx_rescuePullupCollective = pidProfile->rescue.pull_up_collective; + cmsx_rescuePullupTime = pidProfile->rescue.pull_up_time; + cmsx_rescueClimbCollective = pidProfile->rescue.climb_collective; + cmsx_rescueClimbTime = pidProfile->rescue.climb_time; + cmsx_rescueHoverCollective = pidProfile->rescue.hover_collective; + cmsx_rescueLevelGain = pidProfile->rescue.level_gain; + cmsx_rescueFlipGain = pidProfile->rescue.flip_gain; + + return NULL; +} + +static const void *cmsx_profileRescueOnExit(displayPort_t *pDisp, const OSD_Entry *self) +{ + UNUSED(pDisp); + UNUSED(self); + + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + pidInitProfile(currentPidProfile); + + pidProfile->rescue.pull_up_collective = cmsx_rescuePullupCollective; + pidProfile->rescue.pull_up_time = cmsx_rescuePullupTime; + pidProfile->rescue.climb_collective = cmsx_rescueClimbCollective; + pidProfile->rescue.climb_time = cmsx_rescueClimbTime; + pidProfile->rescue.hover_collective = cmsx_rescueHoverCollective; + pidProfile->rescue.level_gain = cmsx_rescueLevelGain; + pidProfile->rescue.flip_gain = cmsx_rescueFlipGain; + + return NULL; +} + +static const OSD_Entry cmsx_menuProfileRescueEntries[] = { + { "-- RESCUE --", OME_Label, NULL, pidProfileIndexString }, + { "PU COLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_rescuePullupCollective, 0, 1000, 10 } }, + { "PU TIME", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_rescuePullupTime, 0, 250, 1 } }, + { "CL COLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_rescueClimbCollective, 0, 1000, 10 } }, + { "CL TIME", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_rescueClimbTime, 0, 250, 1 } }, + { "HO COLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_rescueHoverCollective, 0, 1000, 10 } }, + { "LVL GAIN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_rescueLevelGain, 5, 250, 1 } }, + { "FLP GAIN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_rescueFlipGain, 5, 250, 1 } }, + + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL} +}; + +static CMS_Menu cmsx_menuProfileRescue = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XPRORESC", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_profileRescueOnEnter, + .onExit = cmsx_profileRescueOnExit, + .onDisplayUpdate = NULL, + .entries = cmsx_menuProfileRescueEntries, +}; + /////////////////// Governor Profile menu items /////////////////////// static uint16_t cmsx_govHeadspeed; @@ -766,8 +987,11 @@ 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}, + {"PID CTRL", OME_Submenu, cmsMenuChange, &cmsx_menuProfileCtrl}, + {"PID BW", OME_Submenu, cmsMenuChange, &cmsx_menuProfileCtrlBw}, {"YAW", OME_Submenu, cmsMenuChange, &cmsx_menuProfileYaw}, {"LEVEL", OME_Submenu, cmsMenuChange, &cmsx_menuProfileLevel}, + {"RESCUE", OME_Submenu, cmsMenuChange, &cmsx_menuProfileRescue}, {"GOV", OME_Submenu, cmsMenuChange, &cmsx_menuProfileGovernor}, //{"FILT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile}, From 15545b4235d6a25da40d077ad85c4c84b849cbda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B6glund?= Date: Tue, 30 Apr 2024 16:00:50 +0200 Subject: [PATCH 06/11] Fix CMS label alignment on small screen (#109) --- src/main/cms/cms.c | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 50206be5cd..83893e43c9 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -588,7 +588,18 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t case OME_Label: if (IS_PRINTVALUE(*flags) && p->data) { // A label with optional string, immediately following text - cnt = cmsDisplayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, DISPLAYPORT_ATTR_NONE, p->data); + uint8_t start_column = leftMenuColumn; + if (smallScreen) { +#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES + if ((uint8_t)strlen(p->data) <= rightMenuColumn) { + start_column = rightMenuColumn - (uint8_t)strlen(p->data); + } +#endif + } + else { + start_column += (uint8_t)strlen(p->text) +1; + } + cnt = cmsDisplayWrite(pDisplay, start_column, row, DISPLAYPORT_ATTR_NONE, p->data); CLR_PRINTVALUE(*flags); } break; From f18711ef66e2efb6771f9ef93b8df97dd8818ade Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 30 Apr 2024 11:11:24 +0100 Subject: [PATCH 07/11] Change gov_max_throttle range to 0..100% --- src/main/cli/settings.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ca25c215de..69cf8536e9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1159,7 +1159,7 @@ const clivalue_t valueTable[] = { { "gov_yaw_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.yaw_ff_weight) }, { "gov_cyclic_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.cyclic_ff_weight) }, { "gov_collective_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.collective_ff_weight) }, - { "gov_max_throttle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 40, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.max_throttle) }, + { "gov_max_throttle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.max_throttle) }, // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY From 37b8eee4b67e91b1ba4f31bfd79a66820df28bd7 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 30 Apr 2024 12:03:36 +0100 Subject: [PATCH 08/11] Use servo pulse limits for output throttle --- src/main/cli/settings.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 69cf8536e9..b3bdf0d5c0 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -788,9 +788,9 @@ const clivalue_t valueTable[] = { #endif // PG_MOTOR_CONFIG - { "min_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, minthrottle) }, - { "max_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, maxthrottle) }, - { "min_command", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, mincommand) }, + { "min_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_SERVO_PULSE_MIN, PWM_SERVO_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, minthrottle) }, + { "max_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_SERVO_PULSE_MIN, PWM_SERVO_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, maxthrottle) }, + { "min_command", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_SERVO_PULSE_MIN, PWM_SERVO_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, mincommand) }, #ifdef USE_DSHOT #ifdef USE_DSHOT_DMAR { "dshot_burst", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON_AUTO }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useBurstDshot) }, From 1f9792dcd38bf0dd8dce55e93876c96f1b1262cb Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Wed, 1 May 2024 17:44:58 +0100 Subject: [PATCH 09/11] Fix Governor Passthrough IDLE state throttle tracking --- src/main/flight/governor.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/governor.c b/src/main/flight/governor.c index e21d964104..9348daddd7 100644 --- a/src/main/flight/governor.c +++ b/src/main/flight/governor.c @@ -527,7 +527,7 @@ static void governorUpdatePassthrough(void) // -- If NO throttle, move to THROTTLE_OFF // -- if throttle > 20%, move to SPOOLUP case GS_THROTTLE_IDLE: - govMain = govPrev = slewLimit(govPrev, throttleInput, gov.throttleStartupRate); + govMain = govPrev = slewUpLimit(govPrev, throttleInput, gov.throttleStartupRate); if (gov.throttleInputLow) govChangeState(GS_THROTTLE_OFF); else if (throttleInput > gov.maxIdleThrottle) @@ -556,7 +556,7 @@ static void governorUpdatePassthrough(void) govMain = govPrev + govPrev * gov.TTAAdd; if (gov.throttleInputLow) govChangeState(GS_ZERO_THROTTLE); - else if (govMain < gov.maxIdleThrottle) { + else if (throttleInput < gov.maxIdleThrottle) { if (gov.autoEnabled && govStateTime() > gov.autoMinEntry) govChangeState(GS_AUTOROTATION); else From 6d384406a558304baba46735588b40a3c3e0a76e Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 3 May 2024 09:56:34 +0100 Subject: [PATCH 10/11] Improve yaw precomp filtering --- src/main/flight/pid.c | 26 +++++++++++++++----------- src/main/flight/pid.h | 6 ++++-- 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0320e4e95c..e7a1a6c885 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -216,11 +216,13 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile) pid.yawCWStopGain = pidProfile->yaw_cw_stop_gain / 100.0f; pid.yawCCWStopGain = pidProfile->yaw_ccw_stop_gain / 100.0f; - // Collective dynamic filter - pt1FilterInit(&pid.precomp.collFilter, 100.0f / constrainf(pidProfile->yaw_collective_dynamic_decay, 1, 250), pid.freq); + // Collective/cyclic deflection lowpass filters + lowpassFilterInit(&pid.precomp.collDeflectionFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0); + lowpassFilterInit(&pid.precomp.pitchDeflectionFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0); + lowpassFilterInit(&pid.precomp.rollDeflectionFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0); - // Yaw precomp lowpass filter - lowpassFilterInit(&pid.precomp.yawFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0); + // Collective dynamic filter + pt1FilterInit(&pid.precomp.collDynamicFilter, 100.0f / constrainf(pidProfile->yaw_collective_dynamic_decay, 1, 250), pid.freq); // Tail/yaw precomp pid.precomp.yawCyclicFFGain = pidProfile->yaw_cyclic_ff_gain / 100.0f; @@ -389,13 +391,18 @@ static void pidApplyPrecomp(void) const float masterGain = mixerRotationSign() * getSpoolUpRatio(); // Get actual control deflections (from previous cycle) - const float cyclicDeflection = getCyclicDeflection(); - const float collectiveDeflection = getCollectiveDeflection(); + const float collectiveDeflection = filterApply(&pid.precomp.collDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE)); + const float pitchDeflection = filterApply(&pid.precomp.pitchDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_PITCH)); + const float rollDeflection = filterApply(&pid.precomp.rollDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_ROLL)); + + // Calculate cyclic deflection from the filtered controls + const float cyclicDeflection = sqrtf(sq(pitchDeflection) + sq(rollDeflection)); // Collective High Pass Filter (this is possible with PT1) - const float collectiveLF = pt1FilterApply(&pid.precomp.collFilter, collectiveDeflection); + const float collectiveLF = pt1FilterApply(&pid.precomp.collDynamicFilter, collectiveDeflection); const float collectiveHF = collectiveDeflection - collectiveLF; + //// Collective-to-Yaw Precomp // Collective components @@ -406,10 +413,7 @@ static void pidApplyPrecomp(void) float yawCyclicFF = fabsf(cyclicDeflection) * pid.precomp.yawCyclicFFGain; // Calculate total precompensation - float yawPrecomp = yawCollectiveFF + yawCollectiveHF + yawCyclicFF; - - // Lowpass filter - yawPrecomp = filterApply(&pid.precomp.yawFilter, yawPrecomp) * masterGain; + float yawPrecomp = (yawCollectiveFF + yawCollectiveHF + yawCyclicFF) * masterGain; // Add to YAW feedforward pid.data[FD_YAW].F += yawPrecomp; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4ed0ad6b7a..8b01e7c393 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -79,9 +79,11 @@ typedef struct { typedef struct { - filter_t yawFilter; + filter_t collDeflectionFilter; + filter_t pitchDeflectionFilter; + filter_t rollDeflectionFilter; - pt1Filter_t collFilter; + pt1Filter_t collDynamicFilter; float yawCyclicFFGain; float yawCollectiveFFGain; From be50b727fbd8191978c96f9bf1295ccde5180cef Mon Sep 17 00:00:00 2001 From: Rob G Date: Mon, 13 May 2024 05:13:19 -0400 Subject: [PATCH 11/11] Add ESC Forward programming for YGE and Scorpion ESCs (#108) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added OpenYGE transport - TODO frame deserialization * Implemented support for progressive decreasing frame-period during ramp time * Implemented OpenYGE frame deserializer * ESC frame byte alignment workaround * Remove byte alignment workaround, v1.03539 frames good * Added OpenYGE status1 documentation (motor status) - TODO: warning flags * Added status / warning flag documentation * Added status / warning flag documentation * More documentation, fix BB ESC_SENSOR and ESC_SENSOR_DATA values - DEBUG_DATA_EXTRA is BEC voltage * OpenYGE - capture BEC temperature as escSensorData.temperature2 * Capture DEBUG_FRAME_BUFFER (use readBytes vs bufferPos) * escSensorData - added temperature3 (for ESC's with additional temperature sensors), bec_voltage and bec_current * escSensorData - esc sensor parity for gps reuse sources * escSensorData - replaced .temperature3 with .extra1 and .extra2, for capture of non-standard data (for ESC's with additional temperature sensors, status codes, error flags etc) * escSensorData - added ESC sensor protocol CALIBRATE - use to dial in ETX settings (ratio etc) with recognizable values for ESC supplied reuse sources * escSensorData - expose new common ESC telemetry fields as reuse sources * OpenYGE - frame updates for frame version 1 * OpenYGE - final protocol refinements, requires ESC firmware v1.03543 or greater * OpenYGE - final protocol refinements, requires ESC firmware v1.03543 or greater - validate frame type * ESC setup - MSP UI POC - max MSP buffer seems to be btwn 80 and 128 then silent fails * OpenYGE - enable single-wire for future bidirectional use * OpenYGE - enable single-wire for future bidirectional use * OpenYGE - enable single-wire for future bidirectional use, enable txrx mode * OpenYGE - reduced ramp interval based on 3543 LA captures - assume min frame length of 6 until known - apply 0-> -40°C, 255->215°C temperature mapping (frame version >= 2) - minor code cleanups * OpenYGE - disable single-wire telemetry for initial release (ie use RX pin) * ESC Setup - msp - dont send any parameter data until all parameters cached YGE - derive parameter count from parameters received * YGE ESC - msp - save * ESC Setup - minor cosmetic * Tribunus - poc - added request/response one-wire protocol variant * Tribunus - poc - add request/response one-wire protocol variant - wip * Tribunus - poc - add request/response one-wire protocol variant - wip 1 * Tribunus - poc - add request/response one-wire protocol variant - wip 2 * RFF-78 - escdata - escSensorData_t extended to capture esc provided BEC voltage/current, ESC status / fault codes * RFF-78 - reuse - additional CRSF reuse sources to expose extended esc sensor data * ESC MSP - more generic impl pulled back from more advanced branch * Tribunus - poc - add request/response one-wire protocol variant - wip 3 * Tribunus - poc - add request/response one-wire protocol variant - wip 4 * ESC Setup - Scorpion ported and validated on RRFSM (shared request/response FSM), OpenYGE ported to RRFSM * ESC Setup - OpenYGE validated on RRFSM * ESC Setup - Scorpion setup params - wip * ESC Setup - Scorpion setup params - wip2 * SCORP ESC - extended payload to include stick min/max Us * ESC Setup - Scorpion setup params - UNC test * ESC Setup - RRFSM validation * OpenYGE - reverted temp offset change (original implementation was correct) * OpenYGE - save param - wip * OpenYGE - save param - RFP-02 impl * ESC Setup - hack / poc to validate scorpion read/write while in UNC mode * ESC Setup - scorpion read/write while in UNC mode - cleanup of secondary state machine * ESC Setup - scorpion read/write while in UNC mode - cleanup of secondary state machine - verified * ESC Setup - scorpion - support IQ22 settings gov P and I * ESC Setup - scorpion - support IQ22 settings gov P and I * ESC Setup - scorpion - msp params extended to include ESC commands, added 'reset ESC' option * ESC Setup - minor code cleanup * OpenYGE - support v3 frame header extension * OpenYGE - support v3 frame header extension - documentation (cherry picked from commit 00fc315409dd568015c3bb77abaf3bc157c9b503) * OpenYGE - support v3 frame header extension - documentation (cherry picked from commit 00fc315409dd568015c3bb77abaf3bc157c9b503) (cherry picked from commit a1d7b164ca43bd765eca77beacd5e2e915c1d2e8) * ESC Setup - RRFSM / serial port init cleanup * ESC Setup - support PARAM_HEADER_RDONLY (0x40) e.g. no bidirectional connection to ESC * Tribunus - const cleanup * OpenYGE - ESC_SENSOR voltage and current fixes * OpenYGE - const cleanup * OpenYGE - draft bidirectional impl w/ fallback to auto mode, structs to pack and parse packets * OpenYGE - draft bidirectional impl. cosmetic and doc cleanup * ESC Setup - parameter exchange cleanup * OpenYGE - telemetry - BEC voltage and current fix * OpenYGE - telemetry - BEC voltage and current fix * OpenYGE - master - assume role after last auto telemetry frame w/ delay for chained responses * OpenYGE - master - assume role ~24ms quiet time after seeing v3 auto telemetry frame, support TELE_AUTO, TELE_REQ/RESP, WRITE_PARAM_REQ/RESP * OpenYGE - setup - params read only if frame_version < v3 * ESC Setup - params - invalidate param payload and param on param commit, wait until param written and subsequently read before making param payload available again. ESC capability / command cleanup * OpenYGE - fix: address possible frame underflow condition, add 2nd paranoid frame length validation outside ISR/callback, declare globals shared w/ callback volatile * ESC status - send reserved bit to force recognition of status == 0 * RFF-78 - escSensorData - added .throttle to. capture input setpoint as reported by ESC * REUSE - added BEC, BUS, MPU voltage sources to GPS sensor options * CRSF - reuse - expose additional ESC sources on ATT 16-bit sensors - not enough room on GPS sensors * CRSF - reuse - minor code consistency - missing comma * ESC Setup - scorpion - unc timeout fixes, minor cosmetics * ESC Setup - scorpion - remove enc telemetry data * ESC Setup - pre-pr code cosmetics, more defined constants, removed deprecated unc * hd - void signature fixes, kon status as 32 bit * hd - IQ22 - use float vs double - tests ok for range of values used * hd - IQ22 - improper use of round(...) w/ float type, replace with roundf(...) - unit and device tests ok for range of values used --------- Co-authored-by: Rotorflight --- src/main/cli/settings.c | 4 +- src/main/msp/msp.c | 23 + src/main/msp/msp_protocol.h | 3 + src/main/sensors/esc_sensor.c | 1348 +++++++++++++++++++++++++-------- src/main/sensors/esc_sensor.h | 25 +- src/main/telemetry/crsf.c | 67 +- src/main/telemetry/crsf.h | 16 + 7 files changed, 1155 insertions(+), 331 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b3bdf0d5c0..22893ab8d1 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -489,11 +489,11 @@ const char * const lookupTableCrsfFmReuse[] = { }; const char * const lookupTableCrsfAttReuse[] = { - "NONE", "THROTTLE", "ESC_TEMP", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", + "NONE", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", }; const char * const lookupTableCrsfGpsReuse[] = { - "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", + "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", }; const char * const lookupTableCrsfGpsSatsReuse[] = { diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 7f56979979..670d56811f 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -804,6 +804,16 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU8(dst, escSensorConfig()->hw4_current_gain); sbufWriteU8(dst, escSensorConfig()->hw4_voltage_gain); break; + + case MSP_ESC_PARAMETERS: + { + const uint8_t len = escGetParamBufferLength(); + if (len == 0) + return false; + + sbufWriteData(dst, escGetParamBuffer(), len); + } + break; #endif case MSP_BATTERY_STATE: @@ -2641,6 +2651,19 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, escSensorConfigMutable()->hw4_current_gain = sbufReadU8(src); escSensorConfigMutable()->hw4_voltage_gain = sbufReadU8(src); break; + + case MSP_SET_ESC_PARAMETERS: + { + const uint8_t len = escGetParamBufferLength(); + if (len == 0) + return MSP_RESULT_ERROR; + + sbufReadData(src, escGetParamUpdBuffer(), len); + + if (!escCommitParameters()) + return MSP_RESULT_ERROR; + } + break; #endif case MSP_EEPROM_WRITE: diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index b198ddb98a..11b1aa9919 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -234,6 +234,9 @@ #define MSP_SET_NAV_CONFIG 215 #define MSP_SET_ESC_SENSOR_CONFIG 216 +#define MSP_ESC_PARAMETERS 217 +#define MSP_SET_ESC_PARAMETERS 218 + #define MSP_SET_RESET_CURR_PID 219 #define MSP_SET_SENSOR_ALIGNMENT 220 #define MSP_SET_LED_STRIP_MODECOLOR 221 diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 06235816fa..3dfcbaa6fd 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -100,6 +100,15 @@ enum { }; #define TELEMETRY_BUFFER_SIZE 40 +#define REQUEST_BUFFER_SIZE 40 +#define PARAM_BUFFER_SIZE 96 +#define PARAM_HEADER_SIZE 2 +#define PARAM_HEADER_SIG 0 +#define PARAM_HEADER_VER 1 +#define PARAM_HEADER_VER_MASK 0x3F +#define PARAM_HEADER_CMD_MASK 0xC0 +#define PARAM_HEADER_RDONLY 0x40 +#define PARAM_HEADER_USER 0x80 static serialPort_t *escSensorPort = NULL; @@ -124,9 +133,26 @@ static uint8_t buffer[TELEMETRY_BUFFER_SIZE] = { 0, }; static volatile uint8_t bufferSize = 0; static volatile uint8_t bufferPos = 0; -static uint8_t readBytes = 0; +static volatile uint8_t readBytes = 0; +static volatile uint8_t readIngoreBytes = 0; static uint32_t syncCount = 0; +static uint8_t reqLength = 0; +static uint8_t reqbuffer[REQUEST_BUFFER_SIZE] = { 0, }; + +static uint8_t paramPayloadLength = 0; +static uint8_t paramBuffer[PARAM_BUFFER_SIZE] = { 0, }; +static uint8_t paramUpdBuffer[PARAM_BUFFER_SIZE] = { 0, }; +static uint8_t *paramPayload = paramBuffer + PARAM_HEADER_SIZE; +static uint8_t *paramUpdPayload = paramUpdBuffer + PARAM_HEADER_SIZE; +static uint8_t paramSig = 0; +static uint8_t paramVer = 0; +static bool paramMspActive = false; + +// called on MSP_SET_ESC_PARAMETERS when paramUpdPayload / paramUpdBuffer ready +typedef bool (*paramCommitCallbackPtr)(uint8_t cmd); +static paramCommitCallbackPtr paramCommit = NULL; + bool isEscSensorActive(void) { @@ -603,6 +629,7 @@ static void hw4SensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = thr; escSensorData[0].pwm = pwm; escSensorData[0].voltage = lrintf(voltage * 1000); escSensorData[0].current = lrintf(current * 1000); @@ -772,10 +799,13 @@ static void hw5SensorProcess(timeUs_t currentTimeUs) if (calculateCRC16_MODBUS(buffer, 30) == crc) { uint32_t rpm = buffer[14] << 8 | buffer[13]; uint16_t power = buffer[9]; + uint16_t fault = buffer[12]; uint16_t voltage = buffer[16] << 8 | buffer[15]; uint16_t current = buffer[18] << 8 | buffer[17]; uint16_t tempFET = buffer[19]; uint16_t tempBEC = buffer[20]; + uint16_t voltBEC = buffer[22]; + uint16_t currBEC = buffer[23]; // When throttle changes to zero, the last current reading is // repeated until the motor has totally stopped. @@ -787,11 +817,15 @@ static void hw5SensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = power * 10; escSensorData[0].pwm = power * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; escSensorData[0].temperature = tempFET * 10; escSensorData[0].temperature2 = tempBEC * 10; + escSensorData[0].bec_voltage = voltBEC * 100; + escSensorData[0].bec_current = currBEC * 100; + escSensorData[0].status = fault; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 10); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, tempFET * 10); @@ -824,138 +858,6 @@ static void hw5SensorProcess(timeUs_t currentTimeUs) } -/* - * Scorpion Unsolicited Telemetry - * - * - ESC must be set to "Unsolicited mode" - * - Serial protocol is 38400,8N1 - * - Frame rate running:10Hz idle:1Hz - * - Little-Endian fields - * - CRC16-CCITT - * - Error Code bits: - * 0: N/A - * 1: BEC voltage error - * 2: Temperature error - * 3: Consumption error - * 4: Input voltage error - * 5: Current error - * 6: N/A - * 7: Throttle error - * - * Frame Format - * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― - * 0: Header Sync (0x55) - * 1: Message format version (0x00) - * 2: Message Length incl. header and CRC (22) - * 3: Device ID - * 4-6: Timestamp ms - * 7: Throttle in 0.5% - * 8-9: Current in 0.1A - * 10-11: Voltage in 0.1V - * 12-13: Consumption in mAh - * 14: Temperature in °C - * 15: PWM duty cycle in 0.5% - * 16: BEC voltage in 0.1V - * 17-18: RPM in 5rpm steps - * 19: Error code - * 20-21: CRC16 CCITT - * - */ - -static uint16_t calculateCRC16_CCITT(const uint8_t *ptr, size_t len) -{ - uint16_t crc = 0; - - while (len--) { - crc ^= *ptr++; - for (int i = 0; i < 8; i++) - crc = (crc & 1) ? (crc >> 1) ^ 0x8408 : (crc >> 1); - } - - return crc; -} - -static bool processUNCTelemetryStream(uint8_t dataByte) -{ - totalByteCount++; - - buffer[readBytes++] = dataByte; - - if (readBytes == 1) { - if (dataByte != 0x55) - frameSyncError(); - } - else if (readBytes == 2) { - if (dataByte != 0x00) // Proto v0 - frameSyncError(); - } - else if (readBytes == 3) { - if (dataByte != 22) // Message v0 is 22 bytes - frameSyncError(); - else - syncCount++; - } - else if (readBytes == 22) { - readBytes = 0; - return true; - } - - return false; -} - -static void uncSensorProcess(timeUs_t currentTimeUs) -{ - // check for any available bytes in the rx buffer - while (serialRxBytesWaiting(escSensorPort)) { - if (processUNCTelemetryStream(serialRead(escSensorPort))) { - uint16_t crc = buffer[21] << 8 | buffer[20]; - - if (calculateCRC16_CCITT(buffer, 20) == crc) { - uint16_t rpm = buffer[18] << 8 | buffer[17]; - uint16_t temp = buffer[14]; - uint16_t power = buffer[15]; - uint16_t voltage = buffer[11] << 8 | buffer[10]; - uint16_t current = buffer[9] << 8 | buffer[8]; - uint16_t capacity = buffer[13] << 8 | buffer[12]; - uint16_t status = buffer[19]; - - escSensorData[0].age = 0; - escSensorData[0].erpm = rpm * 5; - escSensorData[0].pwm = power * 5; - escSensorData[0].voltage = voltage * 100; - escSensorData[0].current = current * 100; - escSensorData[0].consumption = capacity; - escSensorData[0].temperature = temp * 10; - - DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 5); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, voltage * 10); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, current * 10); - - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, rpm); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_PWM, power); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_TEMP, temp); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, voltage); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, current); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, capacity); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, status); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_AGE, 0); - - dataUpdateUs = currentTimeUs; - - totalFrameCount++; - } - else { - totalCrcErrorCount++; - } - } - } - - // Maximum frame spacing 1000ms - checkFrameTimeout(currentTimeUs, 1200000); -} - - /* * Kontronik Telemetry V4 (23.11.2018) * @@ -1106,21 +1008,29 @@ static void kontronikSensorProcess(timeUs_t currentTimeUs) 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]; uint16_t voltage = buffer[9] << 8 | buffer[8]; uint16_t current = buffer[11] << 8 | buffer[10]; uint16_t capacity = buffer[17] << 8 | buffer[16]; int16_t tempFET = (int8_t)buffer[26]; int16_t tempBEC = (int8_t)buffer[27]; + uint16_t currBEC = buffer[19] << 8 | buffer[18]; + uint16_t voltBEC = buffer[21] << 8 | buffer[20]; + uint32_t status = buffer[31] << 24 | buffer[30] << 16 | buffer[29] << 8 | buffer[28]; escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = (throttle + 100) * 5; escSensorData[0].pwm = pwm * 10; escSensorData[0].voltage = voltage * 10; escSensorData[0].current = current * 100; escSensorData[0].consumption = capacity; escSensorData[0].temperature = tempFET * 10; escSensorData[0].temperature2 = tempBEC * 10; + escSensorData[0].bec_voltage = voltBEC; + escSensorData[0].bec_current = currBEC; + escSensorData[0].status = status; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, tempFET * 10); @@ -1216,6 +1126,7 @@ static void ompSensorProcess(timeUs_t currentTimeUs) // Make sure this is OMP M4 ESC if (buffer[1] == 0x01 && buffer[2] == 0x20 && buffer[11] == 0 && buffer[18] == 0 && buffer[20] == 0) { uint16_t rpm = buffer[8] << 8 | buffer[9]; + uint16_t throttle = buffer[7]; uint16_t pwm = buffer[12]; uint16_t temp = buffer[10]; uint16_t voltage = buffer[3] << 8 | buffer[4]; @@ -1225,11 +1136,13 @@ static void ompSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = throttle * 10; escSensorData[0].pwm = pwm * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; escSensorData[0].consumption = capacity; escSensorData[0].temperature = temp * 10; + escSensorData[0].status = status; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 10); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); @@ -1334,19 +1247,24 @@ static void ztwSensorProcess(timeUs_t currentTimeUs) if (buffer[1] == 0x01 && buffer[2] == 0x20) { uint16_t rpm = buffer[8] << 8 | buffer[9]; uint16_t temp = buffer[10]; + uint16_t throttle = buffer[7]; uint16_t power = buffer[12]; uint16_t voltage = buffer[3] << 8 | buffer[4]; uint16_t current = buffer[5] << 8 | buffer[6]; uint16_t capacity = buffer[15] << 8 | buffer[16]; uint16_t status = buffer[13] << 8 | buffer[14]; + uint16_t voltBEC = buffer[19]; escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = throttle * 10; escSensorData[0].pwm = power * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; escSensorData[0].consumption = capacity; escSensorData[0].temperature = temp * 10; + escSensorData[0].bec_voltage = voltBEC * 1000; + escSensorData[0].status = status; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 10); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); @@ -1472,6 +1390,7 @@ static void apdSensorProcess(timeUs_t currentTimeUs) if (calculateFletcher16(buffer + 2, 18) == crc) { uint16_t rpm = buffer[13] << 24 | buffer[12] << 16 | buffer[11] << 8 | buffer[10]; uint16_t tadc = buffer[3] << 8 | buffer[2]; + uint16_t throttle = buffer[15] << 8 | buffer[14]; uint16_t power = buffer[17] << 8 | buffer[16]; uint16_t voltage = buffer[1] << 8 | buffer[0]; uint16_t current = buffer[5] << 8 | buffer[4]; @@ -1483,10 +1402,12 @@ static void apdSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = throttle; escSensorData[0].pwm = power; escSensorData[0].voltage = voltage * 10; escSensorData[0].current = current * 80; escSensorData[0].temperature = lrintf(temp * 10); + escSensorData[0].status = status; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, lrintf(temp * 10)); @@ -1518,6 +1439,658 @@ static void apdSensorProcess(timeUs_t currentTimeUs) } +/* + * RRFSM + * + */ +// > 0: frame accepted, count as synced (may not be complete), continue accepting +// < 0: frame rejected, will be logged as sync error +// 0: continue accepting +typedef int8_t (*rrfsmAcceptCallbackPtr)(uint16_t c); + +typedef bool (*rrfsmStartCallbackPtr)(timeMs_t currentTimeMs); // return true to continue w/ default initialization (if in doubt return true) +typedef bool (*rrfsmDecodeCallbackPtr)(timeMs_t currentTimeMs); // return true if frame was decoded successfully +typedef bool (*rrfsmCrankCallbackPtr)(timeMs_t currentTimeMs); // return true to continue w/ default loop (advanced, if in doubt return true) + +static rrfsmAcceptCallbackPtr rrfsmAccept = NULL; +static rrfsmStartCallbackPtr rrfsmStart = NULL; +static rrfsmDecodeCallbackPtr rrfsmDecode = NULL; +static rrfsmCrankCallbackPtr rrfsmCrank = NULL; + +static uint16_t rrfsmBootDelayMs = 0; +static uint8_t rrfsmMinFrameLength = 0; + +static uint32_t rrfsmFrameTimestamp = 0; +static volatile uint8_t rrfsmFrameLength = 0; +static uint16_t rrfsmFramePeriod = 0; +static uint16_t rrfsmFrameTimeout = 0; + +static void rrfsmFrameSyncError(void) +{ + readBytes = 0; + syncCount = 0; + + totalSyncErrorCount++; +} + +static void rrfsmStartFrame(timeMs_t currentTimeMs) +{ + readBytes = 0; + rrfsmFrameLength = rrfsmMinFrameLength; + rrfsmFrameTimestamp = currentTimeMs; +} + +static void rrfsmStartFrameAndSendPendingReq(timeMs_t currentTimeMs) +{ + rrfsmStartFrame(currentTimeMs); + + rrfsmFramePeriod = 0; + readIngoreBytes = reqLength; + if (reqLength != 0) + serialWriteBuf(escSensorPort, reqbuffer, reqLength); +} + +static void rrfsmInvalidateReq(void) +{ + reqbuffer[0] = 0x00; // no req + reqLength = 0; + rrfsmFrameTimeout = 0; +} + +static FAST_CODE void rrfsmDataReceive(uint16_t c, void *data) +{ + UNUSED(data); + + // don't listen to self + if (readIngoreBytes > 0) { + readIngoreBytes--; + return; + } + + totalByteCount++; + + if (readBytes >= TELEMETRY_BUFFER_SIZE) { + // avoid buffer overrun + rrfsmFrameSyncError(); + } + else if (readBytes < rrfsmFrameLength) { + buffer[readBytes++] = c; + + int8_t accepted = (rrfsmAccept != NULL ) ? rrfsmAccept(c) : -1; + if (accepted > 0) { + // frame accepted (may not be complete) + syncCount++; + } + else if (accepted < 0) { + // frame rejected + rrfsmFrameSyncError(); + } + } +} + +static void rrfsmSensorProcess(timeUs_t currentTimeUs) +{ + const timeMs_t currentTimeMs = currentTimeUs / 1000; + + // wait before initializing + if (currentTimeMs < rrfsmBootDelayMs) + return; + + // request first log record or just listen if in e.g. UNC mode + if (rrfsmFrameTimestamp == 0) { + if (rrfsmStart == NULL || rrfsmStart(currentTimeMs)) + rrfsmStartFrame(currentTimeMs); + return; + } + + // frame period in effect? request next frame if elapsed + if (rrfsmFramePeriod != 0) { + if (currentTimeMs > rrfsmFrameTimestamp + rrfsmFramePeriod) { + rrfsmStartFrameAndSendPendingReq(currentTimeMs); + } + return; + } + + // timeout waiting for frame? log error, request again + if (rrfsmFrameTimeout != 0 && currentTimeMs > rrfsmFrameTimestamp + rrfsmFrameTimeout) { + increaseDataAge(0); + totalTimeoutCount++; + rrfsmStartFrameAndSendPendingReq(currentTimeMs); + return; + } + + // custom execution (advanced) + if (rrfsmCrank != NULL && !rrfsmCrank(currentTimeMs)) { + return; + } + + // frame incomplete? check later + if (readBytes < rrfsmFrameLength) { + return; + } + + // frame complate, process, prepare for next frame + if (rrfsmDecode == NULL || rrfsmDecode(currentTimeMs)) { + // good frame, log, response handler will have prep'ed next request + totalFrameCount++; + } + else { + // bad frame, log error, retry + increaseDataAge(0); + totalCrcErrorCount++; + } + rrfsmStartFrame(currentTimeMs); +} + + +/* + * Scorpion Telemetry + * - Serial protocol is 38400,8N1 + * - Frame rate running:10Hz idle:1Hz + * - Little-Endian fields + * - CRC16-CCITT + * - Error Code bits: + * 0: N/A + * 1: BEC voltage error + * 2: Temperature error + * 3: Consumption error + * 4: Input voltage error + * 5: Current error + * 6: N/A + * 7: Throttle error + * + * Request Format + * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― + * 0: Req: MSN - read = 0x5, write = 0xD, LSN - region + * 1,2,3: Address: Starting address in region to access + * 4: Length: Data length to read/write, does not include Data crc + * Requested data length in bytes should not be more than 32 bytes and should be a multiple of 2 bytes + * 5: CRC: Header CRC. 0 value means that crc is not used. And no additional bytes for data crc + * Regions + * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― + * 0 – System region, read only + * 1 – Status region, read only + * 2 – Control region, stay away + * 3 – Settings region, read/write access + * 4 – FW region, write only, encrypted. Stay away + * 5 - Log region, read only + * 6 – User data + + * + * Telemetry Frame Format + * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― + * 0-5: Header + * +0-+2: Timestamp ms + * +3: Throttle in 0.5% + * +4-+5: Current in 0.1A + * +6-+7: Voltage in 0.1V + * +8-+9: Consumption in mAh + * +10: Temperature in °C + * +11: PWM duty cycle in 0.5% + * +12: BEC voltage in 0.1V + *+13-+14: RPM in 5rpm steps + * +15: Error code + *+16-+17: CRC16 CCITT + * + */ +#define TRIB_REQ_WRITE 0x80 // request write bit +#define TRIB_REQ_SIG 0x50 // request signature bits +#define TRIB_BOOT_DELAY 10000 // 10 seconds quiet time before expecting first frame +#define TRIB_HEADER_LENGTH 6 // assume header only until actual length of current frame known +#define TRIB_UNC_HEADER_LENGTH 4 // UNC packet header length +#define TRIB_FRAME_PERIOD 100 // aim for approx. 20Hz (note UNC is 10Hz, may have to reduce this) +#define TRIB_REQ_BOOT_DELAY 4000 // 4 seconds quiet time before ESC requests +#define TRIB_REQ_READ_TIMEOUT 200 // Response timeout for read requests +#define TRIB_REQ_WRITE_TIMEOUT 1200 // Response timeout for write requests +#define TRIB_RESP_FRAME_TIMEOUT 200 // Response timeout depends on ESC business but no more than 200ms + // Host must wait ESC response or timeout before sending new packet +#define TRIB_UNC_FRAME_TIMEOUT 1200 // Timeout for UNC packets +#define TRIB_PARAM_FRAME_PERIOD 2 // asap +#define TRIB_PARAM_READ_TIMEOUT 200 // usually less than 200Us +#define TRIB_PARAM_WRITE_TIMEOUT 1200 // usually less than 1200Us +#define TRIB_PARAM_SIG 0x53 // parameter payload signature for this ESC +#define TRIB_PARAM_IQ22_ADDR 0x18 // address of param range w/ IQ22 params +#define TRIB_PARAM_CAP_RESET PARAM_HEADER_USER // reset ESC capable +#define TRIB_PARAM_CMD_RESET PARAM_HEADER_USER // reset ESC command + +#define TRIB_PAL_SYS_MASK 0x8000 // param address/length range system bit mask +#define TRIB_PAL_ADDR_MASK 0x7F00 // param address/length range address mask +#define TRIB_PAL_LEN_MASK 0x00FF // param address/length range length mask + +#define TRIB_PKT_READ_SYSTEM 0x50 // read system region packet +#define TRIB_PKT_READ_STATUS 0x51 // read status region packet +#define TRIB_PKT_READ_SETTINGS 0x53 // read settings region packet +#define TRIB_PKT_WRITE_SETTINGS 0xD3 // write settings region packet +#define TRIB_PKT_UNSOLICITED 0x55 // unsolicited packet + +typedef enum { + TRIB_UNCSETUP_INACTIVE = 0, + TRIB_UNCSETUP_PARAMSREADY, + TRIB_UNCSETUP_ABORTUNC, + TRIB_UNCSETUP_ACTIVE, + TRIB_UNCSETUP_WAIT, +} tribUncSetup_e; + +static tribUncSetup_e tribUncSetup = TRIB_UNCSETUP_INACTIVE; + +// param ranges - hibyte=addr (system region if 0x80 set or setting region otherwise), lobyte=length +static uint16_t tribParamAddrLen[] = { 0x0020, 0x1008, 0x230E, 0x8204, 0x8502, 0x1406, 0x1808, 0x3408 }; + +static uint16_t tribInvalidParams = 0; // bit per param address range not yet received from ESC +static uint16_t tribDirtyParams = 0; // bit per param address that needs to be written to ESC +static bool tribResetEsc = false; + +static uint16_t calculateCRC16_CCITT(const uint8_t *ptr, size_t len) +{ + uint16_t crc = 0; + + while (len--) { + crc ^= *ptr++; + for (int i = 0; i < 8; i++) + crc = (crc & 1) ? (crc >> 1) ^ 0x8408 : (crc >> 1); + } + + return crc; +} + +static bool tribParamCommit(uint8_t cmd) +{ + if (cmd == 0) { + // save page + // find dirty params, settings only + uint8_t offset = 0; + for (uint8_t i = 0; i < ARRAYLEN(tribParamAddrLen); i++) { + const uint16_t *pal = tribParamAddrLen + i; + const uint8_t len = *pal & TRIB_PAL_LEN_MASK; + if ((*pal & TRIB_PAL_SYS_MASK) == 0) { + // schedule writes for dirty address ranges + if (memcmp(paramPayload + offset, paramUpdPayload + offset, len) != 0) { + // set dirty bit + tribDirtyParams |= (1U << i); + // invalidate param + tribInvalidParams |= (1 << i); + // invalidate param payload - will be available again when all params again cached + paramPayloadLength = 0; + } + } + offset += len; + } + return true; + } + else if (cmd == TRIB_PARAM_CMD_RESET) { + // reset ESC + tribResetEsc = true; + return true; + } + else { + // invalid command + return false; + } +} + +static void tribBuildReq(uint8_t req, uint8_t addr, void *src, uint8_t len, uint16_t framePeriod, uint16_t frameTimeout) +{ + reqbuffer[0] = req; // req + reqbuffer[1] = 0; // addr + reqbuffer[2] = 0; + reqbuffer[3] = addr; + reqbuffer[4] = len; // length + reqbuffer[5] = 0; + reqLength = TRIB_HEADER_LENGTH; + if (src != NULL) { + memcpy(reqbuffer + TRIB_HEADER_LENGTH, src, len); + reqLength += len; + } + rrfsmFramePeriod = framePeriod; + rrfsmFrameTimeout = frameTimeout; +} + +static void tribInvalidateParams(void) +{ + tribInvalidParams = ~(~1U << (ARRAYLEN(tribParamAddrLen) - 1)); +} + +static uint8_t tribCalcParamBufferLength() +{ + uint8_t len = 0; + for (uint8_t j = 0; j < ARRAYLEN(tribParamAddrLen); j++) + len += (tribParamAddrLen[j] & TRIB_PAL_LEN_MASK); + return len; +} + +static bool tribBuildNextParamReq(void) +{ + // pending reset request... + if (tribResetEsc) { + tribResetEsc = false; + + tribUncSetup = TRIB_UNCSETUP_INACTIVE; + tribInvalidParams = 0; + tribDirtyParams = 0; + paramMspActive = false; + + uint16_t reset = 0; + tribBuildReq(0xD0, 0x00, &reset, 2, TRIB_PARAM_FRAME_PERIOD, TRIB_PARAM_WRITE_TIMEOUT); + return true; + } + + // ...or pending write request... + uint8_t offset = 0; + for (uint16_t *pal = tribParamAddrLen, dirtybits = tribDirtyParams; dirtybits != 0; pal++, dirtybits >>= 1) { + const uint8_t addr = (*pal & TRIB_PAL_ADDR_MASK) >> 8; + const uint8_t len = *pal & TRIB_PAL_LEN_MASK; + if ((dirtybits & 0x01) != 0) { + void *payload; + uint32_t iq22Payload[2]; + if (addr == TRIB_PARAM_IQ22_ADDR) { + // convert scaled uint32 -> IQ22 + const uint32_t q22 = 1 << 22; + uint32_t *pw = iq22Payload; + uint32_t *pp = (uint32_t*)(paramUpdPayload + offset); + *pw++ = (((float)*pp++) / 100 * q22); + *pw = (((float)*pp) / 100000 * q22); + payload = iq22Payload; + } + else { + payload = paramUpdPayload + offset; + } + tribBuildReq(TRIB_PKT_WRITE_SETTINGS, addr, payload, len, TRIB_PARAM_FRAME_PERIOD, TRIB_PARAM_WRITE_TIMEOUT); + return true; + } + offset += len; + } + + // ...or schedule pending read request if no pending writes... + for (uint16_t *pal = tribParamAddrLen, invalidbits = tribInvalidParams; invalidbits != 0; pal++, invalidbits >>= 1) { + if ((invalidbits & 0x01) != 0) { + uint8_t req = (*pal & TRIB_PAL_SYS_MASK) != 0 ? TRIB_PKT_READ_SYSTEM : TRIB_PKT_READ_SETTINGS; + uint8_t addr = (*pal & TRIB_PAL_ADDR_MASK) >> 8; + uint8_t len = *pal & TRIB_PAL_LEN_MASK; + tribBuildReq(req, addr, NULL, len, TRIB_PARAM_FRAME_PERIOD, TRIB_PARAM_READ_TIMEOUT); + return true; + } + } + + // ...or nothing pending + return false; +} + +static bool tribDecodeReadParamResp(uint8_t addr) +{ + uint8_t offset = 0; + for (uint8_t i = 0; i < ARRAYLEN(tribParamAddrLen); i++) { + const uint16_t *pal = tribParamAddrLen + i; + const uint8_t len = *pal & TRIB_PAL_LEN_MASK; + if ((*pal >> 8) == addr) { + // cache params by addr + if (addr == TRIB_PARAM_IQ22_ADDR) { + // convert IQ22 -> scaled uint32 + const uint32_t q22 = 1 << 22; + uint32_t *pr = (uint32_t*)(buffer + TRIB_HEADER_LENGTH); + uint32_t *pp = (uint32_t*)(paramPayload + offset); + *pp++ = roundf(((float)*pr++) / q22 * 100); + *pp = roundf(((float)*pr) / q22 * 100000); + } + else { + memcpy(paramPayload + offset, buffer + TRIB_HEADER_LENGTH, len); + } + // clear invalid bit + tribInvalidParams &= ~(1 << i); + + // make param payload available if all params cached + if (tribInvalidParams == 0 && paramPayloadLength == 0) + tribUncSetup = TRIB_UNCSETUP_PARAMSREADY; + return true; + } + offset += len; + } + return false; +} + +static bool tribDecodeWriteParamResp(uint8_t addr) +{ + for (uint8_t i = 0; i < ARRAYLEN(tribParamAddrLen); i++) { + const uint16_t *pal = tribParamAddrLen + i; + if ((*pal >> 8) == addr) { + // clear dirty bit and set invalid bit force read + const uint16_t addrbit = (1 << i); + tribDirtyParams &= ~addrbit; + tribInvalidParams |= addrbit; + return true; + } + } + return false; +} + +static bool tribValidateResponseHeader(void) +{ + // req and resp headers should match except for len ([4]) which may differ + for (int i = 0; i < TRIB_HEADER_LENGTH; i++) { + if (i == 4) + continue; + if (buffer[i] != reqbuffer[i]) + return false; + } + return true; +} + +static bool tribDecodeReadSettingResp(uint8_t sysbit) +{ + // validate header + if (!tribValidateResponseHeader()) + return false; + + const uint8_t addr = buffer[3]; + if (!tribDecodeReadParamResp(addr | sysbit) || !tribBuildNextParamReq()) + rrfsmInvalidateReq(); + + return true; +} + +static bool tribDecodeWriteSettingResp(void) +{ + // validate header + if (!tribValidateResponseHeader()) + return false; + + const uint8_t addr = buffer[3]; + if (!tribDecodeWriteParamResp(addr) || !tribBuildNextParamReq()) + rrfsmInvalidateReq(); + + return true; +} + +static bool tribDecodeLogRecord(uint8_t hl) +{ + // payload: 16 byte (Log_rec_t) + const uint16_t rpm = buffer[hl + 14] << 8 | buffer[hl + 13]; + const uint16_t temp = buffer[hl + 10]; + const uint16_t power = buffer[hl + 11]; + const uint16_t voltage = buffer[hl + 7] << 8 | buffer[hl + 6]; + const uint16_t current = buffer[hl + 5] << 8 | buffer[hl + 4]; + const uint16_t capacity = buffer[hl + 9] << 8 | buffer[hl + 8]; + const uint16_t status = buffer[hl + 15]; + const uint16_t voltBEC = buffer[hl + 12]; + + escSensorData[0].age = 0; + escSensorData[0].erpm = rpm * 5; + escSensorData[0].pwm = power * 5; + escSensorData[0].voltage = voltage * 100; + escSensorData[0].current = current * 100; + escSensorData[0].consumption = capacity; + escSensorData[0].temperature = temp * 10; + escSensorData[0].bec_voltage = voltBEC * 100; + + DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 5); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, voltage * 100); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, current * 100); + + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, rpm); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_PWM, power); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_TEMP, temp); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, voltage); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, current); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, capacity); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, status); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_AGE, 0); + + return true; +} + +static bool tribDecodeReadStatusResp(void) +{ + // validate header (no CRC) + if (!tribValidateResponseHeader()) + return false; + + const uint8_t addr = buffer[3]; + if (tribUncSetup == TRIB_UNCSETUP_ABORTUNC && addr == 0) { + tribUncSetup = TRIB_UNCSETUP_ACTIVE; + paramPayloadLength = tribCalcParamBufferLength(); + rrfsmInvalidateReq(); + return true; + } + return false; +} + +static bool tribDecodeUNCFrame(void) +{ + // validate CRC, decode as 4 byte header + 16 byte (Log_rec_t) payload + const uint16_t crc = buffer[rrfsmFrameLength - 1] << 8 | buffer[rrfsmFrameLength - 2]; + if (calculateCRC16_CCITT(buffer, 20) != crc || !tribDecodeLogRecord(TRIB_UNC_HEADER_LENGTH)) + return false; + + rrfsmFrameTimeout = TRIB_UNC_FRAME_TIMEOUT; + return true; +} + +static bool tribDecode(timeMs_t currentTimeMs) +{ + UNUSED(currentTimeMs); + + const uint8_t req = buffer[0]; + switch (req) { + case TRIB_PKT_READ_STATUS: + return tribDecodeReadStatusResp(); + case TRIB_PKT_READ_SYSTEM: + return tribDecodeReadSettingResp(0x80); + case TRIB_PKT_READ_SETTINGS: + return tribDecodeReadSettingResp(0x00); + case TRIB_PKT_WRITE_SETTINGS: + return tribDecodeWriteSettingResp(); + case TRIB_PKT_UNSOLICITED: + return tribDecodeUNCFrame(); + default: + return false; + } +} + +static bool tribCrankUncSetup(timeMs_t currentTimeMs) +{ + switch(tribUncSetup) { + case TRIB_UNCSETUP_INACTIVE: + case TRIB_UNCSETUP_ABORTUNC: + // unexpected but required to keep compiler happy + break; + case TRIB_UNCSETUP_PARAMSREADY: + if (paramMspActive && currentTimeMs < rrfsmFrameTimestamp + 4000) { + // try to abort UNC mode + tribUncSetup = TRIB_UNCSETUP_ABORTUNC; + tribBuildReq(TRIB_PKT_READ_STATUS, 0, NULL, 0x10, TRIB_FRAME_PERIOD, TRIB_RESP_FRAME_TIMEOUT); + } + break; + case TRIB_UNCSETUP_ACTIVE: + if (tribBuildNextParamReq()) { + tribUncSetup = TRIB_UNCSETUP_WAIT; + } + break; + case TRIB_UNCSETUP_WAIT: + if (tribInvalidParams == 0 && tribDirtyParams == 0) { + tribUncSetup = TRIB_UNCSETUP_ACTIVE; + rrfsmInvalidateReq(); + } + break; + } + return true; +} + +static bool tribStart(timeMs_t currentTimeMs) +{ + UNUSED(currentTimeMs); + + tribBuildNextParamReq(); + return true; +} + +static int8_t tribAccept(uint16_t c) +{ + if (readBytes == 1) { + // req / singature + if ((c & TRIB_REQ_SIG) != TRIB_REQ_SIG) + return -1; + } + else if (buffer[0] == TRIB_PKT_UNSOLICITED && readBytes == 3) { + // UNC length + if (c > TELEMETRY_BUFFER_SIZE) { + // protect against buffer overflow + return -1; + } + else { + // new frame length for this frame (header + data) + rrfsmFrameLength = c; + return 1; + } + } + else if (buffer[0] != TRIB_PKT_UNSOLICITED && readBytes == 5) { + // STD length + if (c > TELEMETRY_BUFFER_SIZE) { + // protect against buffer overflow + return -1; + } + else if((c & 0x01) || c > 32) { + // invalid length (must not be more than 32 bytes and should be a multiple of 2 bytes) + return -1; + } + else { + // new frame length for this frame - read = (header + data), write = (header) + rrfsmFrameLength = (buffer[0] & TRIB_REQ_WRITE) == 0 ? TRIB_HEADER_LENGTH + c : TRIB_HEADER_LENGTH; + return 1; + } + } + return 0; +} + +static serialReceiveCallbackPtr tribSensorInit(bool bidirectional) +{ + rrfsmMinFrameLength = TRIB_HEADER_LENGTH; + rrfsmAccept = tribAccept; + rrfsmDecode = tribDecode; + + if (bidirectional) { + // request/response telemetry + rrfsmBootDelayMs = TRIB_REQ_BOOT_DELAY; + rrfsmStart = tribStart; + + // enable ESC parameter reads and writes, reset + paramSig = TRIB_PARAM_SIG; + paramVer = 0 | TRIB_PARAM_CAP_RESET; + paramCommit = tribParamCommit; + tribInvalidateParams(); + + // enable UNC setup FSM + rrfsmCrank = tribCrankUncSetup; + } + else { + // telemetry data only + rrfsmBootDelayMs = TRIB_BOOT_DELAY; + rrfsmFrameTimeout = TRIB_UNC_FRAME_TIMEOUT; + } + + return rrfsmDataReceive; +} + + /* * OpenYGE Telemetry * @@ -1556,61 +2129,75 @@ static void apdSensorProcess(timeUs_t currentTimeUs) * WARN_OVERTEMP = 0x20 // Fail if Motor Status == STATE_POWER_CUT * WARN_OVERAMP = 0x40 // Fail if Motor Status == STATE_POWER_CUT * WARN_SETPOINT_NOISE = 0xC0 // note this is special case (can never have OVERAMP w/ BEC hence reuse) - * - * Data Frame Format - * ――――――――――――――――――――――――――――――――――――――――――――――― - * Header... - * 0: sync; // sync, 0xA5 - * 1: version; // frame version - * 2: frame_type // telemetry data = 0 - * 3: frame_length; // frame length including header and CRC - * 4: reserved // reserved - * 5: reserved // reserved - * - * Payload... - * +0: reserved // reserved - * 1: temperature; // C degrees (0-> -40°C, 255->215°C) - * 2,3: voltage; // 0.01V Little endian! - * 4,5: current; // 0.01A Little endian! - * 6,7: consumption; // mAh Little endian! - * 8,9: rpm; // 0.1rpm Little endian! - * 10: pwm; // % - * 11: throttle; // % - * 12,13: bec_voltage; // 0.01V Little endian! - * 14,15: bec_current; // 0.01A Little endian! - * 16: bec_temp; // C degrees (0-> -40°C, 255->215°C) - * 17: status1; // see documentation - * 18: cap_temp; // C degrees (0-> -40°C, 255->215°C) - * 19: aux_temp; // C degrees (0-> -40°C, 255->215°C) - * 20: status2; // reserved - * 21: reserved1; // reserved - * 22,23: idx; // maybe future use - * 24,25: idx_data; // maybe future use - * - * 32,33: crc16; // CCITT, poly: 0x1021 - * */ -#define OPENYGE_SYNC 0xA5 // sync -#define OPENYGE_BOOT_DELAY 5000 // 5 seconds -#define OPENYGE_RAMP_INTERVAL 6000 // 6 seconds -#define OPENYGE_FRAME_PERIOD_INITIAL 900 // intially 800 w/ progressive decreasing frame-period during ramp time... -#define OPENYGE_FRAME_PERIOD_FINAL 60 // ...to the final 50ms -#define OPENYGE_FRAME_MIN_LENGTH 6 // assume minimum frame (header + CRC) until actual length of current frame known - -#define OPENYGE_TEMP_OFFSET 40 - -enum { - OPENYGE_FRAME_FAILED = 0, - OPENYGE_FRAME_PENDING = 1, - OPENYGE_FRAME_COMPLETE = 2, -}; - -static timeMs_t oygeRampTimer = 0; -static uint32_t oygeFrameTimestamp = 0; -static uint16_t oygeFramePeriod = OPENYGE_FRAME_PERIOD_INITIAL; -static volatile uint8_t oygeFrameLength = 0; - +#define OPENYGE_SYNC 0xA5 // sync +#define OPENYGE_VERSION 3 // protocol version +#define OPENYGE_HEADER_LENGTH 6 // header length +#define OPENYGE_HEADER_LENGTH_LEGACY 4 // legacy (pre-v3) header length +#define OPENYGE_CRC_LENGTH 2 // CRC length +#define OPENYGE_BOOT_DELAY 5000 // 5 seconds +#define OPENYGE_MIN_FRAME_LENGTH OPENYGE_HEADER_LENGTH_LEGACY + OPENYGE_CRC_LENGTH // assume minimum frame (legacy header + CRC) until actual length of current frame known +#define OPENYGE_AUTO_INITIAL_FRAME_TIMEOUT 900 // intially ~800ms w/ progressive decreasing frame-period... +#define OPENYGE_AUTO_MIN_FRAME_TIMEOUT 60U // ...to final ~50ms (no less) + +#define OPENYGE_FRAME_PERIOD_INIT 20 // delay before sending first master request after v3+ auto telemetry frame seen (possibly last chained) +#define OPENYGE_FRAME_PERIOD 38 // aim for approx. 50ms/20Hz +#define OPENYGE_PARAM_FRAME_PERIOD 4 // TBD ASAP? +#define OPENYGE_REQ_READ_TIMEOUT 200 // Response timeout for read requests TBD: confirm +#define OPENYGE_REQ_WRITE_TIMEOUT 400 // Response timeout for write requests TBD: confirm + +#define OPENYGE_PARAM_SIG 0xA5 // parameter payload signature for this ESC +#define OPENYGE_PARAM_CACHE_SIZE_MAX 64 // limited by use of uint64_t as bit array for oygeCachedParams + +#define OPENYGE_FTYPE_TELE_AUTO 0x00 // auto telemetry frame +#define OPENYGE_FTYPE_TELE_RESP 0x02 // telemetry response +#define OPENYGE_FTYPE_TELE_REQ 0x03 // telemetry request +#define OPENYGE_FTYPE_WRITE_PARAM_RESP 0x04 // write param response +#define OPENYGE_FTYPE_WRITE_PARAM_REQ 0x05 // write param request + +#define OPENYGE_DEV_MASTER 0x80 // device address master role bit + +#define OPENYGE_TEMP_OFFSET 40 + +typedef struct { + uint8_t sync; // sync 0xA5 + uint8_t version; // version + uint8_t frame_type; // high bit 0x00 for read, 0x80 for write, e.g 0x00 - telemetry frame data, 0x81 - update parameter + uint8_t frame_length; // frame length + uint8_t seq; // count for packet-control/assignment master counts, slave(ESC) sends this value back + uint8_t device; // ESC address, 0x80 for master and 7 bits for ESC address 0x01…0x7F where 0x00 might mean all ESCs +} OpenYGEHeader_t; + +typedef struct { + uint8_t reserved; // reserved + uint8_t temperature; // C degrees+40 (-40..215°C) + uint16_t voltage; // 0.01V + uint16_t current; // 0.01A + uint16_t consumption; // mAh + uint16_t rpm; // 0.1erpm + int8_t pwm; // % + int8_t throttle; // % + uint16_t bec_voltage; // 0.001V + uint16_t bec_current; // 0.001A + uint8_t bec_temp; // C degrees + uint8_t status1; // see documentation + uint8_t cap_temp; // C degrees+40 (-40..215°C) + uint8_t aux_temp; // C degrees+40 (-40..215°C) + uint8_t status2; // Debug/Reserved + uint8_t reserved1; // Debug/Reserved maybe consumption high-byte for more than 65Ah(industrial) + uint16_t pidx; // maybe future use + uint16_t pdata; // maybe future use +} OpenYGETelemetryFrame_t; + +typedef struct { + uint16_t index; // parameter addr + uint16_t param; // parameter value +} OpenYGEControlFrame_t; + +static uint16_t oygeAutoFrameTimeout = OPENYGE_AUTO_INITIAL_FRAME_TIMEOUT; +static uint64_t oygeCachedParams = 0; +static uint64_t oygeDirtyParams = 0; static uint16_t oygeCalculateCRC16_CCITT(const uint8_t *ptr, size_t len) { @@ -1631,165 +2218,278 @@ static uint16_t oygeCalculateCRC16_CCITT(const uint8_t *ptr, size_t len) return crc; } -static void oygeFrameSyncError(void) +static bool oygeParamCommit(uint8_t cmd) { - readBytes = 0; - syncCount = 0; - - totalSyncErrorCount++; + if (cmd == 0) { + // save page + // find dirty params, skip para[0] (parameter count) + uint16_t *ygeParams = (uint16_t*)paramPayload; + const uint16_t *ygeUpdParams = (uint16_t*)paramUpdPayload; + if (ygeParams[0] != ygeUpdParams[0]) + return false; + + const uint16_t ygeParamCount = ygeParams[0]; + for (uint8_t idx = 1; idx < ygeParamCount; idx++) { + // schedule writes for dirty params + if (ygeParams[idx] != ygeUpdParams[idx]) { + // set dirty bit + oygeDirtyParams |= (1ULL << idx); + // invalidate param + oygeCachedParams &= ~(1ULL << idx); + // invalidate param payload - will be available again when all params again cached + paramPayloadLength = 0; + } + } + return true; + } + else { + return false; + } } -static FAST_CODE void oygeDataReceive(uint16_t c, void *data) +static void oygeCacheParam(uint8_t pidx, uint16_t pdata) { - UNUSED(data); + const uint8_t maxParams = PARAM_BUFFER_SIZE / 2; + if (pidx >= maxParams || pidx >= OPENYGE_PARAM_CACHE_SIZE_MAX) + return; - totalByteCount++; + // don't accept params until pending writes cleared + if (oygeDirtyParams != 0) + return; - if (readBytes < oygeFrameLength) { - buffer[readBytes++] = c; + uint16_t *ygeParams = (uint16_t*)paramPayload; + ygeParams[pidx] = pdata; + oygeCachedParams |= (1ULL << pidx); - if (readBytes == 1) { - // sync - if (c != OPENYGE_SYNC) - oygeFrameSyncError(); - } - else if (readBytes == 3) { - if (c != 0) { - // unsupported frame type - oygeFrameSyncError(); - } - } - else if (readBytes == 4) { - // frame length - // protect against buffer overflow - if (c < OPENYGE_FRAME_MIN_LENGTH || c > TELEMETRY_BUFFER_SIZE) { - oygeFrameSyncError(); - } - else { - oygeFrameLength = c; - syncCount++; - } - } + // skip if count already known (parameter data ready) or count parameter not yet seen (param[0] for YGE) + if (paramPayloadLength > 0 || (oygeCachedParams & 0x01) == 0) + return; + + // make param payload available if all params cached + const uint16_t ygeParamCount = ygeParams[0]; + if (ygeParamCount > 0 && ygeParamCount <= OPENYGE_PARAM_CACHE_SIZE_MAX && + ~(~1ULL << (ygeParamCount - 1)) == oygeCachedParams) { + paramPayloadLength = ygeParamCount * 2; } } -static void oygeStartTelemetryFrame(timeMs_t currentTimeMs) +static void oygeBuildReq(uint8_t req, uint8_t device, void *payload, uint8_t len, uint16_t framePeriod, uint16_t frameTimeout) { - readBytes = 0; - oygeFrameLength = OPENYGE_FRAME_MIN_LENGTH; - oygeFrameTimestamp = currentTimeMs; + OpenYGEHeader_t *hdr = (OpenYGEHeader_t*)reqbuffer; + reqLength = sizeof(*hdr) + len + OPENYGE_CRC_LENGTH; // header + payload + crc + if (reqLength > REQUEST_BUFFER_SIZE) + return; + + hdr->sync = OPENYGE_SYNC; + hdr->version = OPENYGE_VERSION; + hdr->frame_type = req; + hdr->frame_length = reqLength; + hdr->seq++; // advance sequence number, overlapped operations not supported by this implementation + hdr->device = device | OPENYGE_DEV_MASTER; // as master + + if (payload != NULL) + memcpy(hdr + 1, payload, len); + + *((uint16_t*)(reqbuffer + reqLength - OPENYGE_CRC_LENGTH)) = oygeCalculateCRC16_CCITT(reqbuffer, reqLength - OPENYGE_CRC_LENGTH); + + rrfsmFramePeriod = framePeriod; + rrfsmFrameTimeout = frameTimeout; } -static uint8_t oygeDecodeTelemetryFrame(void) +static void oygeBuildNextReq(const OpenYGEHeader_t *hdr) { - // First, check the variables that can change in the interrupt - if (readBytes < oygeFrameLength) - return OPENYGE_FRAME_PENDING; - - // paranoid length check - uint8_t len = buffer[3]; - if (len < OPENYGE_FRAME_MIN_LENGTH || len > TELEMETRY_BUFFER_SIZE) { - totalCrcErrorCount++; - return OPENYGE_FRAME_FAILED; - } - // verify CRC16 checksum - uint16_t crc = buffer[len - 1] << 8 | buffer[len - 2]; - if (oygeCalculateCRC16_CCITT(buffer, len - 2) != crc) { - totalCrcErrorCount++; - return OPENYGE_FRAME_FAILED; + OpenYGEControlFrame_t ctl; + + // schedule pending write request... + const uint16_t *ygeUpdParams = (uint16_t*)paramUpdPayload; + for (uint8_t idx = 0; oygeDirtyParams != 0; idx++) { + uint64_t bit = 1ULL << idx; + // index dirty? + if ((oygeDirtyParams & bit) != 0) { + // clear dirty bit + oygeDirtyParams &= ~(bit); + + // schedule write request + ctl.index = idx; + ctl.param = ygeUpdParams[idx]; + oygeBuildReq(OPENYGE_FTYPE_WRITE_PARAM_REQ, 1, &ctl, sizeof(ctl), OPENYGE_PARAM_FRAME_PERIOD, OPENYGE_REQ_WRITE_TIMEOUT); + return; + } } - uint8_t version = buffer[1]; - uint8_t hl = (version >= 3) ? 6 : 4; + // ...or nothing pending, schedule read telemetry request + ctl.index = ctl.param = 0; + const uint8_t framePeriod = hdr->frame_type == OPENYGE_FTYPE_TELE_AUTO ? OPENYGE_FRAME_PERIOD_INIT : OPENYGE_FRAME_PERIOD; + oygeBuildReq(OPENYGE_FTYPE_TELE_REQ, 1, &ctl, sizeof(ctl), framePeriod, OPENYGE_REQ_READ_TIMEOUT); +} - int16_t temp = buffer[hl+1]; - uint16_t volt = buffer[hl+3] << 8 | buffer[hl+2]; - uint16_t curr = buffer[hl+5] << 8 | buffer[hl+4]; - uint16_t capa = buffer[hl+7] << 8 | buffer[hl+6]; - uint16_t erpm = buffer[hl+9] << 8 | buffer[hl+8]; - uint8_t pwm = buffer[hl+10]; - uint16_t voltBEC = buffer[hl+13] << 8 | buffer[hl+12]; - int16_t tempBEC = buffer[hl+16]; +static void oygeDecodeTelemetryFrame(void) +{ + const OpenYGEHeader_t *hdr = (OpenYGEHeader_t*)buffer; + const OpenYGETelemetryFrame_t *tele = (OpenYGETelemetryFrame_t*)(buffer + (hdr->version >= 3 ? OPENYGE_HEADER_LENGTH : OPENYGE_HEADER_LENGTH_LEGACY)); - if (version >= 2) { - // apply temperature mapping offsets - temp -= OPENYGE_TEMP_OFFSET; - tempBEC -= OPENYGE_TEMP_OFFSET; - } + int16_t temp = tele->temperature - OPENYGE_TEMP_OFFSET; + int16_t tempBEC = tele->bec_temp - OPENYGE_TEMP_OFFSET; escSensorData[0].age = 0; - escSensorData[0].erpm = erpm * 10; - escSensorData[0].pwm = pwm * 10; - escSensorData[0].voltage = volt * 10; - escSensorData[0].current = curr * 10; - escSensorData[0].consumption = capa; + escSensorData[0].erpm = tele->rpm * 10; + escSensorData[0].pwm = tele->pwm * 10; + escSensorData[0].throttle = tele->throttle * 10; + escSensorData[0].voltage = tele->voltage * 10; + escSensorData[0].current = tele->current * 10; + escSensorData[0].consumption = tele->consumption; escSensorData[0].temperature = temp * 10; escSensorData[0].temperature2 = tempBEC * 10; + escSensorData[0].bec_voltage = tele->bec_voltage; + escSensorData[0].bec_current = tele->bec_current; + escSensorData[0].status = tele->status1 | 0x0100; - totalFrameCount++; + oygeCacheParam(tele->pidx, tele->pdata); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, erpm * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, tele->rpm * 10); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, volt); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, curr); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, tele->voltage * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, tele->current * 10); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, erpm); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_PWM, pwm); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, tele->rpm); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_PWM, tele->pwm); DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_TEMP, temp); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, volt); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, curr); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, capa); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, voltBEC); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, tele->voltage); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, tele->current); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, tele->consumption); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, tele->status1); DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_AGE, 0); +} - return OPENYGE_FRAME_COMPLETE; +static const OpenYGEHeader_t *oygeGetHeaderWithCrcCheck() +{ + // get header (w/ paranoid buffer access) + const OpenYGEHeader_t *hdr = (OpenYGEHeader_t*)buffer; + const uint8_t len = hdr->frame_length; + if (len < OPENYGE_MIN_FRAME_LENGTH || len > TELEMETRY_BUFFER_SIZE) + return NULL; + + // check CRC + const uint16_t crc = *(uint16_t*)(buffer + len - OPENYGE_CRC_LENGTH); + if (oygeCalculateCRC16_CCITT(buffer, len - OPENYGE_CRC_LENGTH) != crc) + return NULL; + + return hdr; } -static void oygeSensorProcess(timeUs_t currentTimeUs) +static bool oygeDecodeAuto(timeMs_t currentTimeMs) { - const timeMs_t currentTimeMs = currentTimeUs / 1000; + // get header w/ CRC check + const OpenYGEHeader_t *hdr = oygeGetHeaderWithCrcCheck(); + if (hdr == NULL) + return false; - // wait before initializing - if (currentTimeMs < OPENYGE_BOOT_DELAY) - return; + // decode payload + oygeDecodeTelemetryFrame(); - // one time init - if (oygeFrameTimestamp == 0) { - oygeStartTelemetryFrame(currentTimeMs); - return; + // adjust auto frame timeout (10ms + last seen decreasing interval until min) + if (oygeAutoFrameTimeout > OPENYGE_AUTO_MIN_FRAME_TIMEOUT && currentTimeMs - rrfsmFrameTimestamp < oygeAutoFrameTimeout) { + oygeAutoFrameTimeout = MAX(OPENYGE_AUTO_MIN_FRAME_TIMEOUT, currentTimeMs - rrfsmFrameTimestamp + 10); } + rrfsmFrameTimeout = oygeAutoFrameTimeout; - // switch to final frame timeout if ramp time complete - if (oygeFramePeriod == OPENYGE_FRAME_PERIOD_INITIAL && oygeRampTimer != 0 && currentTimeMs > oygeRampTimer) - oygeFramePeriod = OPENYGE_FRAME_PERIOD_FINAL; + return true; +} - // timeout waiting for frame? - if (currentTimeMs > oygeFrameTimestamp + oygeFramePeriod) { - increaseDataAge(0); - oygeStartTelemetryFrame(currentTimeMs); - totalTimeoutCount++; - return; +static bool oygeDecodeTelemetry(const OpenYGEHeader_t *hdr, timeMs_t currentTimeMs) +{ + // switch to auto telemetry mode if ESC FW too old + if (hdr->version < 3) { + rrfsmDecode = oygeDecodeAuto; + paramCommit = NULL; + return oygeDecodeAuto(currentTimeMs); } - // attempt to decode frame - uint8_t state = oygeDecodeTelemetryFrame(); - switch (state) { - case OPENYGE_FRAME_PENDING: - // frame not ready yet - break; - case OPENYGE_FRAME_FAILED: - increaseDataAge(0); - // next frame - oygeStartTelemetryFrame(currentTimeMs); - break; - case OPENYGE_FRAME_COMPLETE: - // start ramp timer if first frame seen - if (oygeRampTimer == 0) - oygeRampTimer = currentTimeMs + OPENYGE_RAMP_INTERVAL; - // next frame - oygeStartTelemetryFrame(currentTimeMs); - break; + // response sequence number should match request (ignore auto telemetry) + const OpenYGEHeader_t *req = (OpenYGEHeader_t*)reqbuffer; + if (hdr->frame_type != OPENYGE_FTYPE_TELE_AUTO && hdr->seq != req->seq) + return false; + + // decode payload + oygeDecodeTelemetryFrame(); + + // schedule next request + oygeBuildNextReq(hdr); + + return true; +} + +static bool oygeDecode(timeMs_t currentTimeMs) +{ + // get header w/ CRC check + const OpenYGEHeader_t *hdr = oygeGetHeaderWithCrcCheck(); + if (hdr == NULL) + return false; + + switch (hdr->frame_type) { + case OPENYGE_FTYPE_TELE_AUTO: + case OPENYGE_FTYPE_TELE_RESP: + case OPENYGE_FTYPE_WRITE_PARAM_RESP: + return oygeDecodeTelemetry(hdr, currentTimeMs); + default: + return false; + } +} + +static int8_t oygeAccept(uint16_t c) +{ + if (readBytes == 1) { + // sync + if (c != OPENYGE_SYNC) + return -1; + } + else if (readBytes == 3) { + switch (c) { + case OPENYGE_FTYPE_TELE_AUTO: + case OPENYGE_FTYPE_TELE_REQ: + case OPENYGE_FTYPE_TELE_RESP: + case OPENYGE_FTYPE_WRITE_PARAM_REQ: + case OPENYGE_FTYPE_WRITE_PARAM_RESP: + break; + default: + // unsupported frame type + return -1; + } + } + else if (readBytes == 4) { + // frame length + if (c < OPENYGE_MIN_FRAME_LENGTH || c > TELEMETRY_BUFFER_SIZE) { + // protect against buffer underflow/overflow + return -1; + } + else { + // new frame length for this frame + rrfsmFrameLength = c; + return 1; + } } + return 0; +} + +static serialReceiveCallbackPtr oygeSensorInit(bool bidirectional) +{ + rrfsmBootDelayMs = OPENYGE_BOOT_DELAY; + rrfsmMinFrameLength = OPENYGE_MIN_FRAME_LENGTH; + rrfsmAccept = oygeAccept; + + paramSig = OPENYGE_PARAM_SIG; + + if (bidirectional) { + // use request/response telemetry mode, enable parameter writes to ESC + rrfsmDecode = oygeDecode; + paramCommit = oygeParamCommit; + } + else { + // use auto telemetry mode + rrfsmDecode = oygeDecodeAuto; + } + + return rrfsmDataReceive; } @@ -1829,7 +2529,7 @@ void escSensorProcess(timeUs_t currentTimeUs) hw5SensorProcess(currentTimeUs); break; case ESC_SENSOR_PROTO_SCORPION: - uncSensorProcess(currentTimeUs); + rrfsmSensorProcess(currentTimeUs); break; case ESC_SENSOR_PROTO_KONTRONIK: kontronikSensorProcess(currentTimeUs); @@ -1844,7 +2544,7 @@ void escSensorProcess(timeUs_t currentTimeUs) apdSensorProcess(currentTimeUs); break; case ESC_SENSOR_PROTO_OPENYGE: - oygeSensorProcess(currentTimeUs); + rrfsmSensorProcess(currentTimeUs); break; case ESC_SENSOR_PROTO_RECORD: recordSensorProcess(currentTimeUs); @@ -1863,6 +2563,7 @@ void escSensorProcess(timeUs_t currentTimeUs) bool INIT_CODE escSensorInit(void) { + const bool escHalfDuplex = escSensorConfig()->halfDuplex; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR); serialReceiveCallbackPtr callback = NULL; portOptions_e options = 0; @@ -1872,7 +2573,7 @@ bool INIT_CODE escSensorInit(void) return false; } - options = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED | (escSensorConfig()->halfDuplex ? SERIAL_BIDIR : 0); + options = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED | (escHalfDuplex ? SERIAL_BIDIR : 0); switch (escSensorConfig()->protocol) { case ESC_SENSOR_PROTO_BLHELI32: @@ -1883,6 +2584,7 @@ bool INIT_CODE escSensorInit(void) baudrate = 19200; break; case ESC_SENSOR_PROTO_SCORPION: + callback = tribSensorInit(escHalfDuplex); baudrate = 38400; break; case ESC_SENSOR_PROTO_KONTRONIK: @@ -1896,7 +2598,7 @@ bool INIT_CODE escSensorInit(void) baudrate = 115200; break; case ESC_SENSOR_PROTO_OPENYGE: - callback = oygeDataReceive; + callback = oygeSensorInit(escHalfDuplex); baudrate = 115200; break; case ESC_SENSOR_PROTO_RECORD: @@ -1905,7 +2607,7 @@ bool INIT_CODE escSensorInit(void) } if (baudrate) { - escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, callback, NULL, baudrate, MODE_RX, options); + escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, callback, NULL, baudrate, escHalfDuplex ? MODE_RXTX : MODE_RX, options); } for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { @@ -1917,4 +2619,30 @@ bool INIT_CODE escSensorInit(void) return (escSensorPort != NULL); } + +uint8_t escGetParamBufferLength(void) +{ + paramMspActive = true; + return paramPayloadLength != 0 ? PARAM_HEADER_SIZE + paramPayloadLength : 0; +} + +uint8_t *escGetParamBuffer(void) +{ + paramBuffer[PARAM_HEADER_SIG] = paramSig; + paramBuffer[PARAM_HEADER_VER] = paramVer | (paramCommit == NULL ? PARAM_HEADER_RDONLY : 0); + return paramBuffer; +} + +uint8_t *escGetParamUpdBuffer(void) +{ + return paramUpdBuffer; +} + +bool escCommitParameters(void) +{ + return paramUpdBuffer[PARAM_HEADER_SIG] == paramBuffer[PARAM_HEADER_SIG] && + (paramUpdBuffer[PARAM_HEADER_VER] & PARAM_HEADER_VER_MASK) == (paramBuffer[PARAM_HEADER_VER] & PARAM_HEADER_VER_MASK) && + paramCommit != NULL && paramCommit(paramUpdBuffer[PARAM_HEADER_VER] & PARAM_HEADER_CMD_MASK); +} + #endif diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index 91f03be064..32e96d2d09 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -56,14 +56,18 @@ typedef struct escSensorConfig_s { PG_DECLARE(escSensorConfig_t, escSensorConfig); typedef struct { - uint8_t age; // Data age - uint16_t pwm; // Output duty cycle 0.1% - uint32_t erpm; // eRPM - uint32_t voltage; // mV - uint32_t current; // mA - uint32_t consumption; // mAh - int16_t temperature; // 0.1°C - int16_t temperature2; // 0.1°C + uint8_t age; // Data age + uint16_t pwm; // Output duty cycle 0.1% + uint16_t throttle; // Input setpoint 0.1% + uint32_t erpm; // eRPM + uint32_t voltage; // mV + uint32_t current; // mA + uint32_t consumption; // mAh + int16_t temperature; // 0.1°C + int16_t temperature2; // 0.1°C + uint32_t bec_voltage; // mV + uint32_t bec_current; // mA + uint32_t status; // status / fault codes } escSensorData_t; #define ESC_DATA_INVALID 255 @@ -79,3 +83,8 @@ bool isEscSensorActive(void); uint32_t getEscSensorRPM(uint8_t motorNumber); escSensorData_t *getEscSensorData(uint8_t motorNumber); + +uint8_t escGetParamBufferLength(void); +uint8_t *escGetParamBuffer(void); +uint8_t *escGetParamUpdBuffer(void); +bool escCommitParameters(void); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 815de674bc..52360d1503 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -238,6 +238,17 @@ uint16 Altitude ( meter ­1000m offset ) uint8_t Satellites in use ( counter ) */ +static int getVoltageMeter(voltageMeterId_e id) +{ + voltageMeter_t meter; + + voltageMeterRead(id, &meter); + + // Use ratio 200 in EdgeTx 2.9.3 and 20 in earlier versions + // Max voltage 25.5V + return meter.voltage * 255 / 200; +} + static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) { escSensorData_t *escData; @@ -252,6 +263,27 @@ static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) case CRSF_GPS_REUSE_ESC_TEMP: escData = getEscSensorData(ESC_SENSOR_COMBINED); return (escData) ? escData->temperature : 0; + case CRSF_GPS_REUSE_ESC_PWM: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->pwm : 0; + case CRSF_GPS_REUSE_ESC_THROTTLE: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->throttle : 0; + case CRSF_GPS_REUSE_ESC_BEC_VOLTAGE: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_voltage : 0; + case CRSF_GPS_REUSE_ESC_BEC_CURRENT: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_current : 0; + case CRSF_GPS_REUSE_ESC_BEC_TEMP: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->temperature2 : 0; + case CRSF_GPS_REUSE_ESC_STATUS: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status & 0xFFFF) : 0; + case CRSF_GPS_REUSE_ESC_STATUS2: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status >> 16) : 0; case CRSF_GPS_REUSE_MCU_TEMP: return getCoreTemperatureCelsius() * 10; case CRSF_GPS_REUSE_MCU_LOAD: @@ -260,6 +292,12 @@ static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) return getAverageSystemLoad(); case CRSF_GPS_REUSE_RT_LOAD: return getMaxRealTimeLoad(); + case CRSF_GPS_REUSE_BEC_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_BEC); + case CRSF_GPS_REUSE_BUS_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_BUS); + case CRSF_GPS_REUSE_MCU_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_MCU); } return 0; @@ -417,17 +455,6 @@ static int16_t decidegrees2Radians10000(int16_t angle_decidegree) return (int16_t)(RAD * 1000.0f * angle_decidegree); } -static int getVoltageMeter(voltageMeterId_e id) -{ - voltageMeter_t meter; - - voltageMeterRead(id, &meter); - - // Use ratio 200 in EdgeTx 2.9.3 and 20 in earlier versions - // Max voltage 25.5V - return meter.voltage * 255 / 200; -} - static int16_t crsfAttitudeReuse(uint8_t reuse, int attitude) { escSensorData_t *escData; @@ -440,6 +467,24 @@ static int16_t crsfAttitudeReuse(uint8_t reuse, int attitude) case CRSF_ATT_REUSE_ESC_TEMP: escData = getEscSensorData(ESC_SENSOR_COMBINED); return (escData) ? escData->temperature * 10 : 0; + case CRSF_ATT_REUSE_ESC_PWM: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->pwm : 0; + case CRSF_ATT_REUSE_ESC_BEC_VOLTAGE: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_voltage : 0; + case CRSF_ATT_REUSE_ESC_BEC_CURRENT: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_current : 0; + case CRSF_ATT_REUSE_ESC_BEC_TEMP: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->temperature2 * 10 : 0; + case CRSF_ATT_REUSE_ESC_STATUS: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status & 0xFFFF) : 0; + case CRSF_ATT_REUSE_ESC_STATUS2: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status >> 16) : 0; case CRSF_ATT_REUSE_MCU_TEMP: return getCoreTemperatureCelsius() * 100; case CRSF_ATT_REUSE_MCU_LOAD: diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 58a4c7059a..ceaab554ff 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -49,6 +49,12 @@ enum { CRSF_ATT_REUSE_NONE = 0, CRSF_ATT_REUSE_THROTTLE, CRSF_ATT_REUSE_ESC_TEMP, + CRSF_ATT_REUSE_ESC_PWM, + CRSF_ATT_REUSE_ESC_BEC_VOLTAGE, + CRSF_ATT_REUSE_ESC_BEC_CURRENT, + CRSF_ATT_REUSE_ESC_BEC_TEMP, + CRSF_ATT_REUSE_ESC_STATUS, + CRSF_ATT_REUSE_ESC_STATUS2, CRSF_ATT_REUSE_MCU_TEMP, CRSF_ATT_REUSE_MCU_LOAD, CRSF_ATT_REUSE_SYS_LOAD, @@ -63,10 +69,20 @@ enum { CRSF_GPS_REUSE_HEADSPEED, CRSF_GPS_REUSE_THROTTLE, CRSF_GPS_REUSE_ESC_TEMP, + CRSF_GPS_REUSE_ESC_PWM, + CRSF_GPS_REUSE_ESC_THROTTLE, + CRSF_GPS_REUSE_ESC_BEC_VOLTAGE, + CRSF_GPS_REUSE_ESC_BEC_CURRENT, + CRSF_GPS_REUSE_ESC_BEC_TEMP, + CRSF_GPS_REUSE_ESC_STATUS, + CRSF_GPS_REUSE_ESC_STATUS2, CRSF_GPS_REUSE_MCU_TEMP, CRSF_GPS_REUSE_MCU_LOAD, CRSF_GPS_REUSE_SYS_LOAD, CRSF_GPS_REUSE_RT_LOAD, + CRSF_GPS_REUSE_BEC_VOLTAGE, + CRSF_GPS_REUSE_BUS_VOLTAGE, + CRSF_GPS_REUSE_MCU_VOLTAGE, }; enum {