mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 11:59:58 +03:00
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
This commit is contained in:
danmos
iso9660
parent
06c6189ca8
commit
21b3fc15b7
18 changed files with 562 additions and 167 deletions
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<<DSHOT_TELEMETRY_TYPE_eRPM))
|
||||
|
||||
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_COUNT = 8
|
||||
} dshotTelemetryType_t;
|
||||
|
||||
typedef struct dshotProtocolControl_s {
|
||||
uint16_t value;
|
||||
bool requestTelemetry;
|
||||
|
|
@ -66,8 +83,9 @@ uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb);
|
|||
extern bool useDshotTelemetry;
|
||||
|
||||
typedef struct dshotTelemetryMotorState_s {
|
||||
uint16_t telemetryValue;
|
||||
bool telemetryActive;
|
||||
uint8_t telemetryTypes;
|
||||
uint16_t telemetryData[DSHOT_TELEMETRY_TYPE_COUNT];
|
||||
uint8_t maxTemp;
|
||||
} dshotTelemetryMotorState_t;
|
||||
|
||||
|
||||
|
|
@ -93,3 +111,8 @@ bool isDshotTelemetryActive(void);
|
|||
int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);
|
||||
|
||||
void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size);
|
||||
|
||||
dshotTelemetryType_t dshot_get_telemetry_type_to_decode(uint8_t motorIndex);
|
||||
uint32_t dshot_decode_telemetry_value(uint32_t value, dshotTelemetryType_t *type);
|
||||
void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t type, uint16_t value);
|
||||
|
||||
|
|
|
|||
|
|
@ -501,12 +501,14 @@ static bool bbUpdateStart(void)
|
|||
const timeMs_t currentTimeMs = millis();
|
||||
#endif
|
||||
timeUs_t currentUs = micros();
|
||||
|
||||
// don't send while telemetry frames might still be incoming
|
||||
if (cmpTimeUs(currentUs, lastSendUs) < (timeDelta_t)(40 + 2 * dshotFrameUs)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
dshotTelemetryType_t type;
|
||||
#ifdef USE_DSHOT_CACHE_MGMT
|
||||
// Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
|
||||
bool invalidated = false;
|
||||
|
|
@ -521,25 +523,27 @@ static bool bbUpdateStart(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
// Get dshot telemetry type to decode
|
||||
type = dshot_get_telemetry_type_to_decode(motorIndex);
|
||||
|
||||
#ifdef STM32F4
|
||||
uint32_t value = decode_bb_bitband(
|
||||
bbMotors[motorIndex].bbPort->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
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -18,12 +18,14 @@
|
|||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)) {
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -22,12 +22,47 @@
|
|||
#include <iostream>
|
||||
|
||||
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;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue