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
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <math.h>
  26 #include <string.h>
  27
  28 #include "platform.h"
  29
  30 #ifdef USE_DSHOT
  31
  32 #include "build/debug.h"
  33 #include "build/atomic.h"
  34
  35 #include "common/maths.h"
  36
  37 #include "config/feature.h"
  38
  39 #include "drivers/motor.h"
  40 #include "drivers/timer.h"
  41
  42 #include "drivers/dshot_command.h"
  43 #include "drivers/nvic.h"
  44
  45 #include "flight/mixer.h"
  46
  47 #include "rx/rx.h"
  48 #include "dshot.h"
  49
  50 void dshotInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
  51 {
  52     float outputLimitOffset = DSHOT_RANGE * (1 - outputLimit);
  53     *disarm = DSHOT_CMD_MOTOR_STOP;
  54     if (featureIsEnabled(FEATURE_3D)) {
  55         *outputLow = DSHOT_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * (DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE);
  56         *outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2;
  57         *deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * (DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE);
  58         *deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2;
  59     } else {
  60         *outputLow = DSHOT_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * DSHOT_RANGE;
  61         *outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset;
  62     }
  63 }
  64
  65 float dshotConvertFromExternal(uint16_t externalValue)
  66 {
  67     float motorValue;
  68
  69     externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
  70
  71     if (featureIsEnabled(FEATURE_3D)) {
  72         if (externalValue == PWM_RANGE_MIDDLE) {
  73             motorValue = DSHOT_CMD_MOTOR_STOP;
  74         } else if (externalValue < PWM_RANGE_MIDDLE) {
  75             motorValue = scaleRangef(externalValue, PWM_RANGE_MIN, PWM_RANGE_MIDDLE - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
  76         } else {
  77             motorValue = scaleRangef(externalValue, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
  78         }
  79     } else {
  80         motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_CMD_MOTOR_STOP : scaleRangef(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
  81     }
  82
  83     return motorValue;
  84 }
  85
  86 uint16_t dshotConvertToExternal(float motorValue)
  87 {
  88     float externalValue;
  89
  90     if (featureIsEnabled(FEATURE_3D)) {
  91         if (motorValue == DSHOT_CMD_MOTOR_STOP || motorValue < DSHOT_MIN_THROTTLE) {
  92             externalValue = PWM_RANGE_MIDDLE;
  93         } else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
  94             externalValue = scaleRangef(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MIDDLE - 1, PWM_RANGE_MIN);
  95         } else {
  96             externalValue = scaleRangef(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX);
  97         }
  98     } else {
  99         externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRangef(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
 100     }
 101
 102     return lrintf(externalValue);
 103 }
 104
 105 FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb)
 106 {
 107     uint16_t packet;
 108
 109     ATOMIC_BLOCK(NVIC_PRIO_DSHOT_DMA) {
 110         packet = (pcb->value << 1) | (pcb->requestTelemetry ? 1 : 0);
 111         pcb->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 112     }
 113
 114     // compute checksum
 115     unsigned csum = 0;
 116     unsigned csum_data = packet;
 117     for (int i = 0; i < 3; i++) {
 118         csum ^=  csum_data;   // xor data by nibbles
 119         csum_data >>= 4;
 120     }
 121     // append checksum
 122 #ifdef USE_DSHOT_TELEMETRY
 123     if (useDshotTelemetry) {
 124         csum = ~csum;
 125     }
 126 #endif
 127     csum &= 0xf;
 128     packet = (packet << 4) | csum;
 129
 130     return packet;
 131 }
 132
 133 #ifdef USE_DSHOT_TELEMETRY
 134
 135
 136 FAST_DATA_ZERO_INIT dshotTelemetryState_t dshotTelemetryState;
 137
 138 static uint32_t dshot_decode_eRPM_telemetry_value(uint16_t value)
 139 {
 140     // eRPM range
 141     if (value == 0x0fff) {
 142         return 0;
 143     }
 144
 145     // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm)
 146     value = (value & 0x01ff) << ((value & 0xfe00) >> 9);
 147     if (!value) {
 148         return DSHOT_TELEMETRY_INVALID;
 149     }
 150
 151     // Convert period to erpm * 100
 152     return (1000000 * 60 / 100 + value / 2) / value;
 153 }
 154
 155 static void dshot_decode_telemetry_value(uint8_t motorIndex, uint32_t *pDecoded, dshotTelemetryType_t *pType)
 156 {
 157     uint16_t value = dshotTelemetryState.motorState[motorIndex].rawValue;
 158
 159     if (dshotTelemetryState.motorState[motorIndex].telemetryTypes == DSHOT_NORMAL_TELEMETRY_MASK) {   /* Check DSHOT_TELEMETRY_TYPE_eRPM mask */
 160         // Decode eRPM telemetry
 161         *pDecoded = dshot_decode_eRPM_telemetry_value(value);
 162
 163         // Update debug buffer
 164         if (motorIndex < 4) {
 165             DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, *pDecoded);
 166         }
 167
 168         // Set telemetry type
 169         *pType = DSHOT_TELEMETRY_TYPE_eRPM;
 170     } else {
 171         // Decode Extended DSHOT telemetry
 172         switch (value & 0x0f00) {
 173
 174         case 0x0200:
 175             // Temperature range (in degree Celsius, just like Blheli_32 and KISS)
 176             *pDecoded = value & 0x00ff;
 177
 178             // Set telemetry type
 179             *pType = DSHOT_TELEMETRY_TYPE_TEMPERATURE;
 180             break;
 181
 182         case 0x0400:
 183             // Voltage range (0-63,75V step 0,25V)
 184             *pDecoded = value & 0x00ff;
 185
 186             // Set telemetry type
 187             *pType = DSHOT_TELEMETRY_TYPE_VOLTAGE;
 188             break;
 189
 190         case 0x0600:
 191             // Current range (0-255A step 1A)
 192             *pDecoded = value & 0x00ff;
 193
 194             // Set telemetry type
 195             *pType = DSHOT_TELEMETRY_TYPE_CURRENT;
 196             break;
 197
 198         case 0x0800:
 199             // Debug 1 value
 200             *pDecoded = value & 0x00ff;
 201
 202             // Set telemetry type
 203             *pType = DSHOT_TELEMETRY_TYPE_DEBUG1;
 204             break;
 205
 206         case 0x0A00:
 207             // Debug 2 value
 208             *pDecoded = value & 0x00ff;
 209
 210             // Set telemetry type
 211             *pType = DSHOT_TELEMETRY_TYPE_DEBUG2;
 212             break;
 213
 214         case 0x0C00:
 215             // Debug 3 value
 216             *pDecoded = value & 0x00ff;
 217
 218             // Set telemetry type
 219             *pType = DSHOT_TELEMETRY_TYPE_DEBUG3;
 220             break;
 221
 222         case 0x0E00:
 223             // State / events
 224             *pDecoded = value & 0x00ff;
 225
 226             // Set telemetry type
 227             *pType = DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
 228             break;
 229
 230         default:
 231             // Decode as eRPM
 232             *pDecoded = dshot_decode_eRPM_telemetry_value(value);
 233
 234             // Update debug buffer
 235             if (motorIndex < 4) {
 236                 DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, *pDecoded);
 237             }
 238
 239             // Set telemetry type
 240             *pType = DSHOT_TELEMETRY_TYPE_eRPM;
 241             break;
 242
 243         }
 244     }
 245 }
 246
 247 static void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t type, uint32_t value)
 248 {
 249     // Update telemetry data
 250     dshotTelemetryState.motorState[motorIndex].telemetryData[type] = value;
 251     dshotTelemetryState.motorState[motorIndex].telemetryTypes |= (1 << type);
 252
 253     // Update max temp
 254     if ((type == DSHOT_TELEMETRY_TYPE_TEMPERATURE) && (value > dshotTelemetryState.motorState[motorIndex].maxTemp)) {
 255         dshotTelemetryState.motorState[motorIndex].maxTemp = value;
 256     }
 257 }
 258
 259 uint16_t getDshotTelemetry(uint8_t index)
 260 {
 261     // Process telemetry in case it haven´t been processed yet
 262     if (dshotTelemetryState.rawValueState == DSHOT_RAW_VALUE_STATE_NOT_PROCESSED) {
 263         const unsigned motorCount = motorDeviceCount();
 264         uint32_t erpmTotal = 0;
 265         uint32_t rpmSamples = 0;
 266
 267         // Decode all telemetry data now to discharge interrupt from this task
 268         for (uint8_t k = 0; k < motorCount; k++) {
 269             dshotTelemetryType_t type;
 270             uint32_t value;
 271
 272             dshot_decode_telemetry_value(k, &value, &type);
 273
 274             if (value != DSHOT_TELEMETRY_INVALID) {
 275                 dshotUpdateTelemetryData(k, type, value);
 276
 277                 if (type == DSHOT_TELEMETRY_TYPE_eRPM) {
 278                     erpmTotal += value;
 279                     rpmSamples++;
 280                 }
 281             }
 282         }
 283
 284         // Update average
 285         if (rpmSamples > 0) {
 286             dshotTelemetryState.averageErpm = (uint16_t)(erpmTotal / rpmSamples);
 287         }
 288
 289         // Set state to processed
 290         dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_PROCESSED;
 291     }
 292
 293     return dshotTelemetryState.motorState[index].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM];
 294 }
 295
 296 bool isDshotMotorTelemetryActive(uint8_t motorIndex)
 297 {
 298     return (dshotTelemetryState.motorState[motorIndex].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_eRPM)) != 0;
 299 }
 300
 301 bool isDshotTelemetryActive(void)
 302 {
 303     const unsigned motorCount = motorDeviceCount();
 304     if (motorCount) {
 305         for (unsigned i = 0; i < motorCount; i++) {
 306             if (!isDshotMotorTelemetryActive(i)) {
 307                 return false;
 308             }
 309         }
 310         return true;
 311     }
 312     return false;
 313 }
 314
 315 void dshotCleanTelemetryData(void)
 316 {
 317     memset(&dshotTelemetryState, 0, sizeof(dshotTelemetryState));
 318 }
 319
 320 uint32_t erpmToRpm(uint16_t erpm)
 321 {
 322     //  rpm = (erpm * 100) / (motorConfig()->motorPoleCount / 2)
 323     return (erpm * 200) / motorConfig()->motorPoleCount;
 324 }
 325
 326 uint32_t getDshotAverageRpm(void)
 327 {
 328     return erpmToRpm(dshotTelemetryState.averageErpm);
 329 }
 330
 331 #endif // USE_DSHOT_TELEMETRY
 332
 333 #ifdef USE_DSHOT_TELEMETRY_STATS
 334
 335 FAST_DATA_ZERO_INIT dshotTelemetryQuality_t dshotTelemetryQuality[MAX_SUPPORTED_MOTORS];
 336
 337 int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex)
 338 {
 339     int16_t invalidPercent = 0;
 340
 341     if (isDshotMotorTelemetryActive(motorIndex)) {
 342         const uint32_t totalCount = dshotTelemetryQuality[motorIndex].packetCountSum;
 343         const uint32_t invalidCount = dshotTelemetryQuality[motorIndex].invalidCountSum;
 344         if (totalCount > 0) {
 345             invalidPercent = lrintf(invalidCount * 10000.0f / totalCount);
 346         }
 347     } else {
 348         invalidPercent = 10000;  // 100.00%
 349     }
 350     return invalidPercent;
 351 }
 352
 353 void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool packetValid, timeMs_t currentTimeMs)
 354 {
 355     uint8_t statsBucketIndex = (currentTimeMs / DSHOT_TELEMETRY_QUALITY_BUCKET_MS) % DSHOT_TELEMETRY_QUALITY_BUCKET_COUNT;
 356     if (statsBucketIndex != qualityStats->lastBucketIndex) {
 357         qualityStats->packetCountSum -= qualityStats->packetCountArray[statsBucketIndex];
 358         qualityStats->invalidCountSum -= qualityStats->invalidCountArray[statsBucketIndex];
 359         qualityStats->packetCountArray[statsBucketIndex] = 0;
 360         qualityStats->invalidCountArray[statsBucketIndex] = 0;
 361         qualityStats->lastBucketIndex = statsBucketIndex;
 362     }
 363     qualityStats->packetCountSum++;
 364     qualityStats->packetCountArray[statsBucketIndex]++;
 365     if (!packetValid) {
 366         qualityStats->invalidCountSum++;
 367         qualityStats->invalidCountArray[statsBucketIndex]++;
 368     }
 369 }
 370 #endif // USE_DSHOT_TELEMETRY_STATS
 371
 372 #endif // USE_DSHOT
 373
 374 // temporarily here, needs to be moved during refactoring
 375 void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size)
 376 {
 377     bool invalid = false;
 378
 379     for (unsigned i = 0; i < size; i++) {
 380         if (array[i] >= size) {
 381             invalid = true;
 382             break;
 383         }
 384     }
 385
 386     int valuesAsIndexes[size];
 387
 388     for (unsigned i = 0; i < size; i++) {
 389         valuesAsIndexes[i] = -1;
 390     }
 391
 392     if (!invalid) {
 393         for (unsigned i = 0; i < size; i++) {
 394             if (-1 != valuesAsIndexes[array[i]]) {
 395                 invalid = true;
 396                 break;
 397             }
 398
 399             valuesAsIndexes[array[i]] = array[i];
 400         }
 401     }
 402
 403     if (invalid) {
 404         for (unsigned i = 0; i < size; i++) {
 405             array[i] = i;
 406         }
 407     }
 408 }