Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added demag and stall events to edt status frame #12170

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
7 changes: 7 additions & 0 deletions src/main/build/debug.c
Expand Up @@ -117,4 +117,11 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"MAG_CALIB",
"MAG_TASK_RATE",
"EZLANDING",
"DSHOT_STATUS_N_TEMPERATURE",
"DSHOT_STATUS_N_VOLTAGE",
"DSHOT_STATUS_N_CURRENT",
"DSHOT_STATUS_N_DEBUG1",
"DSHOT_STATUS_N_DEBUG2",
"DSHOT_STATUS_N_STRESS_LVL",
"DSHOT_STATUS_N_ERPM_FRACTION_18",
};
7 changes: 7 additions & 0 deletions src/main/build/debug.h
Expand Up @@ -119,6 +119,13 @@ typedef enum {
DEBUG_MAG_CALIB,
DEBUG_MAG_TASK_RATE,
DEBUG_EZLANDING,
DEBUG_DSHOT_STATUS_N_TEMPERATURE,
DEBUG_DSHOT_STATUS_N_VOLTAGE,
DEBUG_DSHOT_STATUS_N_CURRENT,
DEBUG_DSHOT_STATUS_N_DEBUG1,
DEBUG_DSHOT_STATUS_N_DEBUG2,
DEBUG_DSHOT_STATUS_N_STRESS_LVL,
DEBUG_DSHOT_STATUS_N_ERPM_FRACTION_18,
DEBUG_COUNT
} debugType_e;

Expand Down
12 changes: 6 additions & 6 deletions src/main/cli/cli.c
Expand Up @@ -6111,10 +6111,10 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
cliPrintLinefeed();

#ifdef USE_DSHOT_TELEMETRY_STATS
cliPrintLine("Motor Type eRPM RPM Hz Invalid TEMP VCC CURR ST/EV DBG1 DBG2 DBG3");
cliPrintLine("Motor Type eRPM RPM Hz Invalid TEMP VCC CURR STAT DBG1 DBG2 DMET");
cliPrintLine("===== ====== ====== ====== ====== ======= ====== ====== ====== ====== ====== ====== ======");
#else
cliPrintLine("Motor Type eRPM RPM Hz TEMP VCC CURR ST/EV DBG1 DBG2 DBG3");
cliPrintLine("Motor Type eRPM RPM Hz TEMP VCC CURR STAT DBG1 DBG2 DMET");
cliPrintLine("===== ====== ====== ====== ====== ====== ====== ====== ====== ====== ====== ======");
#endif

Expand All @@ -6128,7 +6128,7 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) ? 'T' : '-'),
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) ? 'V' : '-'),
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) ? 'C' : '-'),
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS)) ? 'S' : '-'),
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_STATUS)) ? 'S' : '-'),
erpm * 100, rpm, rpm / 60);

#ifdef USE_DSHOT_TELEMETRY_STATS
Expand All @@ -6140,15 +6140,15 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
}
#endif

cliPrintLinef(" %6d %3d.%02d %6d %6d %6d %6d %6d",
cliPrintLinef(" %6d %3d.%02d %6d %6x %6d %6d %6d",
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE],
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_VOLTAGE] / 4,
25 * (dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_VOLTAGE] % 4),
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT],
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_STATE_EVENTS],
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_STATUS],
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_DEBUG1],
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_DEBUG2],
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_DEBUG3]
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_STRESS_LEVEL]
);
}
cliPrintLinefeed();
Expand Down
4 changes: 2 additions & 2 deletions src/main/drivers/at32/dshot_bitbang.c
Expand Up @@ -526,8 +526,8 @@ static bool bbDecodeTelemetry(void)

