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