src/main/drivers/dshot.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  *
  20  * Author: jflyper
  21  *
  22  * Follows the extended dshot telemetry documentation found at https://github.com/bird-sanctuary/extended-dshot-telemetry
  23  */
  24
  25 #include <float.h>
  26 #include <math.h>
  27 #include <stdbool.h>
  28 #include <string.h>
  29
  30 #include "platform.h"
  31
  32 #ifdef USE_DSHOT
  33
  34 #include "build/debug.h"
  35 #include "build/atomic.h"
  36
  37 #include "common/filter.h"
  38 #include "common/maths.h"
  39
  40 #include "config/feature.h"
  41
  42 #include "drivers/motor.h"
  43 #include "drivers/timer.h"
  44
  45 #include "drivers/dshot_command.h"
  46 #include "drivers/nvic.h"
  47
  48 #include "flight/mixer.h"
  49
  50 #include "pg/rpm_filter.h"
  51
  52 #include "rx/rx.h"
  53
  54 #include "dshot.h"
  55
  56 #define ERPM_PER_LSB            100.0f
  57
  58 void dshotInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
  59 {
  60     float outputLimitOffset = DSHOT_RANGE * (1 - outputLimit);
  61     *disarm = DSHOT_CMD_MOTOR_STOP;
  62     if (featureIsEnabled(FEATURE_3D)) {
  63         *outputLow = DSHOT_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * (DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE);
  64         *outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2;
  65         *deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * (DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE);
  66         *deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2;
  67     } else {
  68         *outputLow = DSHOT_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * DSHOT_RANGE;
  69         *outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset;
  70     }
  71 }
  72
  73 float dshotConvertFromExternal(uint16_t externalValue)
  74 {
  75     float motorValue;
  76
  77     externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
  78
  79     if (featureIsEnabled(FEATURE_3D)) {
  80         if (externalValue == PWM_RANGE_MIDDLE) {
  81             motorValue = DSHOT_CMD_MOTOR_STOP;
  82         } else if (externalValue < PWM_RANGE_MIDDLE) {
  83             motorValue = scaleRangef(externalValue, PWM_RANGE_MIN, PWM_RANGE_MIDDLE - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
  84         } else {
  85             motorValue = scaleRangef(externalValue, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
  86         }
  87     } else {
  88         motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_CMD_MOTOR_STOP : scaleRangef(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
  89     }
  90
  91     return motorValue;
  92 }
  93
  94 uint16_t dshotConvertToExternal(float motorValue)
  95 {
  96     float externalValue;
  97
  98     if (featureIsEnabled(FEATURE_3D)) {
  99         if (motorValue == DSHOT_CMD_MOTOR_STOP || motorValue < DSHOT_MIN_THROTTLE) {
 100             externalValue = PWM_RANGE_MIDDLE;
 101         } else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
 102             externalValue = scaleRangef(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MIDDLE - 1, PWM_RANGE_MIN);
 103         } else {
 104             externalValue = scaleRangef(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX);
 105         }
 106     } else {
 107         externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRangef(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
 108     }
 109
 110     return lrintf(externalValue);
 111 }
 112
 113 FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb)
 114 {
 115     uint16_t packet;
 116
 117     ATOMIC_BLOCK(NVIC_PRIO_DSHOT_DMA) {
 118         packet = (pcb->value << 1) | (pcb->requestTelemetry ? 1 : 0);
 119         pcb->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 120     }
 121
 122     // compute checksum
 123     unsigned csum = 0;
 124     unsigned csum_data = packet;
 125     for (int i = 0; i < 3; i++) {
 126         csum ^=  csum_data;   // xor data by nibbles
 127         csum_data >>= 4;
 128     }
 129     // append checksum
 130 #ifdef USE_DSHOT_TELEMETRY
 131     if (useDshotTelemetry) {
 132         csum = ~csum;
 133     }
 134 #endif
 135     csum &= 0xf;
 136     packet = (packet << 4) | csum;
 137
 138     return packet;
 139 }
 140
 141 #ifdef USE_DSHOT_TELEMETRY
 142
 143 FAST_DATA_ZERO_INIT dshotTelemetryState_t dshotTelemetryState;
 144
 145 FAST_DATA_ZERO_INIT static pt1Filter_t motorFreqLpf[MAX_SUPPORTED_MOTORS];
 146 FAST_DATA_ZERO_INIT static float motorFrequencyHz[MAX_SUPPORTED_MOTORS];
 147 FAST_DATA_ZERO_INIT static float minMotorFrequencyHz;
 148 FAST_DATA_ZERO_INIT static float erpmToHz;
 149 FAST_DATA_ZERO_INIT static float dshotRpmAverage;
 150 FAST_DATA_ZERO_INIT static float dshotRpm[MAX_SUPPORTED_MOTORS];
 151
 152 void initDshotTelemetry(const timeUs_t looptimeUs)
 153 {
 154     // if bidirectional DShot is not available
 155     if (!motorConfig()->dev.useDshotTelemetry && !featureIsEnabled(FEATURE_ESC_SENSOR)) {
 156         return;
 157     }
 158
 159     // erpmToHz is used by bidir dshot and ESC telemetry
 160     erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f);
 161
 162     if (motorConfig()->dev.useDshotTelemetry) {
 163         // init LPFs for RPM data
 164         for (int i = 0; i < getMotorCount(); i++) {
 165             pt1FilterInit(&motorFreqLpf[i], pt1FilterGain(rpmFilterConfig()->rpm_filter_lpf_hz, looptimeUs * 1e-6f));
 166         }
 167     }
 168 }
 169
 170 static uint32_t dshot_decode_eRPM_telemetry_value(uint16_t value)
 171 {
 172     // eRPM range
 173     if (value == 0x0fff) {
 174         return 0;
 175     }
 176
 177     // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm)
 178     value = (value & 0x01ff) << ((value & 0xfe00) >> 9);
 179     if (!value) {
 180         return DSHOT_TELEMETRY_INVALID;
 181     }
 182
 183     // Convert period to erpm * 100
 184     return (1000000 * 60 / 100 + value / 2) / value;
 185 }
 186
 187 static void dshot_decode_telemetry_value(uint8_t motorIndex, uint32_t *pDecoded, dshotTelemetryType_t *pType)
 188 {
 189     uint16_t value = dshotTelemetryState.motorState[motorIndex].rawValue;
 190     const unsigned motorCount = motorDeviceCount();
 191
 192     if (dshotTelemetryState.motorState[motorIndex].telemetryTypes == DSHOT_NORMAL_TELEMETRY_MASK) {   /* Check DSHOT_TELEMETRY_TYPE_eRPM mask */
 193         // Decode eRPM telemetry
 194         *pDecoded = dshot_decode_eRPM_telemetry_value(value);
 195
 196         // Update debug buffer
 197         if (motorIndex < motorCount && motorIndex < DEBUG16_VALUE_COUNT) {
 198             DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, *pDecoded);
 199         }
 200
 201         // Set telemetry type
 202         *pType = DSHOT_TELEMETRY_TYPE_eRPM;
 203     } else {
 204         // Decode Extended DSHOT telemetry
 205         switch (value & 0x0f00) {
 206
 207         case 0x0200:
 208             // Temperature range (in degree Celsius, just like Blheli_32 and KISS)
 209             *pDecoded = value & 0x00ff;
 210
 211             // Set telemetry type
 212             *pType = DSHOT_TELEMETRY_TYPE_TEMPERATURE;
 213             break;
 214
 215         case 0x0400:
 216             // Voltage range (0-63,75V step 0,25V)
 217             *pDecoded = value & 0x00ff;
 218
 219             // Set telemetry type
 220             *pType = DSHOT_TELEMETRY_TYPE_VOLTAGE;
 221             break;
 222
 223         case 0x0600:
 224             // Current range (0-255A step 1A)
 225             *pDecoded = value & 0x00ff;
 226
 227             // Set telemetry type
 228             *pType = DSHOT_TELEMETRY_TYPE_CURRENT;
 229             break;
 230
 231         case 0x0800:
 232             // Debug 1 value
 233             *pDecoded = value & 0x00ff;
 234
 235             // Set telemetry type
 236             *pType = DSHOT_TELEMETRY_TYPE_DEBUG1;
 237             break;
 238
 239         case 0x0A00:
 240             // Debug 2 value
 241             *pDecoded = value & 0x00ff;
 242
 243             // Set telemetry type
 244             *pType = DSHOT_TELEMETRY_TYPE_DEBUG2;
 245             break;
 246
 247         case 0x0C00:
 248             // Debug 3 value
 249             *pDecoded = value & 0x00ff;
 250
 251             // Set telemetry type
 252             *pType = DSHOT_TELEMETRY_TYPE_DEBUG3;
 253             break;
 254
 255         case 0x0E00:
 256             // State / events
 257             *pDecoded = value & 0x00ff;
 258
 259             // Set telemetry type
 260             *pType = DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
 261             break;
 262
 263         default:
 264             // Decode as eRPM
 265             *pDecoded = dshot_decode_eRPM_telemetry_value(value);
 266
 267             // Update debug buffer
 268             if (motorIndex < motorCount && motorIndex < DEBUG16_VALUE_COUNT) {
 269                 DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, *pDecoded);
 270             }
 271
 272             // Set telemetry type
 273             *pType = DSHOT_TELEMETRY_TYPE_eRPM;
 274             break;
 275
 276         }
 277     }
 278 }
 279
 280 static void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t type, uint32_t value)
 281 {
 282     // Update telemetry data
 283     dshotTelemetryState.motorState[motorIndex].telemetryData[type] = value;
 284     dshotTelemetryState.motorState[motorIndex].telemetryTypes |= (1 << type);
 285
 286     // Update max temp
 287     if ((type == DSHOT_TELEMETRY_TYPE_TEMPERATURE) && (value > dshotTelemetryState.motorState[motorIndex].maxTemp)) {
 288         dshotTelemetryState.motorState[motorIndex].maxTemp = value;
 289     }
 290 }
 291
 292 FAST_CODE_NOINLINE void updateDshotTelemetry(void)
 293 {
 294     if (!useDshotTelemetry) {
 295         return;
 296     }
 297
 298     // Only process telemetry in case it hasn´t been processed yet
 299     if (dshotTelemetryState.rawValueState != DSHOT_RAW_VALUE_STATE_NOT_PROCESSED) {
 300         return;
 301     }
 302
 303     const unsigned motorCount = motorDeviceCount();
 304     uint32_t erpmTotal = 0;
 305     uint32_t rpmSamples = 0;
 306
 307     // Decode all telemetry data now to discharge interrupt from this task
 308     for (uint8_t k = 0; k < motorCount; k++) {
 309         dshotTelemetryType_t type;
 310         uint32_t value;
 311
 312         dshot_decode_telemetry_value(k, &value, &type);
 313
 314         if (value != DSHOT_TELEMETRY_INVALID) {
 315             dshotUpdateTelemetryData(k, type, value);
 316
 317             if (type == DSHOT_TELEMETRY_TYPE_eRPM) {
 318                 dshotRpm[k] = erpmToRpm(value);
 319                 erpmTotal += value;
 320                 rpmSamples++;
 321             }
 322         }
 323     }
 324
 325     // Update average
 326     if (rpmSamples > 0) {
 327         dshotRpmAverage = erpmToRpm(erpmTotal) / (float)rpmSamples;
 328     }
 329
 330     // update filtered rotation speed of motors for features (e.g. "RPM filter")
 331     minMotorFrequencyHz = FLT_MAX;
 332     for (int motor = 0; motor < getMotorCount(); motor++) {
 333         motorFrequencyHz[motor] = pt1FilterApply(&motorFreqLpf[motor], erpmToHz * getDshotErpm(motor));
 334         minMotorFrequencyHz = MIN(minMotorFrequencyHz, motorFrequencyHz[motor]);
 335     }
 336
 337     // Set state to processed
 338     dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_PROCESSED;
 339 }
 340
 341 uint16_t getDshotErpm(uint8_t motorIndex)
 342 {
 343     return dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM];
 344 }
 345
 346 float getDshotRpm(uint8_t motorIndex)
 347 {
 348     return dshotRpm[motorIndex];
 349 }
 350
 351 float getDshotRpmAverage(void)
 352 {
 353     return dshotRpmAverage;
 354 }
 355
 356 float getMotorFrequencyHz(uint8_t motorIndex)
 357 {
 358     return motorFrequencyHz[motorIndex];
 359 }
 360
 361 float getMinMotorFrequencyHz(void)
 362 {
 363     return minMotorFrequencyHz;
 364 }
 365
 366 bool isDshotMotorTelemetryActive(uint8_t motorIndex)
 367 {
 368     return (dshotTelemetryState.motorState[motorIndex].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_eRPM)) != 0;
 369 }
 370
 371 bool isDshotTelemetryActive(void)
 372 {
 373     const unsigned motorCount = motorDeviceCount();
 374     if (motorCount) {
 375         for (unsigned i = 0; i < motorCount; i++) {
 376             if (!isDshotMotorTelemetryActive(i)) {
 377                 return false;
 378             }
 379         }
 380         return true;
 381     }
 382     return false;
 383 }
 384
 385 void dshotCleanTelemetryData(void)
 386 {
 387     memset(&dshotTelemetryState, 0, sizeof(dshotTelemetryState));
 388 }
 389
 390 #endif // USE_DSHOT_TELEMETRY
 391
 392 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 393
 394 // Used with serial esc telem as well as dshot telem
 395 float erpmToRpm(uint32_t erpm)
 396 {
 397     // rpm = (erpm * ERPM_PER_LSB) / (motorConfig()->motorPoleCount / 2)
 398     return erpm * erpmToHz * SECONDS_PER_MINUTE;
 399 }
 400
 401 #endif // USE_ESC_SENSOR || USE_DSHOT_TELEMETRY
 402
 403 #ifdef USE_DSHOT_TELEMETRY_STATS
 404
 405 FAST_DATA_ZERO_INIT dshotTelemetryQuality_t dshotTelemetryQuality[MAX_SUPPORTED_MOTORS];
 406
 407 int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex)
 408 {
 409     int16_t invalidPercent = 0;
 410
 411     if (isDshotMotorTelemetryActive(motorIndex)) {
 412         const uint32_t totalCount = dshotTelemetryQuality[motorIndex].packetCountSum;
 413         const uint32_t invalidCount = dshotTelemetryQuality[motorIndex].invalidCountSum;
 414         if (totalCount > 0) {
 415             invalidPercent = lrintf(invalidCount * 10000.0f / totalCount);
 416         }
 417     } else {
 418         invalidPercent = 10000;  // 100.00%
 419     }
 420     return invalidPercent;
 421 }
 422
 423 void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool packetValid, timeMs_t currentTimeMs)
 424 {
 425     uint8_t statsBucketIndex = (currentTimeMs / DSHOT_TELEMETRY_QUALITY_BUCKET_MS) % DSHOT_TELEMETRY_QUALITY_BUCKET_COUNT;
 426     if (statsBucketIndex != qualityStats->lastBucketIndex) {
 427         qualityStats->packetCountSum -= qualityStats->packetCountArray[statsBucketIndex];
 428         qualityStats->invalidCountSum -= qualityStats->invalidCountArray[statsBucketIndex];
 429         qualityStats->packetCountArray[statsBucketIndex] = 0;
 430         qualityStats->invalidCountArray[statsBucketIndex] = 0;
 431         qualityStats->lastBucketIndex = statsBucketIndex;
 432     }
 433     qualityStats->packetCountSum++;
 434     qualityStats->packetCountArray[statsBucketIndex]++;
 435     if (!packetValid) {
 436         qualityStats->invalidCountSum++;
 437         qualityStats->invalidCountArray[statsBucketIndex]++;
 438     }
 439 }
 440 #endif // USE_DSHOT_TELEMETRY_STATS
 441
 442 #endif // USE_DSHOT
 443
 444 // temporarily here, needs to be moved during refactoring
 445 void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size)
 446 {
 447     bool invalid = false;
 448
 449     for (unsigned i = 0; i < size; i++) {
 450         if (array[i] >= size) {
 451             invalid = true;
 452             break;
 453         }
 454     }
 455
 456     int valuesAsIndexes[size];
 457
 458     for (unsigned i = 0; i < size; i++) {
 459         valuesAsIndexes[i] = -1;
 460     }
 461
 462     if (!invalid) {
 463         for (unsigned i = 0; i < size; i++) {
 464             if (-1 != valuesAsIndexes[array[i]]) {
 465                 invalid = true;
 466                 break;
 467             }
 468
 469             valuesAsIndexes[array[i]] = array[i];
 470         }
 471     }
 472
 473     if (invalid) {
 474         for (unsigned i = 0; i < size; i++) {
 475             array[i] = i;
 476         }
 477     }
 478 }