From 21b3fc15b7902d095a41dba84b315d0ebb011e7d Mon Sep 17 00:00:00 2001 From: danmos Date: Sat, 18 Jun 2022 23:12:44 +0200 Subject: [PATCH] Extended Bidir DShot Extended DSHOT telemetry Fixed broken unit testing Rebased to master Extended DSHOT telemetry Added DSHOT ESC fail to OSD warnings Initial extended DSHOT implementation DSHOT telemetry ranges readjusted Added shot_telemetry_data to cli Added DSHOT telemetry warnings Added extended DSHOT telemetry temperature data to osd elements Fixed DSHOT telemetry osd warnings Make cli dshot telemetry types human readable Fixed ESC temperature OSD element Added extended dshot telemetry enable command to dshot command queue to enable it in tryArm function. Fixed broken automated unit testing Fixed text output for dshot_telemetry_data cli command Decode extended telemetry only when it has been activated in the ESC DSHOT extended telemetry is only enabled when an extended telemetry enable frame is being processed Fixed broken automated unit testing Update tail's dshot command in queue when running blocking commands, so extended dshot telemetry command responses can be associated to their corresponding extended dshot telemetry enable/disable command request Added clariffication to dshot_get_telemetry_type_to_decode to explain mechanish used for processing DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE response Tabs cleanup Added dshot motor stop fix when writting blocking commands Added extended DSHOT telemetry to osd warnings Fixed extended DSHOT telemetry in osd elements Implemented msp extended dshot telemetry Optimized osd warnings Fixed code style Added max temp to osd stats Fixed automated unit testing Fixed broken automated unit testing Fixed missing extended dshot telemetry voltage and wrong current Fixed unadverted change in gitignore file Print esc number besides max esc tempetature in osd stats issues and some other minor issues ESC temperature is coded as an uint8 going from 0 to 255. Updated osd config esc_temp_alarm from int8_t to uint8_t to match esc temperature coding issues and some other minor issues Enable extended dshot telemetry when no esc sensors avaliable and dshot telemetry is enabled Arranged dshot.h header and removed unnecessary headers from dshot.c Updated dshot_telemetry_data cli command output Fixed dshot_telemetry_data command motor_output_unittest fixed Fixed motor number printing in dshot_telemetry_data cli command fixed motor_output_unittest Merged dshot_telemetry_data into dshot_telemetry_info cli command Fixed tabs Fixed motor_output_unittest Support extended DSHOT telemetry on DJI FPV Fixed Compiling fails without USE_ESC_SENSOR and USE_DSHOT_TELEMETRY defines. Fixed whencompiling without USE_ESC_SENSOR and USE_DSHOT_TELEMETRY Reworked so code can be compiled with USE_ESC_SENSOR and USE_DSHOT_TELEMETRY separately Enabling EDT the correct way Removed unecessary conversions Changed motorIndex datatype to uint8_t in dshot functions --- src/main/cli/cli.c | 46 +++-- src/main/cli/settings.c | 2 +- src/main/drivers/dshot.c | 203 ++++++++++++++++++++- src/main/drivers/dshot.h | 39 +++- src/main/drivers/dshot_bitbang.c | 18 +- src/main/drivers/dshot_bitbang_decode.c | 45 +++-- src/main/drivers/dshot_bitbang_decode.h | 10 +- src/main/drivers/dshot_command.c | 22 ++- src/main/drivers/dshot_command.h | 2 + src/main/drivers/pwm_output_dshot_shared.c | 67 ++----- src/main/fc/core.c | 8 + src/main/msp/msp.c | 41 ++++- src/main/osd/osd.c | 50 +++-- src/main/osd/osd.h | 9 +- src/main/osd/osd_elements.c | 68 +++++-- src/main/osd/osd_warnings.c | 59 ++++++ src/test/Makefile | 3 + src/test/unit/motor_output_unittest.cc | 37 +++- 18 files changed, 562 insertions(+), 167 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index ebde8d4192..4a9390bffb 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -6145,6 +6145,8 @@ static void cliResource(const char *cmdName, char *cmdline) #endif #ifdef USE_DSHOT_TELEMETRY + + static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline) { UNUSED(cmdName); @@ -6159,27 +6161,44 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline) cliPrintLinefeed(); #ifdef USE_DSHOT_TELEMETRY_STATS - cliPrintLine("Motor eRPM RPM Hz Invalid"); - cliPrintLine("===== ======= ====== ===== ======="); + cliPrintLine("Motor Type eRPM RPM Hz Invalid TEMP VCC CURR ST/EV DBG1 DBG2 DBG3"); + cliPrintLine("===== ====== ====== ====== ====== ======= ====== ====== ====== ====== ====== ====== ======"); #else - cliPrintLine("Motor eRPM RPM Hz"); - cliPrintLine("===== ======= ====== ====="); + cliPrintLine("Motor Type eRPM RPM Hz TEMP VCC CURR ST/EV DBG1 DBG2 DBG3"); + cliPrintLine("===== ====== ====== ====== ====== ====== ====== ====== ====== ====== ====== ======"); #endif + for (uint8_t i = 0; i < getMotorCount(); i++) { - cliPrintf("%5d %7d %6d %5d ", i, - (int)getDshotTelemetry(i) * 100, - (int)getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount, - (int)getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount / 60); + cliPrintf("%5d %c%c%c%c%c %6d %6d %6d", + i + 1, + ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_eRPM)) ? 'R' : '-'), + ((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].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100, + dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100 * 2 / motorConfig()->motorPoleCount, + dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100 * 2 / motorConfig()->motorPoleCount / 60); + #ifdef USE_DSHOT_TELEMETRY_STATS if (isDshotMotorTelemetryActive(i)) { - const int calcPercent = getDshotTelemetryMotorInvalidPercent(i); - cliPrintLinef("%3d.%02d%%", calcPercent / 100, calcPercent % 100); + int32_t calcPercent = getDshotTelemetryMotorInvalidPercent(i); + cliPrintf(" %3d.%02d%%", calcPercent / 100, calcPercent % 100); } else { - cliPrintLine("NO DATA"); + cliPrint(" NO DATA"); } -#else - cliPrintLinefeed(); #endif + + cliPrintLinef(" %6d %3d.%02d %6d %6d %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_DEBUG1], + dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_DEBUG2], + dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_DEBUG3] + ); } cliPrintLinefeed(); @@ -6203,6 +6222,7 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline) cliPrintLine("Dshot telemetry not enabled"); } } + #endif static void printConfig(const char *cmdName, char *cmdline, bool doDiff) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 3f2de5f154..22e28b334c 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1314,7 +1314,7 @@ const clivalue_t valueTable[] = { { "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, cap_alarm) }, { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, alt_alarm) }, { "osd_distance_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, distance_alarm) }, - { "osd_esc_temp_alarm", VAR_INT8 | MASTER_VALUE, .config.minmax = { INT8_MIN, INT8_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, esc_temp_alarm) }, + { "osd_esc_temp_alarm", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, UINT8_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, esc_temp_alarm) }, { "osd_esc_rpm_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { ESC_RPM_ALARM_OFF, INT16_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, esc_rpm_alarm) }, { "osd_esc_current_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { ESC_CURRENT_ALARM_OFF, INT16_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, esc_current_alarm) }, #ifdef USE_ADC_INTERNAL diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c index 9321ad9bb2..e206022b01 100644 --- a/src/main/drivers/dshot.c +++ b/src/main/drivers/dshot.c @@ -31,24 +31,19 @@ #include "build/atomic.h" #include "common/maths.h" -#include "common/time.h" #include "config/feature.h" #include "drivers/motor.h" #include "drivers/timer.h" -#include "drivers/dshot_dpwm.h" // for motorDmaOutput_t, should be gone #include "drivers/dshot_command.h" #include "drivers/nvic.h" -#include "drivers/pwm_output.h" // for PWM_TYPE_* and others - -#include "fc/rc_controls.h" // for flight3DConfig_t #include "rx/rx.h" - #include "dshot.h" + void dshotInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) { float outputLimitOffset = DSHOT_RANGE * (1 - outputLimit); *disarm = DSHOT_CMD_MOTOR_STOP; @@ -132,18 +127,89 @@ FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb) } #ifdef USE_DSHOT_TELEMETRY + + FAST_DATA_ZERO_INIT dshotTelemetryState_t dshotTelemetryState; uint16_t getDshotTelemetry(uint8_t index) { - return dshotTelemetryState.motorState[index].telemetryValue; + return dshotTelemetryState.motorState[index].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM]; } -#endif +bool isDshotMotorTelemetryActive(uint8_t motorIndex) +{ + return (dshotTelemetryState.motorState[motorIndex].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_eRPM)) != 0; +} + +bool isDshotTelemetryActive(void) +{ + const unsigned motorCount = motorDeviceCount(); + if (motorCount) { + for (unsigned i = 0; i < motorCount; i++) { + if (!isDshotMotorTelemetryActive(i)) { + return false; + } + } + return true; + } + return false; +} + +dshotTelemetryType_t dshot_get_telemetry_type_to_decode(uint8_t motorIndex) +{ + dshotTelemetryType_t type; + + // Prepare the allowed telemetry to be read + if ((dshotTelemetryState.motorState[motorIndex].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) != 0) { + // Allow decoding all kind of telemetry frames + type = DSHOT_TELEMETRY_TYPE_COUNT; + } else if (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE) { + // No empty command queue check needed because responses are always originated after a request + // Always checking the current existing request + // Allow decoding only extended telemetry enable frame (during arming) + type = DSHOT_TELEMETRY_TYPE_STATE_EVENTS; + } else { + // Allow decoding only eRPM telemetry frame + type = DSHOT_TELEMETRY_TYPE_eRPM; + } + + return type; +} + +FAST_CODE void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t type, uint16_t value) +{ + // Update telemetry data + dshotTelemetryState.motorState[motorIndex].telemetryData[type] = value; + dshotTelemetryState.motorState[motorIndex].telemetryTypes |= (1 << type); + + // Update max temp + if ((type == DSHOT_TELEMETRY_TYPE_TEMPERATURE) && (value > dshotTelemetryState.motorState[motorIndex].maxTemp)) { + dshotTelemetryState.motorState[motorIndex].maxTemp = value; + } +} + +#endif // USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY_STATS + FAST_DATA_ZERO_INIT dshotTelemetryQuality_t dshotTelemetryQuality[MAX_SUPPORTED_MOTORS]; +int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex) +{ + int16_t invalidPercent = 0; + + if (isDshotMotorTelemetryActive(motorIndex)) { + const uint32_t totalCount = dshotTelemetryQuality[motorIndex].packetCountSum; + const uint32_t invalidCount = dshotTelemetryQuality[motorIndex].invalidCountSum; + if (totalCount > 0) { + invalidPercent = lrintf(invalidCount * 10000.0f / totalCount); + } + } else { + invalidPercent = 10000; // 100.00% + } + return invalidPercent; +} + void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool packetValid, timeMs_t currentTimeMs) { uint8_t statsBucketIndex = (currentTimeMs / DSHOT_TELEMETRY_QUALITY_BUCKET_MS) % DSHOT_TELEMETRY_QUALITY_BUCKET_COUNT; @@ -200,3 +266,124 @@ void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size) } } } + +static uint32_t dshot_decode_eRPM_telemetry_value(uint32_t value) +{ + // eRPM range + if (value == 0x0fff) { + return 0; + } + + // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm) + value = (value & 0x000001ff) << ((value & 0xfffffe00) >> 9); + if (!value) { + return DSHOT_TELEMETRY_INVALID; + } + + // Convert period to erpm * 100 + return (1000000 * 60 / 100 + value / 2) / value; +} + +uint32_t dshot_decode_telemetry_value(uint32_t value, dshotTelemetryType_t *type) +{ + uint32_t decoded; + + switch (*type) { + + case DSHOT_TELEMETRY_TYPE_eRPM: + // Expect only eRPM telemetry + decoded = dshot_decode_eRPM_telemetry_value(value); + break; + + case DSHOT_TELEMETRY_TYPE_STATE_EVENTS: + // Expect an extended telemetry enable frame + if (value == 0x0E00) { + // Decode + decoded = 0; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_STATE_EVENTS; + } else { + // Unexpected frame + decoded = DSHOT_TELEMETRY_INVALID; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_eRPM; + } + break; + + default: + // Extended DSHOT telemetry + switch (value & 0x0f00) { + + case 0x0200: + // Temperature range (in degree Celsius, just like Blheli_32 and KISS) + decoded = value & 0x00ff; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_TEMPERATURE; + break; + + case 0x0400: + // Voltage range (0-63,75V step 0,25V) + decoded = value & 0x00ff; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_VOLTAGE; + break; + + case 0x0600: + // Current range (0-255A step 1A) + decoded = value & 0x00ff; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_CURRENT; + break; + + case 0x0800: + // Debug 1 value + decoded = value & 0x00ff; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_DEBUG1; + break; + + case 0x0A00: + // Debug 2 value + decoded = value & 0x00ff; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_DEBUG2; + break; + + case 0x0C00: + // Debug 3 value + decoded = value & 0x00ff; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_DEBUG3; + break; + + case 0x0E00: + // State / events + decoded = value & 0x00ff; + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_STATE_EVENTS; + break; + + default: + // Decode as eRPM + decoded = dshot_decode_eRPM_telemetry_value(value); + + // Set telemetry type + *type = DSHOT_TELEMETRY_TYPE_eRPM; + break; + + } + break; + + } + + return decoded; +} diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h index 15f665be1c..2d0094ead2 100644 --- a/src/main/drivers/dshot.h +++ b/src/main/drivers/dshot.h @@ -24,13 +24,16 @@ #include "pg/motor.h" -#define DSHOT_MIN_THROTTLE 48 -#define DSHOT_MAX_THROTTLE 2047 -#define DSHOT_3D_FORWARD_MIN_THROTTLE 1048 -#define DSHOT_RANGE (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) +#define DSHOT_MIN_THROTTLE (48) +#define DSHOT_MAX_THROTTLE (2047) +#define DSHOT_3D_FORWARD_MIN_THROTTLE (1048) +#define DSHOT_RANGE (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) -#define MIN_GCR_EDGES 7 -#define MAX_GCR_EDGES 22 +#define DSHOT_TELEMETRY_NOEDGE (0xfffe) +#define DSHOT_TELEMETRY_INVALID (0xffff) + +#define MIN_GCR_EDGES (7) +#define MAX_GCR_EDGES (22) // comment out to see frame dump of corrupted frames in dshot_telemetry_info //#define DEBUG_BBDECODE @@ -51,6 +54,20 @@ typedef struct dshotTelemetryQuality_s { extern dshotTelemetryQuality_t dshotTelemetryQuality[MAX_SUPPORTED_MOTORS]; #endif // USE_DSHOT_TELEMETRY_STATS +#define DSHOT_EXTENDED_TELEMETRY_MASK (~(1<portInputBuffer, bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort), - bbMotors[motorIndex].pinIndex); + bbMotors[motorIndex].pinIndex, &type); #else uint32_t value = decode_bb( bbMotors[motorIndex].bbPort->portInputBuffer, bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort), - bbMotors[motorIndex].pinIndex); + bbMotors[motorIndex].pinIndex, &type); #endif - if (value == BB_NOEDGE) { + if (value == DSHOT_TELEMETRY_NOEDGE) { continue; } dshotTelemetryState.readCount++; - if (value != BB_INVALID) { - dshotTelemetryState.motorState[motorIndex].telemetryValue = value; - dshotTelemetryState.motorState[motorIndex].telemetryActive = true; + if (value != DSHOT_TELEMETRY_INVALID) { + dshotUpdateTelemetryData(motorIndex, type, value); if (motorIndex < 4) { DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, value); } @@ -547,7 +551,7 @@ static bool bbUpdateStart(void) dshotTelemetryState.invalidPacketCount++; } #ifdef USE_DSHOT_TELEMETRY_STATS - updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], value != BB_INVALID, currentTimeMs); + updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], value != DSHOT_TELEMETRY_INVALID, currentTimeMs); #endif } } diff --git a/src/main/drivers/dshot_bitbang_decode.c b/src/main/drivers/dshot_bitbang_decode.c index 6736d47fdf..b0cb630c41 100644 --- a/src/main/drivers/dshot_bitbang_decode.c +++ b/src/main/drivers/dshot_bitbang_decode.c @@ -28,18 +28,22 @@ uint16_t bbBuffer[134]; #define BITBAND_SRAM_BASE 0x22000000 #define BITBAND_SRAM(a,b) ((BITBAND_SRAM_BASE + (((a)-BITBAND_SRAM_REF)<<5) + ((b)<<2))) // Convert SRAM address + + typedef struct bitBandWord_s { uint32_t value; uint32_t junk[15]; } bitBandWord_t; + + #ifdef DEBUG_BBDECODE uint32_t sequence[MAX_GCR_EDGES]; int sequenceIndex = 0; #endif -static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t count, uint32_t bit) +static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t count, uint32_t bit, dshotTelemetryType_t *type) { #ifndef DEBUG_BBDECODE UNUSED(buffer); @@ -69,26 +73,16 @@ static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t coun bbBuffer[i] = !!(buffer[i] & (1 << bit)); } #endif - value = BB_INVALID; + value = DSHOT_TELEMETRY_INVALID; } else { - value = decodedValue >> 4; - - if (value == 0x0fff) { - return 0; - } - // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm) - value = (value & 0x000001ff) << ((value & 0xfffffe00) >> 9); - if (!value) { - return BB_INVALID; - } - // Convert period to erpm * 100 - value = (1000000 * 60 / 100 + value / 2) / value; + value = dshot_decode_telemetry_value(decodedValue >> 4, type); } + return value; } -uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) +uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit, dshotTelemetryType_t *type) { #ifdef DEBUG_BBDECODE memset(sequence, 0, sizeof(sequence)); @@ -115,7 +109,7 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) // not returning telemetry is ok if the esc cpu is // overburdened. in that case no edge will be found and // BB_NOEDGE indicates the condition to caller - return BB_NOEDGE; + return DSHOT_TELEMETRY_NOEDGE; } int remaining = MIN(count - (p - b), (unsigned int)MAX_VALID_BBSAMPLES); @@ -181,13 +175,13 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) } if (bits < 18) { - return BB_NOEDGE; + return DSHOT_TELEMETRY_NOEDGE; } // length of last sequence has to be inferred since the last bit with inverted dshot is high const int nlen = 21 - bits; if (nlen < 0) { - value = BB_INVALID; + return DSHOT_TELEMETRY_NOEDGE; } #ifdef DEBUG_BBDECODE @@ -198,10 +192,11 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) value <<= nlen; value |= 1 << (nlen - 1); } - return decode_bb_value(value, buffer, count, bit); + + return decode_bb_value(value, buffer, count, bit, type); } -FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) +FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit, dshotTelemetryType_t *type) { #ifdef DEBUG_BBDECODE memset(sequence, 0, sizeof(sequence)); @@ -235,7 +230,7 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) // not returning telemetry is ok if the esc cpu is // overburdened. in that case no edge will be found and // BB_NOEDGE indicates the condition to caller - return BB_NOEDGE; + return DSHOT_TELEMETRY_NOEDGE; } int remaining = MIN(count - (p - buffer), (unsigned int)MAX_VALID_BBSAMPLES); @@ -273,7 +268,7 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) // length of last sequence has to be inferred since the last bit with inverted dshot is high if (bits < 18) { - return BB_NOEDGE; + return DSHOT_TELEMETRY_NOEDGE; } const int nlen = 21 - bits; @@ -283,13 +278,15 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) #endif if (nlen < 0) { - value = BB_INVALID; + return DSHOT_TELEMETRY_NOEDGE; } + if (nlen > 0) { value <<= nlen; value |= 1 << (nlen - 1); } - return decode_bb_value(value, buffer, count, bit); + + return decode_bb_value(value, buffer, count, bit, type); } #endif diff --git a/src/main/drivers/dshot_bitbang_decode.h b/src/main/drivers/dshot_bitbang_decode.h index da546c06d1..a6854f817d 100644 --- a/src/main/drivers/dshot_bitbang_decode.h +++ b/src/main/drivers/dshot_bitbang_decode.h @@ -18,12 +18,14 @@ * If not, see . */ + + #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) -#define BB_NOEDGE 0xfffe -#define BB_INVALID 0xffff -uint32_t decode_bb(uint16_t buffer[], uint32_t count, uint32_t mask); -uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit); + +uint32_t decode_bb(uint16_t buffer[], uint32_t count, uint32_t mask, dshotTelemetryType_t *type); +uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit, dshotTelemetryType_t *type); + #endif diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c index 0b7dec3a20..8f74708207 100644 --- a/src/main/drivers/dshot_command.c +++ b/src/main/drivers/dshot_command.c @@ -195,6 +195,8 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot case DSHOT_CMD_SAVE_SETTINGS: case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: + case DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE: + case DSHOT_CMD_EXTENDED_TELEMETRY_DISABLE: repeats = 10; break; case DSHOT_CMD_BEACON1: @@ -209,6 +211,11 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot } if (commandType == DSHOT_CMD_TYPE_BLOCKING) { + // Fake command in queue. Blocking commands are launched from cli, and no inline commands are running + for (uint8_t i = 0; i < motorDeviceCount(); i++) { + commandQueue[commandQueueTail].command[i] = (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP; + } + delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US); for (; repeats; repeats--) { delayMicroseconds(DSHOT_COMMAND_DELAY_US); @@ -219,18 +226,19 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot cmpTimeUs(timeoutUs, micros()) > 0); #endif for (uint8_t i = 0; i < motorDeviceCount(); i++) { - if ((i == index) || (index == ALL_MOTORS)) { - motorDmaOutput_t *const motor = getMotorDmaOutput(i); - motor->protocolControl.requestTelemetry = true; - motorGetVTable().writeInt(i, command); - } else { - motorGetVTable().writeInt(i, DSHOT_CMD_MOTOR_STOP); - } + motorDmaOutput_t *const motor = getMotorDmaOutput(i); + motor->protocolControl.requestTelemetry = true; + motorGetVTable().writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP); } motorGetVTable().updateComplete(); } delayMicroseconds(delayAfterCommandUs); + + // Clean fake command in queue. When running blocking commands are launched from cli, and no inline commands are running + for (uint8_t i = 0; i < motorDeviceCount(); i++) { + commandQueue[commandQueueTail].command[i] = DSHOT_CMD_MOTOR_STOP; + } } else if (commandType == DSHOT_CMD_TYPE_INLINE) { dshotCommandControl_t *commandControl = addCommand(); if (commandControl) { diff --git a/src/main/drivers/dshot_command.h b/src/main/drivers/dshot_command.h index d3691dde19..9ade0d459f 100644 --- a/src/main/drivers/dshot_command.h +++ b/src/main/drivers/dshot_command.h @@ -45,6 +45,8 @@ typedef enum { DSHOT_CMD_3D_MODE_ON, DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented DSHOT_CMD_SAVE_SETTINGS, + DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE, + DSHOT_CMD_EXTENDED_TELEMETRY_DISABLE, DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, DSHOT_CMD_LED0_ON, // BLHeli32 only diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c index 66ecd10b81..3952eda10c 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/src/main/drivers/pwm_output_dshot_shared.c @@ -127,7 +127,7 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) void dshotEnableChannels(uint8_t motorCount); -static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count) +static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count, dshotTelemetryType_t *type) { uint32_t value = 0; uint32_t oldValue = buffer[0]; @@ -167,19 +167,10 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count) csum = csum ^ (csum >> 4); // xor nibbles if ((csum & 0xf) != 0xf) { - return 0xffff; + return DSHOT_TELEMETRY_INVALID; } - decodedValue >>= 4; - if (decodedValue == 0x0fff) { - return 0; - } - decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9); - if (!decodedValue) { - return 0xffff; - } - uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue; - return ret; + return dshot_decode_telemetry_value(decodedValue >> 4, type); } #endif @@ -190,10 +181,13 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void) if (!useDshotTelemetry) { return true; } + #ifdef USE_DSHOT_TELEMETRY_STATS const timeMs_t currentTimeMs = millis(); #endif const timeUs_t currentUs = micros(); + dshotTelemetryType_t type; + for (int i = 0; i < dshotPwmDevice.count; i++) { timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs); if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) { @@ -212,18 +206,21 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void) TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE); #endif - uint16_t value = 0xffff; + uint16_t value; if (edges > MIN_GCR_EDGES) { dshotTelemetryState.readCount++; - value = decodeTelemetryPacket(dmaMotors[i].dmaBuffer, edges); + + // Get dshot telemetry type to decode + type = dshot_get_telemetry_type_to_decode(i); + + value = decodeTelemetryPacket(dmaMotors[i].dmaBuffer, edges, &type); #ifdef USE_DSHOT_TELEMETRY_STATS bool validTelemetryPacket = false; #endif - if (value != 0xffff) { - dshotTelemetryState.motorState[i].telemetryValue = value; - dshotTelemetryState.motorState[i].telemetryActive = true; + if (value != DSHOT_TELEMETRY_INVALID) { + dshotUpdateTelemetryData(value, type, value); if (i < 4) { DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value); } @@ -248,41 +245,5 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void) return true; } -bool isDshotMotorTelemetryActive(uint8_t motorIndex) -{ - return dshotTelemetryState.motorState[motorIndex].telemetryActive; -} - -bool isDshotTelemetryActive(void) -{ - const unsigned motorCount = motorDeviceCount(); - if (motorCount) { - for (unsigned i = 0; i < motorCount; i++) { - if (!isDshotMotorTelemetryActive(i)) { - return false; - } - } - return true; - } - return false; -} - -#ifdef USE_DSHOT_TELEMETRY_STATS -int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex) -{ - int16_t invalidPercent = 0; - - if (dshotTelemetryState.motorState[motorIndex].telemetryActive) { - const uint32_t totalCount = dshotTelemetryQuality[motorIndex].packetCountSum; - const uint32_t invalidCount = dshotTelemetryQuality[motorIndex].invalidCountSum; - if (totalCount > 0) { - invalidPercent = lrintf(invalidCount * 10000.0f / totalCount); - } - } else { - invalidPercent = 10000; // 100.00% - } - return invalidPercent; -} -#endif // USE_DSHOT_TELEMETRY_STATS #endif // USE_DSHOT_TELEMETRY #endif // USE_DSHOT diff --git a/src/main/fc/core.c b/src/main/fc/core.c index a6d05f5fd0..004f0cdf91 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -509,7 +509,15 @@ void tryArm(void) return; } +#if defined(USE_ESC_SENSOR) && defined(USE_DSHOT_TELEMETRY) + // Try to activate extended DSHOT telemetry only if no esc sensor exists and dshot telemetry is active + if (isMotorProtocolDshot() && !featureIsEnabled(FEATURE_ESC_SENSOR) && motorConfig()->dev.useDshotTelemetry) { + dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE, DSHOT_CMD_TYPE_INLINE); + } +#endif + if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { + // Set motor spin direction if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) { flipOverAfterCrashActive = false; if (!featureIsEnabled(FEATURE_3D)) { diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 9a4de85310..694e9dff10 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1211,14 +1211,35 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t #ifdef USE_DSHOT_TELEMETRY if (motorConfig()->dev.useDshotTelemetry) { - rpm = (int)getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount; + rpm = getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount; rpmDataAvailable = true; invalidPct = 10000; // 100.00% + + #ifdef USE_DSHOT_TELEMETRY_STATS if (isDshotMotorTelemetryActive(i)) { invalidPct = getDshotTelemetryMotorInvalidPercent(i); } #endif + + + // Provide extended dshot telemetry + if ((dshotTelemetryState.motorState[i].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) != 0) { + // Temperature Celsius [0, 1, ..., 255] in degree Celsius, just like Blheli_32 and KISS + if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0) { + escTemperature = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]; + } + + // Current -> 0-255A step 1A + if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) != 0) { + escCurrent = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT]; + } + + // Voltage -> 0-63,75V step 0,25V + if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) != 0) { + escVoltage = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_VOLTAGE] >> 2; + } + } } #endif @@ -1431,9 +1452,10 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t #endif break; -#if defined(USE_ESC_SENSOR) // Deprecated in favor of MSP_MOTOR_TELEMETY as of API version 1.42 + // Used by DJI FPV case MSP_ESC_SENSOR_DATA: +#if defined(USE_ESC_SENSOR) if (featureIsEnabled(FEATURE_ESC_SENSOR)) { sbufWriteU8(dst, getMotorCount()); for (int i = 0; i < getMotorCount(); i++) { @@ -1441,12 +1463,23 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t sbufWriteU8(dst, escData->temperature); sbufWriteU16(dst, escData->rpm); } - } else { + } else +#endif +#if defined(USE_DSHOT_TELEMETRY) + if (motorConfig()->dev.useDshotTelemetry) { + sbufWriteU8(dst, getMotorCount()); + for (int i = 0; i < getMotorCount(); i++) { + sbufWriteU8(dst, dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]); + sbufWriteU16(dst, getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount); + } + } + else +#endif + { unsupportedCommand = true; } break; -#endif #ifdef USE_GPS case MSP_GPS_CONFIG: diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 70e8a3473d..317b54fedc 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -483,17 +483,18 @@ void osdInit(displayPort_t *osdDisplayPortToUse, osdDisplayPortDevice_e displayP static void osdResetStats(void) { - stats.max_current = 0; - stats.max_speed = 0; - stats.min_voltage = 5000; - stats.end_voltage = 0; - stats.min_rssi = 99; // percent - stats.max_altitude = 0; - stats.max_distance = 0; - stats.armed_time = 0; - stats.max_g_force = 0; - stats.max_esc_temp = 0; - stats.max_esc_rpm = 0; + stats.max_current = 0; + stats.max_speed = 0; + stats.min_voltage = 5000; + stats.end_voltage = 0; + stats.min_rssi = 99; // percent + stats.max_altitude = 0; + stats.max_distance = 0; + stats.armed_time = 0; + stats.max_g_force = 0; + stats.max_esc_temp_ix = 0; + stats.max_esc_temp = 0; + stats.max_esc_rpm = 0; stats.min_link_quality = (linkQualitySource == LQ_SOURCE_NONE) ? 99 : 100; // percent stats.min_rssi_dbm = CRSF_SNR_MAX; } @@ -588,13 +589,26 @@ static void osdUpdateStats(void) } #endif -#ifdef USE_ESC_SENSOR +#if defined(USE_ESC_SENSOR) if (featureIsEnabled(FEATURE_ESC_SENSOR)) { value = osdEscDataCombined->temperature; if (stats.max_esc_temp < value) { stats.max_esc_temp = value; } + } else +#endif +#if defined(USE_DSHOT_TELEMETRY) + { + // Take max temp from dshot telemetry + for (uint8_t k = 0; k < getMotorCount(); k++) { + if (dshotTelemetryState.motorState[k].maxTemp > stats.max_esc_temp) { + stats.max_esc_temp_ix = k + 1; + stats.max_esc_temp = dshotTelemetryState.motorState[k].maxTemp; + } + } } +#else + {} #endif #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY) @@ -806,9 +820,15 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow) #ifdef USE_ESC_SENSOR case OSD_STAT_MAX_ESC_TEMP: - tfp_sprintf(buff, "%d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit()); - osdDisplayStatisticLabel(displayRow, "MAX ESC TEMP", buff); - return true; + { + uint16_t ix = 0; + if (stats.max_esc_temp_ix > 0) { + ix = tfp_sprintf(buff, "%d ", stats.max_esc_temp_ix); + } + tfp_sprintf(buff + ix, "%d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit()); + osdDisplayStatisticLabel(displayRow, "MAX ESC TEMP", buff); + return true; + } #endif #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY) diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h index 164b293380..734db48542 100644 --- a/src/main/osd/osd.h +++ b/src/main/osd/osd.h @@ -259,9 +259,9 @@ typedef enum { // Make sure the number of warnings do not exceed the available 32bit storage STATIC_ASSERT(OSD_WARNING_COUNT <= 32, osdwarnings_overflow); -#define ESC_RPM_ALARM_OFF -1 -#define ESC_TEMP_ALARM_OFF INT8_MIN -#define ESC_CURRENT_ALARM_OFF -1 +#define ESC_RPM_ALARM_OFF -1 +#define ESC_TEMP_ALARM_OFF 0 +#define ESC_CURRENT_ALARM_OFF -1 #define OSD_GPS_RESCUE_DISABLED_WARNING_DURATION_US 3000000 // 3 seconds @@ -282,7 +282,7 @@ typedef struct osdConfig_s { uint8_t ahMaxPitch; uint8_t ahMaxRoll; uint32_t enabled_stats; - int8_t esc_temp_alarm; + uint8_t esc_temp_alarm; int16_t esc_rpm_alarm; int16_t esc_current_alarm; uint8_t core_temp_alarm; @@ -326,6 +326,7 @@ typedef struct statistic_s { int32_t max_altitude; int16_t max_distance; float max_g_force; + int16_t max_esc_temp_ix; int16_t max_esc_temp; int32_t max_esc_rpm; uint16_t min_link_quality; diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 6da7c44d91..660fd0d471 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -921,16 +921,34 @@ static void osdElementOsdProfileName(osdElementParms_t *element) } #endif -#ifdef USE_ESC_SENSOR +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY) + static void osdElementEscTemperature(osdElementParms_t *element) { +#if defined(USE_ESC_SENSOR) if (featureIsEnabled(FEATURE_ESC_SENSOR)) { tfp_sprintf(element->buff, "E%c%3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(osdEscDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit()); - } -} -#endif // USE_ESC_SENSOR + } else +#endif +#if defined(USE_DSHOT_TELEMETRY) + { + uint32_t osdEleIx = tfp_sprintf(element->buff, "E%c", SYM_TEMPERATURE); + + for (uint8_t k = 0; k < getMotorCount(); k++) { + if ((dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0) { + osdEleIx += tfp_sprintf(element->buff + osdEleIx, "%3d%c", + osdConvertTemperatureToSelectedUnit(dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]), + osdGetTemperatureSymbolForSelectedUnit()); + } else { + osdEleIx += tfp_sprintf(element->buff + osdEleIx, " 0%c", osdGetTemperatureSymbolForSelectedUnit()); + } + } + } +#else + {} +#endif +} -#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY) static void osdElementEscRpm(osdElementParms_t *element) { renderOsdEscRpmOrFreq(&getEscRpm,element); @@ -940,6 +958,7 @@ static void osdElementEscRpmFreq(osdElementParms_t *element) { renderOsdEscRpmOrFreq(&getEscRpmFreq,element); } + #endif static void osdElementFlymode(osdElementParms_t *element) @@ -1612,10 +1631,8 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = { [OSD_NUMERICAL_VARIO] = osdElementNumericalVario, #endif [OSD_COMPASS_BAR] = osdElementCompassBar, -#ifdef USE_ESC_SENSOR - [OSD_ESC_TMP] = osdElementEscTemperature, -#endif #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR) + [OSD_ESC_TMP] = osdElementEscTemperature, [OSD_ESC_RPM] = osdElementEscRpm, #endif [OSD_REMAINING_TIME_ESTIMATE] = osdElementRemainingTimeEstimate, @@ -1727,14 +1744,10 @@ void osdAddActiveElements(void) osdAddActiveElement(OSD_EFFICIENCY); } #endif // GPS -#ifdef USE_ESC_SENSOR - if (featureIsEnabled(FEATURE_ESC_SENSOR)) { - osdAddActiveElement(OSD_ESC_TMP); - } -#endif #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR) if ((featureIsEnabled(FEATURE_ESC_SENSOR)) || (motorConfig()->dev.useDshotTelemetry)) { + osdAddActiveElement(OSD_ESC_TMP); osdAddActiveElement(OSD_ESC_RPM); osdAddActiveElement(OSD_ESC_RPM_FREQ); } @@ -1955,15 +1968,34 @@ void osdUpdateAlarms(void) } #endif -#ifdef USE_ESC_SENSOR +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY) + bool blink; + +#if defined(USE_ESC_SENSOR) if (featureIsEnabled(FEATURE_ESC_SENSOR)) { // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs - if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && osdEscDataCombined->temperature >= osdConfig()->esc_temp_alarm) { - SET_BLINK(OSD_ESC_TMP); - } else { - CLR_BLINK(OSD_ESC_TMP); + blink = osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && osdEscDataCombined->temperature >= osdConfig()->esc_temp_alarm; + } else +#endif +#if defined(USE_DSHOT_TELEMETRY) + { + blink = false; + if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF) { + for (uint32_t k = 0; !blink && (k < getMotorCount()); k++) { + blink = (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0 && + dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE] >= osdConfig()->esc_temp_alarm; + } } } +#else + {} +#endif + + if (blink) { + SET_BLINK(OSD_ESC_TMP); + } else { + CLR_BLINK(OSD_ESC_TMP); + } #endif } diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c index 997049cce4..452b477bd8 100644 --- a/src/main/osd/osd_warnings.c +++ b/src/main/osd/osd_warnings.c @@ -38,6 +38,7 @@ #include "drivers/display.h" #include "drivers/osd_symbols.h" #include "drivers/time.h" +#include "drivers/dshot.h" #include "fc/core.h" #include "fc/rc.h" @@ -302,6 +303,64 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr) } #endif // USE_ESC_SENSOR +#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) + // Show esc error + if (osdWarnGetState(OSD_WARNING_ESC_FAIL)) { + uint32_t dshotEscErrorLengthMotorBegin; + uint32_t dshotEscErrorLength = 0; + + // Write 'ESC' + warningText[dshotEscErrorLength++] = 'E'; + warningText[dshotEscErrorLength++] = 'S'; + warningText[dshotEscErrorLength++] = 'C'; + + for (uint8_t k = 0; k < getMotorCount(); k++) { + // Skip if no extended telemetry at all + if ((dshotTelemetryState.motorState[k].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) == 0) { + continue; + } + + // Remember text index before writing warnings + dshotEscErrorLengthMotorBegin = dshotEscErrorLength; + + // Write ESC nr + warningText[dshotEscErrorLength++] = ' '; + warningText[dshotEscErrorLength++] = '0' + k + 1; + + // Add esc warnings + if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF + && (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_eRPM)) != 0 + && (dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100 * 2 / motorConfig()->motorPoleCount) <= osdConfig()->esc_rpm_alarm) { + warningText[dshotEscErrorLength++] = 'R'; + } + if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF + && (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0 + && dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE] >= osdConfig()->esc_temp_alarm) { + warningText[dshotEscErrorLength++] = 'T'; + } + if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF + && (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) != 0 + && dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT] >= osdConfig()->esc_current_alarm) { + warningText[dshotEscErrorLength++] = 'C'; + } + + // If no esc warning data undo esc nr (esc telemetry data types depends on the esc hw/sw) + if (dshotEscErrorLengthMotorBegin + 2 == dshotEscErrorLength) + dshotEscErrorLength = dshotEscErrorLengthMotorBegin; + } + + // If warning exists then notify, otherwise clear warning message + if (dshotEscErrorLength > 3) { + warningText[dshotEscErrorLength] = 0; // End string + *displayAttr = DISPLAYPORT_ATTR_WARNING; + *blinking = true; + return; + } else { + warningText[0] = 0; + } + } +#endif + if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) { tfp_sprintf(warningText, "LOW BATTERY"); *displayAttr = DISPLAYPORT_ATTR_WARNING; diff --git a/src/test/Makefile b/src/test/Makefile index 6497ec8a6f..992890e95a 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -182,8 +182,11 @@ maths_unittest_SRC := \ motor_output_unittest_SRC := \ + $(USER_DIR)/build/atomic.c \ $(USER_DIR)/drivers/dshot.c +motor_output_unittest_DEFINES := \ + USE_DSHOT= osd_unittest_SRC := \ $(USER_DIR)/osd/osd.c \ diff --git a/src/test/unit/motor_output_unittest.cc b/src/test/unit/motor_output_unittest.cc index 37a20bfad7..14c3245102 100644 --- a/src/test/unit/motor_output_unittest.cc +++ b/src/test/unit/motor_output_unittest.cc @@ -22,12 +22,47 @@ #include extern "C" { - #include "drivers/dshot.h" +#include "drivers/dshot.h" +#include "build/atomic.h" } #include "unittest_macros.h" #include "gtest/gtest.h" +extern "C" { + +bool featureIsEnabled(uint8_t f); +float getDigitalIdleOffset(const motorConfig_t *motorConfig); +float scaleRangef(float a, float b, float c, float d, float e); + +// Mocking functions + +bool featureIsEnabled(uint8_t f) +{ + UNUSED(f); + return true; +} + +float getDigitalIdleOffset(const motorConfig_t *motorConfig) +{ + UNUSED(motorConfig); + return 0; +} + +float scaleRangef(float a, float b, float c, float d, float e) +{ + UNUSED(a); + UNUSED(b); + UNUSED(c); + UNUSED(d); + UNUSED(e); + return 0; +} + +} + +// Tests + TEST(MotorOutputUnittest, TestFixMotorOutputReordering) { const unsigned size = 8;