if (rawValue != DSHOT_TELEMETRY_INVALID) {
// Check EDT enable or store raw value
if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
if ((rawValue == DSHOT_TELEMETRY_ENABLE_RESPONSE) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATUS;
} else {
dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
}
Expand Down
104 changes: 73 additions & 31 deletions src/main/drivers/dshot.c
Expand Up @@ -33,6 +33,7 @@

#include "build/debug.h"
#include "build/atomic.h"
#include "build/debug.h"

#include "common/filter.h"
#include "common/maths.h"
Expand All @@ -53,7 +54,9 @@

#include "dshot.h"

#define ERPM_PER_LSB 100.0f
#define ERPM_PER_LSB (100.0f)
#define SHIFT_BYTE (8u)
#define MULTIPLE_RPM300_ERPM_FRACTION (18u)

void dshotInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
{
Expand Down Expand Up @@ -183,86 +186,125 @@ static uint32_t dshot_decode_eRPM_telemetry_value(uint16_t value)

static void dshot_decode_telemetry_value(uint8_t motorIndex, uint32_t *pDecoded, dshotTelemetryType_t *pType)
{
uint16_t value = dshotTelemetryState.motorState[motorIndex].rawValue;
const unsigned motorCount = motorDeviceCount();
uint16_t dshotDebugHighByte;
const uint16_t value = dshotTelemetryState.motorState[motorIndex].rawValue;

if (dshotTelemetryState.motorState[motorIndex].telemetryTypes == DSHOT_NORMAL_TELEMETRY_MASK) { /* Check DSHOT_TELEMETRY_TYPE_eRPM mask */
// Decode eRPM telemetry
*pDecoded = dshot_decode_eRPM_telemetry_value(value);

// Update debug buffer
if (motorIndex < motorCount && motorIndex < DEBUG16_VALUE_COUNT) {
// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just grepping some code here.

#define DEBUG16_VALUE_COUNT 8

🤔 should we only update for motorCount as some code seems dubious in how it uses the current amount of assigned motors:

for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {

Not letting motorCount exceed MAX_SUPPORTED_MOTORS would take out the first condition in several loops. This loop is used 11 times at minimum (in master)

Like to understand this statement as the if condition would not be needed:

// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {

Copy link
Contributor Author

@damosvil damosvil Dec 11, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That code (inside dshot_decode_telemetry_value) is called from updateDshotTelemetry function, in a loop that that checks all existing motors.
The only place in the whole project where dshot_decode_telemetry_value is called is in this line:
imagen

DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, *pDecoded);
}

// Set telemetry type
*pType = DSHOT_TELEMETRY_TYPE_eRPM;
} else {
// Decode Extended DSHOT telemetry
switch (value & 0x0f00) {
switch (value & DSHOT_TELEMETRY_RANGE_MASK) {

case 0x0200:
// Temperature range (in degree Celsius, just like Blheli_32 and KISS)
*pDecoded = value & 0x00ff;
case DSHOT_TELEMETRY_RANGE_TEMPERATURE:
// Temperature frame (in degree Celsius, just like Blheli_32 and KISS)
*pDecoded = value & DSHOT_TELEMETRY_VALUE_MASK;

// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {
dshotDebugHighByte = dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_STATUS] << SHIFT_BYTE;
DEBUG_SET(DEBUG_DSHOT_STATUS_N_TEMPERATURE, motorIndex, dshotDebugHighByte | *pDecoded);
}

// Set telemetry type
*pType = DSHOT_TELEMETRY_TYPE_TEMPERATURE;
break;

case 0x0400:
// Voltage range (0-63,75V step 0,25V)
*pDecoded = value & 0x00ff;
case DSHOT_TELEMETRY_RANGE_VOLTAGE:
// Voltage frame (0-63,75V step 0,25V)
*pDecoded = value & DSHOT_TELEMETRY_VALUE_MASK;

// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {
dshotDebugHighByte = dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_STATUS] << SHIFT_BYTE;
DEBUG_SET(DEBUG_DSHOT_STATUS_N_VOLTAGE, motorIndex, dshotDebugHighByte | *pDecoded);
}

// Set telemetry type
*pType = DSHOT_TELEMETRY_TYPE_VOLTAGE;
break;

case 0x0600:
// Current range (0-255A step 1A)
*pDecoded = value & 0x00ff;
case DSHOT_TELEMETRY_RANGE_CURRENT:
// Current frame (0-255A step 1A)
*pDecoded = value & DSHOT_TELEMETRY_VALUE_MASK;

// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {
dshotDebugHighByte = dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_STATUS] << SHIFT_BYTE;
DEBUG_SET(DEBUG_DSHOT_STATUS_N_CURRENT, motorIndex, dshotDebugHighByte | *pDecoded);
}

// Set telemetry type
*pType = DSHOT_TELEMETRY_TYPE_CURRENT;
break;

case 0x0800:
// Debug 1 value
*pDecoded = value & 0x00ff;
case DSHOT_TELEMETRY_RANGE_DEBUG1:
// Debug 1 frame
*pDecoded = value & DSHOT_TELEMETRY_VALUE_MASK;

// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {
dshotDebugHighByte = dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_STATUS] << SHIFT_BYTE;
DEBUG_SET(DEBUG_DSHOT_STATUS_N_DEBUG1, motorIndex, dshotDebugHighByte | *pDecoded);
}

// Set telemetry type
*pType = DSHOT_TELEMETRY_TYPE_DEBUG1;
break;

