mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +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
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue