mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
Merge pull request #11694 from damosvil/feature/dshot_extended_telemetry_on_unused_ranges
Feature/dshot extended telemetry on unused ranges
This commit is contained in:
haslinghuis
GitHub
commit
c78e14cc27
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