case 0x0A00:
// Debug 2 value
*pDecoded = value & 0x00ff;
case DSHOT_TELEMETRY_RANGE_DEBUG2:
// Debug 2 frame
*pDecoded = value & DSHOT_TELEMETRY_VALUE_MASK;

// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {
dshotDebugHighByte = dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_STATUS] << SHIFT_BYTE;
DEBUG_SET(DEBUG_DSHOT_STATUS_N_DEBUG2, motorIndex, dshotDebugHighByte | *pDecoded);
}

// Set telemetry type
*pType = DSHOT_TELEMETRY_TYPE_DEBUG2;
break;

case 0x0C00:
// Debug 3 value
*pDecoded = value & 0x00ff;
case DSHOT_TELEMETRY_RANGE_STRESS_LEVEL:
// Stress level frame
*pDecoded = value & DSHOT_TELEMETRY_VALUE_MASK;

// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {
dshotDebugHighByte = dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_STATUS] << SHIFT_BYTE;
DEBUG_SET(DEBUG_DSHOT_STATUS_N_STRESS_LVL, motorIndex, dshotDebugHighByte | *pDecoded);
}

// Set telemetry type
*pType = DSHOT_TELEMETRY_TYPE_DEBUG3;
*pType = DSHOT_TELEMETRY_TYPE_STRESS_LEVEL;
break;

case 0x0E00:
// State / events
*pDecoded = value & 0x00ff;
case DSHOT_TELEMETRY_RANGE_STATUS:
// State / events frame
*pDecoded = value & DSHOT_TELEMETRY_VALUE_MASK;

// Set telemetry type
*pType = DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
*pType = DSHOT_TELEMETRY_TYPE_STATUS;
break;

default:
// Decode as eRPM
*pDecoded = dshot_decode_eRPM_telemetry_value(value);

// Update debug buffer
if (motorIndex < motorCount && motorIndex < DEBUG16_VALUE_COUNT) {
// Update debug buffer (motorIndex < motorCount guaranteed by caller)
if (motorIndex < DEBUG16_VALUE_COUNT) {
// In this case two debug options to maximize logging info
dshotDebugHighByte = dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_STATUS] << SHIFT_BYTE;
DEBUG_SET(DEBUG_DSHOT_STATUS_N_ERPM_FRACTION_18, motorIndex, dshotDebugHighByte | ((*pDecoded) / MULTIPLE_RPM300_ERPM_FRACTION));
DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, *pDecoded);
}

Expand Down
44 changes: 31 additions & 13 deletions src/main/drivers/dshot.h
Expand Up @@ -57,25 +57,43 @@ typedef struct dshotTelemetryQuality_s {
extern dshotTelemetryQuality_t dshotTelemetryQuality[MAX_SUPPORTED_MOTORS];
#endif // USE_DSHOT_TELEMETRY_STATS

#define DSHOT_NORMAL_TELEMETRY_MASK (1 << DSHOT_TELEMETRY_TYPE_eRPM)
#define DSHOT_EXTENDED_TELEMETRY_MASK (~DSHOT_NORMAL_TELEMETRY_MASK)
#define DSHOT_NORMAL_TELEMETRY_MASK (1 << DSHOT_TELEMETRY_TYPE_eRPM)
#define DSHOT_EXTENDED_TELEMETRY_MASK (~DSHOT_NORMAL_TELEMETRY_MASK)
#define DSHOT_TELEMETRY_RANGE_MASK (0x0f00u)
#define DSHOT_TELEMETRY_VALUE_MASK (0x00ffu)
#define DSHOT_TELEMETRY_STATUS_ALERT_EVENT_MASK (0x80u)
#define DSHOT_TELEMETRY_STATUS_WARNING_EVENT_MASK (0x40u)
#define DSHOT_TELEMETRY_STATUS_ERROR_EVENT_MASK (0x20u)
#define DSHOT_TELEMETRY_STATUS_MAX_STRESS_LVL_MASK (0x0Fu)
#define DSHOT_MAX_STRESS_LVL_WARNING_THRESHOLD (9u)
#define DSHOT_TELEMETRY_ENABLE_RESPONSE (DSHOT_TELEMETRY_RANGE_STATUS)

typedef enum dshotTelemetryType_e {
DSHOT_TELEMETRY_TYPE_eRPM = 0,
DSHOT_TELEMETRY_TYPE_TEMPERATURE = 1,
DSHOT_TELEMETRY_TYPE_VOLTAGE = 2,
DSHOT_TELEMETRY_TYPE_CURRENT = 3,
DSHOT_TELEMETRY_TYPE_DEBUG1 = 4,
DSHOT_TELEMETRY_TYPE_DEBUG2 = 5,
DSHOT_TELEMETRY_TYPE_DEBUG3 = 6,
DSHOT_TELEMETRY_TYPE_STATE_EVENTS = 7,
DSHOT_TELEMETRY_TYPE_eRPM,
DSHOT_TELEMETRY_TYPE_TEMPERATURE,
DSHOT_TELEMETRY_TYPE_VOLTAGE,
DSHOT_TELEMETRY_TYPE_CURRENT,
DSHOT_TELEMETRY_TYPE_DEBUG1,
DSHOT_TELEMETRY_TYPE_DEBUG2,
DSHOT_TELEMETRY_TYPE_STRESS_LEVEL,
DSHOT_TELEMETRY_TYPE_STATUS,
DSHOT_TELEMETRY_TYPE_COUNT
} dshotTelemetryType_t;

typedef enum dshotTelemetryRange_e {
DSHOT_TELEMETRY_RANGE_TEMPERATURE = 0x200,
DSHOT_TELEMETRY_RANGE_VOLTAGE = 0x400,
DSHOT_TELEMETRY_RANGE_CURRENT = 0x600,
DSHOT_TELEMETRY_RANGE_DEBUG1 = 0x800,
DSHOT_TELEMETRY_RANGE_DEBUG2 = 0xA00,
DSHOT_TELEMETRY_RANGE_STRESS_LEVEL = 0xC00,
DSHOT_TELEMETRY_RANGE_STATUS = 0xE00
} dshotTelemetryRange_t;

typedef enum dshotRawValueState_e {
DSHOT_RAW_VALUE_STATE_INVALID = 0,
DSHOT_RAW_VALUE_STATE_NOT_PROCESSED = 1,
DSHOT_RAW_VALUE_STATE_PROCESSED = 2,
DSHOT_RAW_VALUE_STATE_INVALID,
DSHOT_RAW_VALUE_STATE_NOT_PROCESSED,
DSHOT_RAW_VALUE_STATE_PROCESSED,
DSHOT_RAW_VALUE_STATE_COUNT
} dshotRawValueState_t;

Expand Down
4 changes: 2 additions & 2 deletions src/main/drivers/pwm_output_dshot_shared.c
Expand Up @@ -239,8 +239,8 @@ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)

