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
  27 #include "platform.h"
  28
  29 #ifdef USE_DSHOT
  30
  31 #include "build/atomic.h"
  32
  33 #include "common/maths.h"
  34 #include "common/time.h"
  35
  36 #include "config/feature.h"
  37
  38 #include "drivers/motor.h"
  39 #include "drivers/timer.h"
  40
  41 #include "drivers/dshot_dpwm.h" // for motorDmaOutput_t, should be gone
  42 #include "drivers/dshot_command.h"
  43 #include "drivers/nvic.h"
  44 #include "drivers/pwm_output.h" // for PWM_TYPE_* and others
  45
  46 #include "fc/rc_controls.h" // for flight3DConfig_t
  47
  48 #include "rx/rx.h"
  49
  50 #include "dshot.h"
  51
  52 void dshotInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) {
  53     float outputLimitOffset = DSHOT_RANGE * (1 - outputLimit);
  54     *disarm = DSHOT_CMD_MOTOR_STOP;
  55     if (featureIsEnabled(FEATURE_3D)) {
  56         *outputLow = DSHOT_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * (DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE);
  57         *outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2;
  58         *deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * (DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE);
  59         *deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2;
  60     } else {
  61         *outputLow = DSHOT_MIN_THROTTLE + getDigitalIdleOffset(motorConfig) * DSHOT_RANGE;
  62         *outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset;
  63     }
  64 }
  65
  66 float dshotConvertFromExternal(uint16_t externalValue)
  67 {
  68     float motorValue;
  69
  70     externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
  71
  72     if (featureIsEnabled(FEATURE_3D)) {
  73         if (externalValue == PWM_RANGE_MIDDLE) {
  74             motorValue = DSHOT_CMD_MOTOR_STOP;
  75         } else if (externalValue < PWM_RANGE_MIDDLE) {
  76             motorValue = scaleRangef(externalValue, PWM_RANGE_MIN, PWM_RANGE_MIDDLE - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
  77         } else {
  78             motorValue = scaleRangef(externalValue, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
  79         }
  80     } else {
  81         motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_CMD_MOTOR_STOP : scaleRangef(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
  82     }
  83
  84     return motorValue;
  85 }
  86
  87 uint16_t dshotConvertToExternal(float motorValue)
  88 {
  89     float externalValue;
  90
  91     if (featureIsEnabled(FEATURE_3D)) {
  92         if (motorValue == DSHOT_CMD_MOTOR_STOP || motorValue < DSHOT_MIN_THROTTLE) {
  93             externalValue = PWM_RANGE_MIDDLE;
  94         } else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
  95             externalValue = scaleRangef(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MIDDLE - 1, PWM_RANGE_MIN);
  96         } else {
  97             externalValue = scaleRangef(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX);
  98         }
  99     } else {
 100         externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRangef(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
 101     }
 102
 103     return lrintf(externalValue);
 104 }
 105
 106 FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb)
 107 {
 108     uint16_t packet;
 109
 110     ATOMIC_BLOCK(NVIC_PRIO_DSHOT_DMA) {
 111         packet = (pcb->value << 1) | (pcb->requestTelemetry ? 1 : 0);
 112         pcb->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 113     }
 114
 115     // compute checksum
 116     unsigned csum = 0;
 117     unsigned csum_data = packet;
 118     for (int i = 0; i < 3; i++) {
 119         csum ^=  csum_data;   // xor data by nibbles
 120         csum_data >>= 4;
 121     }
 122     // append checksum
 123 #ifdef USE_DSHOT_TELEMETRY
 124     if (useDshotTelemetry) {
 125         csum = ~csum;
 126     }
 127 #endif
 128     csum &= 0xf;
 129     packet = (packet << 4) | csum;
 130
 131     return packet;
 132 }
 133
 134 #ifdef USE_DSHOT_TELEMETRY
 135 FAST_DATA_ZERO_INIT dshotTelemetryState_t dshotTelemetryState;
 136
 137 uint16_t getDshotTelemetry(uint8_t index)
 138 {
 139     return dshotTelemetryState.motorState[index].telemetryValue;
 140 }
 141
 142 #endif
 143
 144 #ifdef USE_DSHOT_TELEMETRY_STATS
 145 FAST_DATA_ZERO_INIT dshotTelemetryQuality_t dshotTelemetryQuality[MAX_SUPPORTED_MOTORS];
 146
 147 void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool packetValid, timeMs_t currentTimeMs)
 148 {
 149     uint8_t statsBucketIndex = (currentTimeMs / DSHOT_TELEMETRY_QUALITY_BUCKET_MS) % DSHOT_TELEMETRY_QUALITY_BUCKET_COUNT;
 150     if (statsBucketIndex != qualityStats->lastBucketIndex) {
 151         qualityStats->packetCountSum -= qualityStats->packetCountArray[statsBucketIndex];
 152         qualityStats->invalidCountSum -= qualityStats->invalidCountArray[statsBucketIndex];
 153         qualityStats->packetCountArray[statsBucketIndex] = 0;
 154         qualityStats->invalidCountArray[statsBucketIndex] = 0;
 155         qualityStats->lastBucketIndex = statsBucketIndex;
 156     }
 157     qualityStats->packetCountSum++;
 158     qualityStats->packetCountArray[statsBucketIndex]++;
 159     if (!packetValid) {
 160         qualityStats->invalidCountSum++;
 161         qualityStats->invalidCountArray[statsBucketIndex]++;
 162     }
 163 }
 164 #endif // USE_DSHOT_TELEMETRY_STATS
 165
 166 #endif // USE_DSHOT
 167
 168 // temporarily here, needs to be moved during refactoring
 169 void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size)
 170 {
 171     bool invalid = false;
 172
 173     for (unsigned i = 0; i < size; i++) {
 174         if (array[i] >= size) {
 175             invalid = true;
 176             break;
 177         }
 178     }
 179
 180     int valuesAsIndexes[size];
 181
 182     for (unsigned i = 0; i < size; i++) {
 183         valuesAsIndexes[i] = -1;
 184     }
 185
 186     if (!invalid) {
 187         for (unsigned i = 0; i < size; i++) {
 188             if (-1 != valuesAsIndexes[array[i]]) {
 189                 invalid = true;
 190                 break;
 191             }
 192
 193             valuesAsIndexes[array[i]] = array[i];
 194         }
 195     }
 196
 197     if (invalid) {
 198         for (unsigned i = 0; i < size; i++) {
 199             array[i] = i;
 200         }
 201     }
 202 }