diff --git a/Src/dshot.c b/Src/dshot.c index a55ffefe..e3e9dee9 100644 --- a/Src/dshot.c +++ b/Src/dshot.c @@ -12,43 +12,47 @@ #include "sounds.h" #include "targets.h" -int dpulse[16] = { 0 }; +static uint8_t dpulse[16]; -const char gcr_encode_table[16] = { +static const uint8_t gcr_encode_table[16] = { 0b11001, 0b11011, 0b10010, 0b10011, 0b11101, 0b10101, 0b10110, 0b10111, 0b11010, 0b01001, 0b01010, 0b01011, 0b11110, 0b01101, 0b01110, 0b01111 }; -char EDT_ARM_ENABLE = 0; -char EDT_ARMED = 0; -int shift_amount = 0; -uint32_t gcrnumber; +char EDT_ARM_ENABLE; +char EDT_ARMED; +static int shift_amount; +static uint32_t gcrnumber; extern int zero_crosses; extern char send_telemetry; extern uint8_t max_duty_cycle_change; -int dshot_full_number; +static uint32_t dshot_full_number; extern char play_tone_flag; -uint8_t command_count = 0; -uint8_t last_command = 0; -uint8_t high_pin_count = 0; -uint32_t gcr[37] = { 0 }; -uint16_t dshot_frametime; -uint16_t dshot_goodcounts; -uint16_t dshot_badcounts; -char dshot_extended_telemetry = 0; -uint16_t send_extended_dshot = 0; -uint16_t processtime = 0; -uint16_t halfpulsetime = 0; +static uint8_t command_count; +static uint8_t last_command; +static uint8_t high_pin_count; +uint32_t gcr[37]; +static uint16_t dshot_frametime; +static uint16_t dshot_goodcounts; +static uint16_t dshot_badcounts; +char dshot_extended_telemetry; +uint16_t send_extended_dshot; +static uint16_t halfpulsetime; void computeDshotDMA() { - int j = 0; dshot_frametime = dma_buffer[31] - dma_buffer[0]; halfpulsetime = dshot_frametime >> 5; if ((dshot_frametime > dshot_frametime_low) && (dshot_frametime < dshot_frametime_high)) { - signaltimeout = 0; + signaltimeout = 0; for (int i = 0; i < 16; i++) { - dpulse[i] = ((dma_buffer[j + (i << 1) + 1] - dma_buffer[j + (i << 1)]) > (halfpulsetime)); + const uint16_t pwidth = dma_buffer[(i << 1) + 1] - dma_buffer[(i << 1)]; + if (pwidth < (halfpulsetime>>1) || pwidth > (halfpulsetime<<1)) { + // pulse width way off, bad frame + dshot_badcounts++; + return; + } + dpulse[i] = (pwidth > halfpulsetime); } uint8_t calcCRC = ((dpulse[0] ^ dpulse[4] ^ dpulse[8]) << 3 | (dpulse[1] ^ dpulse[5] ^ dpulse[9]) << 2 | (dpulse[2] ^ dpulse[6] ^ dpulse[10]) << 1 | (dpulse[3] ^ dpulse[7] ^ dpulse[11])); uint8_t checkCRC = (dpulse[12] << 3 | dpulse[13] << 2 | dpulse[14] << 1 | dpulse[15]);