if (rawValue != DSHOT_TELEMETRY_INVALID) {
// Check EDT enable or store raw value
if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(i) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
dshotTelemetryState.motorState[i].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
if ((rawValue == DSHOT_TELEMETRY_ENABLE_RESPONSE) && (dshotCommandGetCurrent(i) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
dshotTelemetryState.motorState[i].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATUS;
} else {
dshotTelemetryState.motorState[i].rawValue = rawValue;
}
Expand Down
4 changes: 2 additions & 2 deletions src/main/drivers/stm32/dshot_bitbang.c
Expand Up @@ -581,8 +581,8 @@ static bool bbDecodeTelemetry(void)

if (rawValue != DSHOT_TELEMETRY_INVALID) {
// Check EDT enable or store raw value
if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
if ((rawValue == DSHOT_TELEMETRY_ENABLE_RESPONSE) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATUS;
} else {
dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
}
Expand Down
20 changes: 20 additions & 0 deletions src/main/msp/msp.c
Expand Up @@ -1449,6 +1449,8 @@ case MSP_NAME:
// API 1.42
sbufWriteU8(dst, getMotorCount());
sbufWriteU8(dst, motorConfig()->motorPoleCount);

// API 1.44
#ifdef USE_DSHOT_TELEMETRY
sbufWriteU8(dst, motorConfig()->dev.useDshotTelemetry);
#else
Expand All @@ -1460,6 +1462,13 @@ case MSP_NAME:
#else
sbufWriteU8(dst, 0);
#endif

// API 1.46
#ifdef USE_DSHOT_TELEMETRY
sbufWriteU8(dst, motorConfig()->dev.useDshotEdt);
#else
sbufWriteU8(dst, 0);
#endif
break;

// Deprecated in favor of MSP_MOTOR_TELEMETY as of API version 1.42
Expand Down Expand Up @@ -2827,12 +2836,23 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
// version 1.42
if (sbufBytesRemaining(src) >= 2) {
motorConfigMutable()->motorPoleCount = sbufReadU8(src);

// version 1.44
#if defined(USE_DSHOT_TELEMETRY)
motorConfigMutable()->dev.useDshotTelemetry = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
}

// Version 1.46
if (sbufBytesRemaining(src) >= 1) {
#if defined(USE_DSHOT_TELEMETRY)
motorConfigMutable()->dev.useDshotEdt = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
}
break;

#ifdef USE_GPS
Expand Down